RobotLocomotion / drake

Model-based design and verification for robotics.
https://drake.mit.edu
Other
3.25k stars 1.25k forks source link

DiagramContext Clone() fails to preserve DiagramDiscreteValues type #11457

Closed RussTedrake closed 4 years ago

RussTedrake commented 5 years ago

The following code

from pydrake.all import (
    DiagramBuilder, 
    FindResourceOrThrow,
    Parser,
    AddMultibodyPlantSceneGraph,
)

builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0005)
# Build the plant
Parser(parser).AddModelFromFile(FindResourceOrThrow("drake/examples/acrobot/Acrobot.urdf"))
plant.Finalize()

# The diagram
builder.ExportInput(plant.get_actuation_input_port())
diagram = builder.Build()
context = diagram.CreateDefaultContext()
context.FixInputPort(0, [0])
new_context = context.Clone()
discrete_values = new_context.get_mutable_discrete_state()
# discrete_values = diagram.AllocateDiscreteVariables()
diagram.CalcDiscreteVariableUpdates(
        context = context,
        discrete_state = discrete_values)

fails with

In [2]: %tb
---------------------------------------------------------------------------
SystemExit                                Traceback (most recent call last)
<ipython-input-6-f802d3c9d4c2> in <module>()
     24 diagram.CalcDiscreteVariableUpdates(
     25         context = context,
---> 26         discrete_state = discrete_values)

SystemExit: Failure at systems/framework/diagram.cc:1009 in DispatchDiscreteVariableUpdateHandler(): condition 'diagram_discrete' failed.

but if I pass in the result of AllocateDiscreteVariables instead (by uncommenting that line), then everything works as expected. I don't understand why. I dug into the call stack a bit, and it appears that the dynamic_cast to DiagramDiscreteValues<T> is failing on the first case.

EDIT(eric): Updated repro.

EricCousineau-TRI commented 4 years ago

@sherm1 FYI I am still able to reproduce this.

Easy repro steps:

Version info (from notebook):

In [3]: !cat /opt/drake/share/doc/drake/VERSION.TXT
Out [3]: 20200420074541 b02ff6e7b75d1a9a3ae7d4cf21fe76fa558cff74

(I will not apply component: pydrake, as it's unclear if this is Python-specific.)

sherm1 commented 4 years ago

Investigated: the problem is that diagram_context.Clone() incorrectly copies the DiagramDiscreteValues object into a DiscreteValues object. There was no unit test checking the object types, just the values. Fixing now.

sherm1 commented 4 years ago

I see now that neither the original diagram.AllocateContext() (CreateDefaultContext())nor diagram_context.Clone() made any use of DiagramDiscreteValues -- both go through the same DiagramState code. Only diagram.AllocateDiscreteVariables() is doing this correctly.