Add high-level helper methods

[diegoferigo]

• Enable to build Pose from homogeneous matrix (ref)
• Return homogeneous matrix from Pose (ref)
• Return 3D inertia tensor from Inertial
• Return if a model is fixed-base or floating-base
• Return the canonical link (ref)

[traversaro]

Did you validated/tested that this is compatible with http://sdformat.org/tutorials?tut=specify_pose ? Asking as this is tipically is an endless source of problems.

[diegoferigo]

I didn't validate specifically, but I was using this logic already in jaxsim and the models were constructed correctly. Anyway, since have a ready colcon workspace with Ignition Gazebo Fortress, better to check. I compiled the examples with the following commands:

cd /path/to/workspace/src/ign-math/examples
cmake -S . -B build

ign-math/examples (65a7c24) via △ v3.24.1 via 🐍 v3.8.13 via 💎 v3.1.2 🅒 /conda  
✦ ❯ ./build/quaternion_from_euler 0.1 -0.2 0.3
Converting Euler angles:
 roll   0.100000 radians
 pitch -0.200000 radians
 yaw    0.300000 radians

 roll      5.729578 degrees
 pitch   -11.459156 degrees
 yaw      17.188734 degrees

to Quaternion
 W  0.981856
 X  0.064071
 Y -0.091158
 Z  0.153439

to Rotation matrix
    0.936293  -0.312992  -0.159345
    0.289629   0.944702  -0.153792
    0.198669   0.097843   0.975170

In Python, instead:

In [1]: import rod

In [2]: pose = rod.Pose(pose=[0, 0, 0, 0.1, -0.2, 0.3])

In [3]: pose.rpy
Out[3]: [0.1, -0.2, 0.3]

In [4]: pose.transform()
Out[4]: 
array([[ 0.93629, -0.31299, -0.15935,  0.     ],
       [ 0.28963,  0.9447 , -0.15379,  0.     ],
       [ 0.19867,  0.09784,  0.97517,  0.     ],
       [ 0.     ,  0.     ,  0.     ,  1.     ]])

In [5]: rod.Pose.from_transform(pose.transform())
Out[5]: Pose(pose=[0.0, 0.0, 0.0, 0.1, -0.19999999999999996, 0.29999999999999993], relative_to=None, degrees=None, rotation_format=None)

So it seems correct. Note that the scipy class does not always produce the same numerical sequence of rotations since it might apply different but equivalent ranges of the computed Euler angles.

[traversaro]

Ok