Centroidal bias acceleration method is missing

[GiulioRomualdi]

To implement the task-based torque control I need to evaluate the centroidal bias acceleration, however, the KinDynComputation class seems not providing any method to get this quantity.

More specifically only the angular part of the centroidal bias acceleration is missing indeed the linear one is evaluated by the KinDynComputations::getCenterOfMassBiasAcc() method.

[traversaro]

~Note: to read this issue you need to have some browser plugin that renders LaTeX text in GitHub comments, such as https://github.com/traversaro/github-mathjax-firefox or https://chrome.google.com/webstore/detail/github-with-mathjax/ioemnmodlmafdkllaclgeombjnmnbima), sorry for the trouble.~

I suspect this will be easier that I expected, given that we already have the ComputeLinearAndAngularMomentumDerivativeBias. This is an important piece for this computation, because let's call $J_{ {}_A \mathrm{h}} \in \mathbb{R}^6 \times \mathbb{R}^{6+n}$ the "jacobian" of the linear and angular momentum of the robot expressed in the inertial frame $A$:

{}_A \mathrm{h} = J_{ {}_A \mathrm{h} } \nu

Its derivative will be

{}_A \dot{\mathrm{h}} =  J_{ {}_A \mathrm{h} } \dot{\nu} +  \dot{J}_{{}_A \mathrm{h} }\nu

the ComputeLinearAndAngularMomentumDerivativeBias should compute $\dot{J}_{{}_A \mathrm{h} }\~\nu$ (but I still need to double check), and this can be used for computing the centroidal average angular velocity bias acceleration.

[traversaro]

