Is RegionOfAttraction() available for Multibody plants from pydrake?

user_10417678 0
Viewed 105

To get familiar with the Region of Attraction Codes i tried to estimate the RoA of an Acrobot for which i have an urdf description. Running these lines:

import numpy as np

from pydrake.all import DiagramBuilder, AddMultibodyPlantSceneGraph, Parser, MultibodyPlant, Variable, Simulator, ConstantVectorSource, RegionOfAttraction, LinearSystem,LinearQuadraticRegulator,Saturation

import pydot
#%% prepare and load urdf
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)

parser = Parser(plant, scene_graph)
parser.AddModelFromFile("acrobot.urdf")

plant.Finalize()

#%% create down context
q=np.array([0.0,0.0])
qd=np.array([0.0,0.0])

stable_ctxt = plant.CreateDefaultContext() 
plant.SetPositions(stable_ctxt, q)
plant.SetVelocities(stable_ctxt, qd)

actuation_port = plant.get_actuation_input_port()
actuation_port.FixValue(stable_ctxt, 0)

#%% calc RoA
V = RegionOfAttraction(system=plant, context=stable_ctxt)

gave me this error:

SystemExit: Failure at systems/analysis/region_of_attraction.cc:126 in RegionOfAttraction(): condition 'xdot0.template lpNorm<Eigen::Infinity>() <= 1e-14' failed.

Which when looking at the comment in region_of_attraction.cc:123 indicates that x0 is not a stable fixed point. However it should be stable for the stable_ctxt provided to RegionOfAttraction().

Can the RegionOfAttraction() function be used to estimate the RoA of Multibody plants (and do they support conversion to Symbolic) or is there something else i have to consider/some obvious error in my thinking?

Thanks in advance!

drake python
1 Answers

Thanks for asking and providing a minimal reproduction. There were actually a lot of things going on in this example!

First, by calling AddMultibodyPlantSceneGraph you've created a MultibodyPlant that is connected to a SceneGraph, but then passed only the plant into the RegionOfAttraction method without its SceneGraph. It then complains because its geometry port is not connected. One fix would be to do only:

plant = MultibodyPlant(time_step=0.0)
parser = Parser(plant)
parser.AddModelFromFile(
    FindResourceOrThrow("drake/examples/acrobot/Acrobot.urdf"))
plant.Finalize()

Second, the RegionOfAttraction code as written was assuming that you'd only pass in a closed-loop system (no input ports), like what you'd get from using LQR. So the input port "FixValue" assignment wasn't being copied to the symbolic plant. It's reasonable to support the case of fixed input ports, too, so I've just PR'd the fix. Alternatively, you could get around this immediately by adding a ConstantVectorSystem for the input port and then passing the diagram (not the plant) into the RegionOfAttraction method.

Third, one must be mindful of what is reasonable to do in the symbolic MultibodyPlant. We don't support symbolic inversion of the mass matrix (beyond the trivial cases); I don't think of this as a missing feature but rather a case we should avoid. It's much much better to think about the dynamics in implicit form (see http://underactuated.csail.mit.edu/lyapunov.html#ex:implicit). Having said that, we could actually support 2x2 symbolic matrix inverses easily enough through the ABA ldlt call (I actually thought we had done that already).

Finally, the sums-of-squares tools used in RegionOfAttraction only support polynomial systems, and the Acrobot dynamics are not polynomial. You either want to substitute out the sin/cos terms, as in this example, or take a Taylor expansion of the dynamics around the fixed-point. The symbolic tools support Taylor expansion, but I'm afraid I haven't added the tools that make the workflow very nice for this just yet by adding a method TaylorExpand(System, Context) that wraps the symbolic calls. That also would be relatively easy and useful to do!

If there are particular missing pieces that would be of high value, then we could open an issue or even better I could try to help you contribute.

user_95100200
Related