Can I use an SDF as a plant for DirectCollocation? - drake

My goal is to use an SDF I made that models a cart-pole system and use DirectCollocation to find a swing-up trajectory.
However, when I try to use the plant created by the SDF and use DirectCollocation to construct the mathematical program I get the following error:
RuntimeError: This system doesn't have any continuous states. DirectCollocation only makes sense for systems with continuous-time dynamics.
Can an SDF create a continuous system or am I required to create my own LeafSystem?
I've been using Example: 10.2 Direct Collocation for the Pendulum, Acrobot, and Cart-Pole as a reference from the Underactuated Book as well as digging through drake/examples/acrobot. Any additional reference suggestions would be appreciated!

Most likely, the problem is that you have explicitly constructed a discrete MultibodyPlant. Take a look at the constructor documentation. If you construct it as continuous (e.g., time_step = 0), then you can parse your SDF into it.
You can confirm this by looking at your construction of the plant and confirm that you're passing a non-zero time step.

Related

Can you generate a scene graph after a plant has been finalized?

I'm working on a project that requires me to add a model through Parser (which requires the plant to be of the same type as the array used) before Setting the position of the model in said plant and taking distance queries. These queries only work when the query object generated from the scene graph is of type float.
I've run into a problem where setting the position doesn't work due to the array being used being of type AutoDiff. A possible solution would then be converting the plant of type float to Autodiff with plant.ToAutoDiff(), but this only creates a copy of the plant without coupling it to the scene graph (and in turn the query object) from which the queries are derived. Taking queries with a query object generated from the original plant would then fail to reflect the new position passed to the AutoDiff copy.
Is there a way to create a new scene graph from the already finalized symbolic copy of the original plant, so that I can perform the queries with it?
A couple of thoughts:
Don't just convert the plant to autodiff. Convert the whole diagram. That will give you a converted, connected network.
You're stuck with the current workflow. Presumably, your proximity geometries are specified in your parsed file (as <collision> tags). The parsing process is ephemeral. The declaration is consumed, passed through MultibodyPlant into SceneGraph. If there is no SceneGraph at parse time, all knowledge of the declared collision geometry is forgotten.
So, the typical workflow is:
Create a float-valued diagram.
Scalar convert it to an AutoDiff-valued diagram.
Keep both around to serve the different roles.
We don't have a tutorial that directly shows scalar converting an entire diagram, but it's akin to what is shown in this MultibodyPlant-specific tutorial. Just call ToScalarType() on the Diagram root.

How to use inverse dynamics controller on under-actuated systems

I'm trying to use Drake's inverse dyanmics controller on an arm with a floating base, and based on this discussion it seems like the most straightforward way to go about this is to use two separate plants since the controller only supports fully actuated systems.
Following Python bindings error when adding two plants to a scene graph in pyDrake, I attempted to create two plants using the following code:
def register_plant_with_scene_graph(scene_graph, plant):
plant.RegsterAsSourceForSceneGraph(scene_graph)
builder.Connect(
plant.get_geometry_poses_output_port(),
scene_graph.get_source_pose_port(plant.get_source_id()),
)
builder.Connect(
scene_graph.get_query_output_port(),
plant.get_geometry_query_input_port(),
)
builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
plant_1 = builder.AddSystem(MultibodyPlant(time_step=0.0))
register_plant_with_scene_graph(scene_graph, plant_1)
plant_2 = builder.AddSystem(MultibodyPlant(time_step=0.0))
register_plant_with_scene_graph(scene_graph, plant_2)
which produced the error
AttributeError: 'MultibodyPlant_[float]' object has no attribute 'RegsterAsSourceForSceneGraph'
Which seems odd because according to the documentation, the function should exist.
Is this function available in the python bindings for drake? Also, more broadly, is this the correct way to approach using the inverse dynamics controller on a free-floating manipulator?
Inverse dynamics takes desired positions, velocities, and accelerations and computes the required torques. If your robot has a floating base, then you cannot accept arbitrary acceleration commands. For instance the total center of mass of your robot will be falling according to gravity; any acceleration that does not satisfy this requirement will not have a feasible solution to the inverse dynamics. I think there must be something more that we need to understand about your problem formulation.
Often when people ask this question, they are thinking of a robot that is relying on contact forces in addition to generalized force/torques in order to achieve the requested accelerations. In that case, the problem needs to include those contact forces as decision variables, too. Since contact forces have unilateral constraints (e.g. feet cannot pull on the ground), and friction cone constraints, this inverse dynamics problem is almost always formulated as a quadratic program. For instance, as in this paper. We don't currently provide that QP formulation in Drake, but it is not hard to write it against the MathematicalProgram interface. And we do have some older code that was removed from Drake (since it wasn't actively developed) that we can point you to if it helps.

Robust Standard Errors in spatial error models

