there are useful APIs to calc centroidal momentum, like CalcSpatialMomentumInWorldAboutPoint
. But for momentum-based control, I ran into some problems with this API. During control there is:
l = A(q) * v
and I want to take this as a constraint to solve vdot
(joint accelerations)
l_dot = A(q) * vdot + Adot(q) * v
where l
is the stacked vector of linear and angular momentum, A(q)
is the centroidal momentum matrix.
My questions are:
- Is there any method to get the centroidal momentum matrix explicitly?
- Currently drake use
CalcBiasCenterOfMassTranslationalAcceleration
to get the translational part ofAdot(q) * v
, can we get the rotational part too?
@hongkai-dai discussed this at here, it is also mentioned in other literature [1][2]. Thanks very much for any suggestions.
[1] Koolen, Twan, et al. "Design of a momentum-based control framework and application to the humanoid robot atlas." International Journal of Humanoid Robotics 13.01 (2016): 1650007.
[2] Lee, Sung-Hee, and Ambarish Goswami. "A momentum-based balance controller for humanoid robots on non-level and non-stationary ground." Autonomous Robots 33.4 (2012): 399-414.