Can SpatialInertia be bypassed when simulating a MultiBodyPlant in Drake?

Question

I'm attempting to use Drake to simulate an underwater manipulator. One important element of being underwater is that there's an added mass term in addition to a rigid body mass term in the manipulator equation. From the CalcInverseDynamics() function documentation, a kinematic tree has an inverse dynamics equation of:

tau = M(q)v' + C(q, v)v - tau_app - sum(applied spatial forces)

To include the added mass, this should become:

tau = (M(q)+AM(q))v' + C(q, v)v - tau_app - sum(applied spatial forces)

where AM(q) is the configuration-dependent added mass. I've used the Newton-Euler recursive method to solve the inverse dynamics before by including the "added mass" in the spatial inertia matrices for each link. However, as discussed in a previous question (Can a SpatialInertia object be constructed from a 6x6 matrix?), the added mass creates a spatial inertia with a mass submatrix with three different masses instead of one uniform mass (diagonals of [m1 m2 m3] instead of [m m m]), and thus can't be represented by a SpatialInertia object in Drake.

My follow-up question now is, is it possible within the framework of Drake to perform inverse and forward dynamics on a kinematic tree without the SpatialInertia object, or to somehow use ArticulatedBodyInertias instead? It was suggested in the previous post that ArticulatedBodyInertia could represent the spatial inertia with the added mass, but it's unclear to me how it could be used for forward/inverse dynamics and for simulation.

From reading the source code (with my very limited understanding of C++), it seems that SpatialInertia is a fundamental building block to the forward and inverse dynamic functions, as well as the simulation, of a MultiBody Plant.

I should learn C++, but right now I'm using the Python bindings for Drake and would not be comfortable heavily modifying the Drake C++ code. If the answer is "Nope, it is impossible to bypass the SpatialInertia object without making major modifications to the source code", that is understandable, but I figured I would ask those of you more familiar with the program before throwing in the towel.

Thanks in advance for any thoughts on the matter.

Answer 1

Unfortunately the short answer is no, you cannot do that. However, we could help you out if you tell us more about this problem. As far as I remember, the biggest assumption for our SpatialInertia objects is that under the hood they lead to SPD 6x6 matrices. If your new inertia including the virtual mass terms has this property, I don't see why not we could open the door for this kind of modeling. From Can a SpatialInertia object be constructed from a 6x6 matrix? I see you only need to have different mass values in each direction. I believe that it'd be easy to show that for as long as we "add" to the original inertia, then it should stay SPD.

Long story short, can you tell us more about the mathematical properties of this new 6x6 object?

Hacky idea

You could model your virtual mass as an external force on the plant. However, since this external force depends on acceleration, you'd inevitably create an algebraic loop that Drake cannot resolve. The quick and dirty solution to this would be to add a short delay between the virtual mass system block and the plant. If the delay is significantly smaller than the time scales of your problem, then you will be fine and Drake integrators will work like a breeze.

One way to accomplish this is adding a FirstOrderLowPassFilter between the accelerations computed by MBP (you can access them with get_body_spatial_accelerations_output_port()) and the system block that computes the hydrodynamic forces. Since FirstOrderLowPassFilter has state, that breaks the direct feedthrough that you'd have if you connected the spatial acceleration dependent hydrodynamic forces directly into the MBP's inputs. You'll need to choose a time constant tau of the filter. You choose the time constant by considering what frequencies higher than 1/tau you don't care about (a lil Bode analysis here will lead you in the right direction).

Summarizing, here is my bad attempt to an ASCII sketch of your Diagram:

                      ┌───────────────────┐
                      │                   │
applied_spatial_force │                   │ body_spatial_accelerations
               ┌─────►│   MultibodyPlant  ├────────────────────────┐
               │      │                   │                        │
               │      │                   │                        │
               │      └───────────────────┘        ┌───────────────┴─────────────────┐
               │                                   │                                 │
               │                                   │ SpatialAccelerationToBasicVector│
               │                                   │                                 │
               │                                   └───────────────┬─────────────────┘
               │                                                   │
               │    ┌─────────────┐       ┌──────────────┐         │
               │    │             │       │              │         │
               └────┤ HydroForces │◄──────┤ LowPassFilter├─────────┘
                    │             │       │              │
                    └─────────────┘       └──────────────┘

You'll need to write SpatialAccelerationBasicVector yourself to convert from what MBP outputs (SpatialAccleration) to what the LowPassFilter consumes (BasicVector).