Interpreting the output of KinDynComputations::getAverageVelocityJacobian

[diegoferigo]

KinDynComputations::getAverageVelocityJacobian

import numpy as np
import gym_ignition_models
from gym_ignition.rbd.idyntree import kindyncomputations

np.set_printoptions(precision=4, suppress=True)

# Get the urdf model
urdf = gym_ignition_models.get_model_file(robot_name="iCubGazeboV2_5")

# Initialize KinDynComputations
kindyn = kindyncomputations.KinDynComputations(model_file=urdf)

# Defining:
#
# v_g ∈ ℝ⁶:   centroidal velocity
# v_b ∈ ℝⁿ⁺⁶: robot velocity
#
# We want to get J_avg such that:
#
# v_g = J_avg @ v_b

print(kindyn.get_world_base_transform())
# [[1. 0. 0. 0.]
#  [0. 1. 0. 0.]
#  [0. 0. 1. 0.]
#  [0. 0. 0. 1.]]

print(kindyn.get_com_position())
# [ 0.0121 -0.     -0.0767]

J_avg = kindyn.get_average_velocity_jacobian()

# Now, I expect that J_avg[0:6, 0:6] = G_X_B, but instead:
print(J_avg[0:6, 0:6])

# [[ 1.  0. -0.  0.  0.  0.]
#  [ 0.  1. -0.  0.  0.  0.]
#  [-0.  0.  1.  0.  0. -0.]
#  [-0. -0.  0.  1.  0. -0.]
#  [-0. -0. -0.  0.  1. -0.]
#  [ 0. -0. -0. -0. -0.  1.]]

