Missing mass / CoM / maybe centroidal momentum related methods in MBP

[siyuanfeng-tri]

We should probably add these back...

[sherm1]

@amcastro-tri a few missing methods from MBP. Do you have a sense for the priority of these, @siyuanfeng-tri ?

[siyuanfeng-tri]

no immediate use at the moment, i had some one shot use case, and got what i want from rbt. just figured that we should have a issue tracking this.

[mitiguy]

The Center of Mass parts of this issue were addressed with the recent addition of the following methods in MultibodyPlant: Vector3 MultibodyPlant::CalcCenterOfMassPosition(context); Vector3 MultibodyPlant::CalcCenterOfMassPosition(context, const std::vector& model_instances);

New question on StackOverflow How to obtain centroidal momentum matrix? https://stackoverflow.com/questions/63454077/

Proposed answer: Vector3 MultibodyPlant::CalcTranslationalMomentum(context); Vector3 MultibodyPlant::CalcTranslationalMomentum(context, const std::vector& model_instances); Vector6 MultibodyPlant::CalcSpatialMomentum(context, const Vector3& p_WoC_W); Vector6 MultibodyPlant::CalcSpatialMomentum(context, const Vector3& p_WoC_C, const std::vector& model_instances);

Note: The CalcTranslationalMomentum() methods are relatively inexpensive calculations and do not require an "about point". The CalcSpatialMomentum() methods are more expensive calculations and require an "about point", herein specified by p_WoC_W, which is the position vector from Wo (the origin of the World frame W) to an "about-point" C, expressed in world frame W.

A note would be added to the API that to find the Central Spatial Momentum, one would use: p_WoC_W = some_multibody_plant.CalcCenterOfMassPosition(context) spatial_momentum = some_multibody_plant.CalcSpatialMomentum(context, p_WoC_W);

[sherm1]

Since CalcSpatialMomentum() is sufficient, the translational special case would be there only for performance reasons. In this case it would be a classic premature optimization! We have no evidence yet that translational is useful on its own in applications we care about nor that getting the spatial momentum would be too slow even if only translational was needed. Every API comes with significant lifetime costs -- let's not introduce those additional APIs unless there is a proven need.

Would be good to hear from roboticists (hopefully from the stackoverflow OP) as to whether a general CalcSpatialMomentum() about any point makes sense to them. Would CalcSpatialMomentumAbout() be a better name? Is there any use for momentum about any other point than COM? Would it be better if we only provided the central momentum?

cc @hongkai-dai

[hongkai-dai]

Thanks Paul. I think the API is sufficient for the stackoverflow question, namely to compute both the linear and angular momentum about the Center of Mass. On the other hand, for robot control, often times people need to write the momentum as the product of a momentum matrix times the generalized velocity, namely

l = A(q) * v

where l is the stack vector of linear and angular momentum. Namely when doing control, the roboticists explicitly need the matrix A(q). The reason is that often times robot control requires computing the joint accelerations as

l_dot = A(q) * vdot + Adot(q) * v
Mv̇ + C(q, v) = B*u + Jᵀλ

so they solve the equations above by treating vdot, u and λ as unknowns.

[rcywongaa]

In my particular case of implementing the paper Whole-body Motion Planning with Centroidal Dynamics and Full Kinematics (Hongkai Dai, Andrés Valenzuela and Russ Tedrake), I only need the calculation of centroidal spatial momentum, i.e. CalcCenterOfMassSpatialMomentum.

In general, I would believe calculating the dynamics of the center of mass would be a fairly common task when using simplified centroidal dynamics models. Hence, I would imagine the existence of methods that are geared towards centroidal dynamics, the same way there exist specific COM methods CalcBiasCenterOfMassTranslationalAcceleration, CalcCenterOfMassPosition, CalcJacobianCenterOfMassTranslationalVelocity for their general counterparts CalcBiasTranslationalAcceleration, CalcPointsPositions, CalcJacobianTranslationalVelocity respectively.

I cannot speak for the usefulness of a general CalcSpatialMomentumAbout() though.

[amcastro-tri]

Question, is the case p_WoC_W != p_WoScm_W (with S the entire system composite) common? do we need it? I believe the CoM case would cover most cases?

Note: there is a SpatialMomentum which would come in handy here.

[mitiguy]

New proposal for simplicity and efficiency is in PR #13916. SpatialMomentum L_WS_W = plant.CalcSpatialMomentumInWorldAboutWo(context);

Spatial momentum can then be shifted to any other point with the Shift() method, e.g., for central spatial momentum: p_WoC_W = plant.CalcCenterOfMassPosition(context) central_spatial_momentum = L_WS_W.Shift(p_WoC_W);

[sherm1]

Per today's standup: roboticists Rick C and Huihua were in favor of something like CalcSpatialMomentumAbout(point) as above. Huihua said he has applications for both momentum about point=COM and point=foot location. (Here "point" would be the position vector of the point measured and expressed in World.)

[mitiguy]

Updated proposal for spatial momentum API in PR #13916.

To calculate the spatial momentum of an entire plant about an arbitrary point P: plant.CalcSpatialMomentumInWorldAboutPoint(context, p_WoP);

To calculate the spatial momentum of a selected set of objects about an arbitrary point P: plant.CalcSpatialMomentumInWorldAboutPoint(context, p_WoP);

Example: To calculate spatial momentum about the point Scm which denotes a system S's center of mass, use p_WoScm_W = plant.CalcCenterOfMassPosition(context) central_spatial_momentum = plant.CalcSpatialMomentumInWorldAboutPoint(context, p_WoScm_W);

[mitiguy]

PR #12839 was merge a few months ago and addressed calculations of center of mass. PR #13916 was merged today and resolves the remaining parts of issue #10779 (spatial momentum). An update note was added to the Stack Exchange question: https://stackoverflow.com/questions/63454077/

[hongkai-dai]

As asked in the stackoverflow question https://stackoverflow.com/questions/70576914/how-to-obtain-centroidal-momentum-matrix-explicitly-for-momentum-based-control-p, there is a need to compute the centroidal momentum matrix A(q), together with the quantity Adot(q, v) * v.

[sherm1]

@hongkai-dai can you open a separate issue for this and assign to @mitiguy for disposition?