I am fitting a Spatial Error Model using the errorsarlm() function in the spdep library.
The Breusch-Pagan test for spatial models, calculated using the bptest.sarlm() function, suggest the presence of heteroskedasticity.
A natural next step would be to get the robust standard error estimates and update the p-values. In the documentation of the bptest.sarlm() function says the following:
"It is also technically possible to make heteroskedasticity corrections to standard error estimates by using the “lm.target” component of sarlm objects - using functions in the lmtest and sandwich packages."
and the following code (as reference) is presented:
lm.target <- lm(error.col$tary ~ error.col$tarX - 1)
if (require(lmtest) && require(sandwich)) {
print(coeftest(lm.target, vcov=vcovHC(lm.target, type="HC0"), df=Inf))}
where error.col is the spatial error model estimated.
Now, I can easily adapt the code to my problem and get the robust standard errors.
Nevertheless, I was wondering:
What exactly is the “lm.target” component of sarlm objects? I can not find any mention to it in the spdep documentation.
What exactly are $tary and $tarX? Again, it does not seem to be mentioned on the documentation.
Why documentation says it is "technically possible to make heteroskedasticity corrections"? Does it mean that proposed approach is not really recommended to overcome issues of heteroskedasticity?
I report this issue on github and had a response by Roger Bivand:
No, the approach is not recommended at all. Either use sphet or a Bayesian approach giving the marginal posterior distribution. I'll drop the confusing documentation. tary is $y - \rho W y$ and similarly for tarX in the spatial error model case. Note that tary etc. only occur in spdep in documentation for localmoran.exact() and localmoran.sad(); were you using out of date package versions?

Computing the generalized forces arising due to ExternallyAppliedSpatialForces in Drake, e.g. due to Propeller

I am working with a diagram which includes a MultiBodyPlant with a Propeller connected to it. The Propeller actually realizes numerous physical propellers which are distributed among the bodies of the MultiBodyPlant.
I am able to simulate the dynamics of the combined system by setting the prop forces with FixValue, so I'm on the right track.
What I'd like to be able to do is, given a configuration for the system (i.e. the MultiBodyPlant context) and a chosen propeller command, compute the generalized forces acting on the system. My sense is that this is not immediately available since the simulation is actually using the RNEA, and so does not aggregate the forces all together in that way. For what I'm doing (and even just as a sanity check), I would like to compute the forces directly, not just their effect on the evolution of the state.
Is there an existing method to compute this built into Drake, or should I compute it manually using the spatial jacobian of each propeller frame and the applied SpatialForce of the corresponding propeller? (Something along the lines of this question: How to get the matrix that maps external forces to generalized forces?)
Many thanks for your help.
I understand better now. It's a very reasonable request! You have two systems at play: the Propeller and MultibodyPlant. Unfortunately, the quantity you want is all of Propeller and just a piece of MultibodyPlant. We don't offer direct access to the B(q) matrix in that case.
What you can do is call with either AutoDiffXd or symbolic::Expression, call CalcImplicitTimeDerivativesResidual on the Diagram to get the entire dynamics in implicit form (to avoid taking M(q) inverse). You could call it twice -- once with the Propeller inputs set up via FixValue as AutoDiffXd and/or symbolic::Variable and again with them as zero, then subtract the difference.
Note: CalcImplicitTimeDerivativesResidual is relatively new; I haven't pushed the python bindings for it yet (but it's been on my list). Do you need it from python?
I think that perhaps you are looking for the MultibodyPlant reaction_forces output port?

Bootstapping hazard rates with non-linear interpolators

I have been using QuantLib 1.6.2 to bootstrap the hazard rates from a CDS
curve. My code is similar to the example "CDS.cpp" that comes with the
QuantLib distribution, i.e.,
boost::shared_ptr<PiecewiseDefaultCurve<HazardRate, BackwardFlat> >
hazardRateStructure(new PiecewiseDefaultCurve<HazardRate, BackwardFlat>
(todaysDate, instruments, Actual365Fixed()));
I tried to experiment with different non-linear interpolation methods (instead of BackwardFlat listed above) such as:
CubicNaturalSpline
KrugerCubic
Parabolic
FritschButlandCubic
MonotonicParabolic
but I am getting the error "no appropriate default constructor available". What is the proper way of passing one of these interpolators to the
PiecewiseDefaultCurve class?
Thank you,
Chris
[Note: in case someone stumbles on this question, I'm copying here the answer I gave you on the QuantLib mailing list.]
The classes you're listing are the actual interpolation classes, but the curve is expecting a corresponding factory class (for instance, BackwardFlat in the example is the factory for the BackwardFlatInterpolation class). In the case of cubic interpolations, you'll have to use the Cubic class. By default, it builds Kruger interpolations (I'm not aware of the reason for the choice) so if you write:
PiecewiseDefaultCurve<HazardRate, Cubic>(todaysDate, instruments, Actual365Fixed())
you'll get a curve using the KrugerCubic class. To get the other interpolations, you can pass a Cubic instance with the corresponding parameters (you can look them up in the constructors of the interpolation classes); for instance,
PiecewiseDefaultCurve<HazardRate, Cubic>(todaysDate, instruments, Actual365Fixed(),
1e-12, Cubic(CubicInterpolation::Spline, false))
will give you a curve using the CubicNaturalSpline class, and
PiecewiseDefaultCurve<HazardRate, Cubic>(todaysDate, instruments, Actual365Fixed(),
1e-12, Cubic(CubicInterpolation::Parabolic, true))
will use the MonotonicParabolic class.

Resources