[](https://github.com/nschloe/green-pi?activate&inlineMath=$)

[traversaro]

Where is the get_average_velocity_jacobian python method implemented? In the iDynTree bindings ? Or in gym_ignition ? Which version? I can't find it in https://github.com/robotology/gym-ignition/search?q=get_average_velocity_jacobian .

[traversaro]

Can you print the Jacobian with three/four significant numbers?

[diegoferigo]

The function is defined in https://github.com/robotology/gym-ignition/blob/fff3918a72c32b82a7449ca97109e4a295fa61dd/python/gym_ignition/rbd/idyntree/kindyncomputations.py#L384-L391.

Here below the jacobian:

[[ 1.0000e+00  1.3553e-20 -3.4694e-18  5.9292e-21  0.0000e+00  0.0000e+00]
 [ 0.0000e+00  1.0000e+00 -6.7763e-21  0.0000e+00  8.4703e-22  1.7347e-18]
 [-1.7347e-18  0.0000e+00  1.0000e+00  0.0000e+00  0.0000e+00 -9.5291e-22]
 [-2.7105e-20 -2.2204e-16  0.0000e+00  1.0000e+00  1.1858e-20 -3.4694e-18]
 [-2.2204e-16 -2.5411e-21 -5.5511e-17  3.3034e-20  1.0000e+00 -2.7105e-20]
 [ 0.0000e+00 -2.2204e-16 -2.7105e-20 -1.3878e-17 -5.4210e-20  1.0000e+00]]

[traversaro]

I think there is an unfortunate confusion and missing documentation in the naming in iDynTree w.r.t. to my PhD thesis, that is my fault.

TL;DR: You probably want to use KinDynComputations::getCentroidalAverageVelocityJacobian .

This comment contains inline LaTeX formulas that can be rendered thanks to extensions such as https://github.com/nschloe/green-pi .

iDynTree::KinDynComputations

In the KinDynComputations class, there are methods to compute both the AverageVelocity and the CentroidalAverageVelocity

AverageVelocity

$A$ is the absolute frame, $B$ is the base frame, $G[A]$ is the frame whose origin is the center of mass of the robot and the orientation is the absolute frame.

If the 6D AverageVelocity if FrameVelocityConvention is MIXED is defined as

$$ {}^{B[A]} \mathrm{v}{avgVel} = ({}_{B[A]} I^C )^{-1} {}\{B[A]} \mathrm{h} $$

where ${}_{B[A]} \mathrm{h} \in \mathbb{R}^6$ is the total 6D momentum of the robot, defined as: $$ {}_{B[A]} \mathrm{h} = \sum_{L \in \mathcal{L}} {}_{B[A]} X^{L} {}_{L} I_L {}^L \mathbb{v}_{A,L} $$

and ${}_{B[A]} I^C \in \mathbb{R}^{6 \times 6}$ is the so-called Composite Rigid Body Inertia (that apparently is used but not defined in my PhD thesis -_- ): $$ {}_{B[A]} I^C = \sum_{L \in \mathcal{L}}{}_{B[A]} X^L {}^L I_L {}^{L} X_{B[A]} $$

If the FrameVelocityConvention is INERTIAL or BODY, the definition of the average velocity is similar but expressed in $A$ or $B$ .

CentroidalAverageVelocity

The CentroidalAverageVelocity is exactly the average angular velocity, but expressed in a frame in which the origin is the center of mass. In particular, if FrameVelocityConvention is MIXED or ABSOLUTE is defined as the AverageVelocity, but expressed in the frame $G[A]$, i.e. : $$ {}^{G[A]} \mathrm{v}_{avgVel} = ({}_{G[A]} I^C )^{-1} {}_{G[A]} \mathrm{h} = {}^{G[A]} X_{B[A]} {}^{B[A]} \mathrm{v}_{avgVel} $$

If FrameVelocityConvention is BODY, instead the CentroidalAverageVelocity is expressed in $G[B]$.

Traversaro's PhD Thesis

In my PhD thesis (https://traversaro.github.io/traversaro-phd-thesis/traversaro-phd-thesis.pdf, section 3.86) the average velocity is introduced directly expressed in $G[A]$, so in iDynTree::KinDynComputations you can compute that by setting the FrameVelocityConvention to MIXED, and calling all the getCentroidalAverageVelocity method.

6 x 6 sub-block of the Jacobian

In the rest of the section, we assume that FrameVelocityConvention is MIXED. In this case, the first 6 x 6 sub-block of the AverageVelocityJacobian is: $$ {}^{B[A]} X_{B[A]} = 1_6 $$ so it is indeed the identity, as you saw.

On the other hand, the first 6 x 6 sub-block of the CentroidalAverageVelocityJacobian is: $$ {}^{G[A]} X_{B[A]} $$ that indeed in general is not the identity, but has a non-zero off-diagonal term that is related to the ${}^A o_G - {}^A o_B$.

[traversaro]

fyi @GiulioRomualdi @Giulero @CarlottaSartore @gabrielenava

[traversaro]

Please let me know if this clarify your doubts @diegoferigo .

[diegoferigo]

I think there is an unfortunate confusion and missing documentation in the naming in iDynTree w.r.t. to my PhD thesis, that is my fault.

TL;DR: You probably want to use KinDynComputations::getCentroidalAverageVelocityJacobian.

I just checked the section of your thesis and now I understood the confusion. If I had searched more the existing methods of KinDynComputations maybe I could have found the Centroidal variation. My fault of not going through them in enough detail. I guess now I need to make another PR similar to https://github.com/robotology/gym-ignition/pull/283.

(Centroidal) Average 6D Velocity

 

Thanks a lot for all the extra details provided in your comment, I think it is insightful to extend the analysis done in both your thesis and in A Unified View of the Equations of Motion used for Control Design of Humanoid Robots. I'm sure it will be helpful also to the other people tagged in this issue, I hope I was the only one that got confused and used the wrong methods.

I was aiming to obtain the mass matrix in centroidal coordinates, which is block diagonal, and the following code now produces the right result:

T = np.vstack([
    kindyn.get_centroidal_average_velocity_jacobian(),
    np.hstack([np.zeros(shape=(kindyn.dofs, 6)), np.eye(kindyn.dofs)])])

Tinv = np.linalg.inv(T)

Mc = Tinv.T @ kindyn.get_mass_matrix() @ Tinv
# [[33.0617 -0.     -0.      0.     -0.      0.    ]
#  [-0.     33.0617 -0.      0.     -0.      0.    ]
#  [-0.     -0.     33.0617  0.      0.      0.    ]
#  [ 0.      0.      0.      2.4944  0.0003 -0.0674]
#  [-0.     -0.      0.      0.0003  2.3958 -0.0003]
#  [ 0.      0.      0.     -0.0674 -0.0003  0.4005]]

[DanielePucci]

CC @robotology/iit-dynamic-interaction-control