Using relational operators on state vectors

Question

I was writing DoCalcTimeDerivatives method for a plant where derivatives are defined with lots of conditional statements. However, while building a diagram, I encountered following error:

RuntimeError: You should not call `__bool__` / `__nonzero__` on `Formula`. If you are trying to make a map with `Variable`, `Expression`, or `Polynomial` as keys (and then access the map in Python), please use pydrake.common.containers.EqualToDict\`.

Could anyone advise on how I might use relational operators without triggering this error? Here is a simplified version of the plant that reproduces the error:

from pydrake.all import (
    DiagramBuilder,
    LeafSystem_,
    SceneGraph,
    namedview,
    lt,
)

TestState = namedview(
    "TestState", ["x", "xdot"]
)

@TemplateSystem.define("TestPlant_")
def TestPlant_(T):
    class Impl(LeafSystem_[T]):
        def _construct(self, converter=None):
            LeafSystem_[T].__init__(self, converter)
            self.DeclareVectorInputPort("force", 1)
            self.DeclareContinuousState(2)
            self.DeclareVectorOutputPort("state", 2, self.CopyStateOut)

        def _construct_copy(self, other, converter=None):
            Impl._construct(self, converter=converter)

        def DoCalcTimeDerivatives(self, context, derivatives):
            s = TestState(
                context.get_mutable_continuous_state_vector().CopyToVector()
            )
            force = self.EvalVectorInput(context, 0)[0]

            sdot = TestState(s[:])
            sdot[0] = s[1]
            if (s[0] < 0):
                sdot[1] = force
            else:
                sdot[1] = 2*force
            derivatives.get_mutable_vector().SetFromVector(sdot[:])

        def CopyStateOut(self, context, output):
            x = context.get_continuous_state_vector().CopyToVector()
            output.SetFromVector(x)
    return Impl


TestPlant = TestPlant_[None]
builder = DiagramBuilder()
glider = builder.AddSystem(TestPlant())
scene_graph = builder.AddSystem(SceneGraph())
diagram = builder.Build()

Output:

---------------------------------------------------------------------------
RuntimeError                              Traceback (most recent call last)
Cell In[18], line 49
     47 glider = builder.AddSystem(TestPlant())
     48 scene_graph = builder.AddSystem(SceneGraph())
---> 49 diagram = builder.Build()

Cell In[18], line 33, in TestPlant_..Impl.DoCalcTimeDerivatives(self, context, derivatives)
     31 sdot = TestState(s[:])
     32 sdot[0] = s[1]
---> 33 if (s[0] < 0):
     34     sdot[1] = force
     35 else:

RuntimeError: You should not call `__bool__` / `__nonzero__` on `Formula`. If you are trying to make a map with `Variable`, `Expression`, or `Polynomial` as keys (and then access the map in Python), please use pydrake.common.containers.EqualToDict`.

----- What I have tried so far -------

From the website(https://drake.mit.edu/pydrake/pydrake.math.html), I found this:

As a workaround, this module provides the following vectorized operators, following suit with the operator builtin module: lt, le, eq, ne, ge, and gt.

Following this, I tried:

if (lt(s[0],0)):         # instead of if (s[0] < 0):

But this gives the same error:

---------------------------------------------------------------------------
RuntimeError                              Traceback (most recent call last)
Cell In[19], line 49
     47 glider = builder.AddSystem(TestPlant())
     48 scene_graph = builder.AddSystem(SceneGraph())
---> 49 diagram = builder.Build()

Cell In[19], line 33, in TestPlant_..Impl.DoCalcTimeDerivatives(self, context, derivatives)
     31 sdot = TestState(s[:])
     32 sdot[0] = s[1]
---> 33 if (lt(s[0],0)):
     34     sdot[1] = force
     35 else:

RuntimeError: You should not call `__bool__` / `__nonzero__` on `Formula`. If you are trying to make a map with `Variable`, `Expression`, or `Polynomial` as keys (and then access the map in Python), please use pydrake.common.containers.EqualToDict`.

Answer 1

The TemplateSystem decorator, as you've defined it (based on all of our examples), is adding support for float, AutoDiffXd, and symbolic::Expression. Do you need support for symbolic? If all you really need is autodiff, then I believe you can reduce the list of supported types by passing in the additional arguments to the TemplateSystem decorator. https://drake.mit.edu/pydrake/pydrake.systems.scalar_conversion.html#pydrake.systems.scalar_conversion.TemplateSystem

If you do want to support Expression, then the fix is to write the branching logic using if_then_else, which is slightly more cumbersome, but will work correctly for all three scalar types.