The non-obvious thing to derive is the composite/locked rotational inertia of whole mechanical system. Basically we have the mixed centroidal angular velocity ${}^{G[A]} \mathrm{v}_G$ is defined, with an abuse of notation (https://traversaro.github.io/preprints/traversaro-phd-thesis.pdf, Section 3.9.3):

{}^{G[A]} \mathrm{v}_G =  ({}_{G[A]} \mathbb{M}^C)^{-1} {}_{G[A]} X^{A} {}_A \mathrm{h}

The matrices involved in this computation are listed in the following.

• The composite/locked 6D inertia of the model, expressed with respect to the center of mass and with the orientation of the inertial frame (A), expressed with abused of notation $G[A]$ (abuse because $G$ is not a frame):

{}_{G[A]} \mathbb{M}^C = 
\begin{bmatrix}
m^C  1_3 & 0_{3 \times 3} \newline
0_{3 \times 3} & {}_{G[A]} I^C 
\end{bmatrix}

where $m \in \mathbb{R}$ is the total mass of the robot, $13 \in \mathbb{R}^{3 \times 3}$ is the identity matrix, and $ {}{G[A]} I^C $ is the 3D rotational inertia, expressed with the orientation of the inertial frame $A$ and w.r.t. to the center of mass of the system.

• The co-adjoint tranformation matrix between $A$ and $G[A]$:

  {}\_{G[A]} X^{A} = 
  \begin{bmatrix} 
  1_3 & 0_{3 \times 3} \newline
  -{}^A o_{A,G}^\wedge & 1_3
  \end{bmatrix}

  where ${}^A o{A,G}$ is the center of mass 3D position expressed in the inertial frame orientation and w.r.t. to the inertial frame origin (I am not 100% sure about the minus before ${}^A o{A,G}^\wedge$ , we should double check that).

• The 6D total momentum of the multibody system, expressed in the frame $A$, its linear part is actually just the usual linear momentum, that is exactly the velocity of the center of mass scaled by the mass of the robot (note: we use the ${}^l$ and ${}^a$ subscript to refer to the "linear" and "angular" part of a 6D vector).

  {}\_A \mathrm{h} 
  = 
  \begin{bmatrix}
  {}\_A \mathrm{h}^l  \newline 
  {}\_A \mathrm{h}^a
  \end{bmatrix}
  = 
  \begin{bmatrix}
  m {}^A \dot{o}_{A,G}
  \newline 
  \vdots  
  \end{bmatrix}

Given the nice structure (for the linear part) of the centroidal composite rigid body inertia and the co-adjoint transform, the acceleration of the linear part of the of the centroidal average 6D velocity can be easily expressed as:

{}^{G[A]} \mathrm{v}\_G^{l} = m {}\_A \mathrm{h}^l  

assuming (as usual) that the mass of each link of the multibody system is constant, also the total mass of the multibody system is constant, and so we have that:

{}^{G[A]} \dot{\mathrm{v}}\_G^{l} = m {}\_A \dot{\mathrm{h}}^l  =  J\_{ {}\_A \mathrm{h} }^l \dot{\nu} +  \dot{J}\_{{}\_A \mathrm{h} }^l \~\nu

and so the bias acc is clearly just $\dot{J}_{{}_A \mathrm{h} }^l \~\nu$. For the angular part unfortunately the solution is not so easy, because the angular part of ${}^{G[A]} \mathrm{v}_G$ (${}^{G[A]} \mathrm{v}_G^{l}$) depends in a non trivial way from both the angular and linear part of ${}_A \mathrm{h}$ (due to the co-adjoint transform $ {}_{G[A]} X^{A}$) and on the inverse of the 3D composite rotational inertia (due to the inverse of ${}_{G[A]} \mathbb{M}^C$). In my experience, when the linear and angular part of a computation are coupled, it is better to stick to the 6D formulation to obtain a readable result.

In this case, if we have that the centroidal angular velocity is defined as:

{}^{G[A]} \mathrm{v}\_G =  ({}\_{G[A]} \mathbb{M}^C)^{-1} {}\_{G[A]} X^{A} {}\_A \mathrm{h}

its derivative will be:

{}^{G[A]} \dot{\mathrm{v}}\_G =  
\frac{d}{dt}(({}\_{G[A]} {\mathbb{M}}^C)^{-1})  {}\_{G[A]} X^{A} {}\_A \mathrm{h} +
({}\_{G[A]} \mathbb{M}^C)^{-1} {}\_{G[A]} \dot{X}^{A} {}\_A \mathrm{h} +
({}\_{G[A]} \mathbb{M}^C)^{-1} {}\_{G[A]} X^{A} {}\_A \dot{\mathrm{h}}

Discussion on the three terms:

• $\frac{d}{dt}(({}_{G[A]} {\mathbb{M}}^C)^{-1})$ can be simplified with the usual derivative of inverse matrix (eq 59 of https://www.math.uwaterloo.ca/~hwolkowi/matrixcookbook.pdf) but to be honest what I do not know how to compute efficiently is the time derivative of ${}_{G[A]} {\mathbb{M}}^C$, this will probably need a separate derivation, perhaps there is something in some Patrick Wensing's paper. A naive implementation should not be to difficult, its basically just a bunch of sums of term in the form ${}_{C} X^{L} {}^L \mathrm{v}_{C,L} \bar{\times}^{*} \mathbb{M}_L {}^L XC$ and in the form ${}\{C} X^{L} \mathbb{M}_L {}^L XC {}^C \mathrm{v}{L, C} \times $
• ${}_{G[A]} \dot{X}^{A} = {}_{G[A]} {X}^{A} {}^A \mathrm{v}_{G[A],A} \bar{\times}^{*}$, because in general ${}_B \dot{X}^L = {}_{B} {X}^{L} {}^L \mathrm{v}_{B, L} \bar{\times}^{*}$, see equation 2.49 in my thesis.
• The bias term of the ${}_A \dot{\mathrm{h}}$ is already available, as discussed before, so that part is not a problem.

[GiulioRomualdi]

@traversaro

Given the nice structure (for the linear part) of the centroidal composite rigid body inertia and the co-adjoint transform, the acceleration of the linear part of the of the centroidal average 6D velocity can be easily expressed as:

{} ^ {G[A]} v ^ l _ G= m _ A h ^ l

assuming (as usual) that the mass of each link of the multibody system is constant, also the total mass of the multibody system is constant, and so we have that:

{} ^ {G[A]} \dot{v} ^ l _ G=m _ A \dot{h} ^ l=J ^ l _ { { {} _ A} h} \dot{\nu} + \dot{J} ^ l _ { { {} _ A} h} \nu

Is there a typo here? The following term

{} ^ {G[A]} \dot{v} ^ l _ G=m _ A \dot{h} ^ l=J ^ l _ { { {} _ A} h} \dot{\nu} + \dot{J} ^ l _ { { {} _ A} h} \nu

should be

{} ^ {G[A]} \dot{v} ^ l _ G=m _ A \dot{h} ^ l= m ( J ^ l _ { { {} _ A} h} \dot{\nu} + \dot{J} ^ l _ { { {} _ A} h} \nu)

Do you agree?

[GiulioRomualdi]

Furthermore, in case I want to evaluate the terms that compose ${} {G[A]} \dot{h}$ can I proceed as follows? Recalling that ${} {G[A]} h = {} {G[A]} X ^ A {} A h$ the term ${} _ {G[A]} \dot{h}$ can be evaluated by differentiation. More formally:

{} _ {G[A]} \dot{h} = {} _ {G[A]} \dot{X} ^ A {} _ A h +  {} _ {G[A]} X ^ A {} _ A \dot{h} 

where, as you (@traversaro) suggested, the term ${} {G[A]} \dot{X} ^ A$ can be substituded by ${} {G[A]} {X}^{A} {}^A \mathrm{v} {G[A], A} \bar{\times} ^ { * }$. While ${} A \dot{h}$ can be split into $J{ {} A \mathrm{h} } \dot{\nu} + \dot{J} {{} A \mathrm{h} } \nu$. Here, $J { {} A \mathrm{h} }$ is given by getLinearAngularMomentumJacobian(), while the second term $\dot{J} {{} A \mathrm{h} }~\nu$ can be evaluated using ComputeLinearAndAngularMomentumDerivativeBias().

So collecting all the quantities, ${} _ {G[A]} \dot{h}$ can be written in an elegant form

{} _ {G[A]} \dot{h} = ({} _ {G[A]} \dot{X} ^ A J _ { {} _ A \mathrm{h} } \nu + \dot{J} _ {{} _ A \mathrm{h} } \nu) +J_{ {} _ A \mathrm{h} } \dot{\nu} 

where the first term is a sort of bias acceleration (I do not know if it has a proper name)

[traversaro]

Do you agree?

Yes, sorry for the late reply.

[traversaro]

Do you agree?

Yes, sorry for the late reply.

Actually it seems that this part is wrong:

Here, $J { {} A \mathrm{h} }$ is given by getLinearAngularMomentumJacobian()