root_link is not set in

[richardrl]

from adam.pytorch import KinDynComputations

KinDynComputations takes in "root_link" as an argument but never actually sets it for use with self.rbdalgos.

For example, here: https://github.com/ami-iit/adam/blob/76c8457d8b19c7f834e9e6178fb5a6232069564a/src/adam/core/rbd_algorithms.py#L132C9-L132C21

The forward kinematics utilizes self.root_link but it is never set when initializing KinDynComputations.

Repro:

• https://pastebin.com/6QfEYjh8
• https://pastebin.com/8fmfdLiw

[traversaro]

@Giulero

[Giulero]

Hi @richardrl ! Thanks for opening the issue!

You're right. Actually, the root link cannot be changed freely when instantiating the class. That root_link argument in KinDynComputations was a placeholder for a future feature I had not yet implemented (even though I planned to do this in the first implementation). This was clearly a wrong choice :D I removed the option from the examples but we should also raise an error if the user tries to set the root_link. I'll open a PR to fix this.

The root link is chosen as the link with no parents, so even if you do not set it when you instantiate KinDynComputations, it is chosen when the urdf is handled and parsed in an adam model. In your case, self.root_link is the base and everything should work as expected!

[traversaro]

By the way, the feature of "rotating" the URDF by changing its base link is indeed supported in iDynTree, so one could use iDynTree to rotate an URDF, and feed the resulting URDF to adam.

I do not think we ever used that API a lot on Python, so it is not particularly ergonomic, but this is a simple example in which chest is used as a base link instead of root_link for an iCub model (based on a similar example from the repo README):

import adam
import casadi as cs
from adam.casadi import KinDynComputations
import icub_models
import numpy as np
import idyntree.bindings as idyntree
import tempfile

# if you want to icub-models https://github.com/robotology/icub-models to retrieve the urdf
model_path = icub_models.get_model_file("iCubGazeboV2_5")
# The joint list
joints_name_list = [
    'torso_pitch', 'torso_roll', 'torso_yaw', 'l_shoulder_pitch',
    'l_shoulder_roll', 'l_shoulder_yaw', 'l_elbow', 'r_shoulder_pitch',
    'r_shoulder_roll', 'r_shoulder_yaw', 'r_elbow', 'l_hip_pitch', 'l_hip_roll',
    'l_hip_yaw', 'l_knee', 'l_ankle_pitch', 'l_ankle_roll', 'r_hip_pitch',
    'r_hip_roll', 'r_hip_yaw', 'r_knee', 'r_ankle_pitch', 'r_ankle_roll'
]

# Rotate the model in idyntree
idyntree_model_reducer = idyntree.ModelLoader()
idyntree_model_reducer.loadReducedModelFromFile(str(model_path), joints_name_list)

# URDF has some peculiar convention, so we need to move the link frames to be compatible with a different base link
original_model_idyntree = idyntree_model_reducer.model()
original_model_idyntree.setDefaultBaseLink(original_model_idyntree.getLinkIndex("chest"))
rotated_model_idyntree = idyntree.Model()
idyntree.moveLinkFramesToBeCompatibleWithURDFWithGivenBaseLink(original_model_idyntree, rotated_model_idyntree)

# Export the rotated model to a URDF file
idyntree_model_exporter = idyntree.ModelExporter()
idyntree_model_exporter.init(rotated_model_idyntree)

# exportModelToString would be the perfect function to call here, but it doesn ot have Python-friendly signature,
# so we call exportModelToFile instead
with tempfile.NamedTemporaryFile(delete=False) as temp_file:
    temp_file_name = temp_file.name
print("Saving temp file to ", temp_file_name)
idyntree_model_exporter.exportModelToFile(temp_file_name)

kinDyn = KinDynComputations(temp_file_name, joints_name_list)
# choose the representation you want to use the body fixed representation
kinDyn.set_frame_velocity_representation(adam.Representations.BODY_FIXED_REPRESENTATION)
# or, if you want to use the mixed representation (that is the default)
kinDyn.set_frame_velocity_representation(adam.Representations.MIXED_REPRESENTATION)
w_H_b = np.eye(4)
joints = np.ones(len(joints_name_list))
M = kinDyn.mass_matrix_fun()
print(M(w_H_b, joints))

# If you want to use the symbolic version
w_H_b = cs.SX.eye(4)
joints = cs.SX.sym('joints', len(joints_name_list))
M = kinDyn.mass_matrix_fun()
print(M(w_H_b, joints))

# This is usable also with casadi.MX
w_H_b = cs.MX.eye(4)
joints = cs.MX.sym('joints', len(joints_name_list))
M = kinDyn.mass_matrix_fun()
print(M(w_H_b, joints))

[traversaro]

If there is an interest in this, we could even add an helper function to simplify this change of base functionality in adam itself, relying on the similar provided idyntree functionality.

[Giulero]

If there is an interest in this, we could even add an helper function to simplify this change of base functionality in adam itself, relying on the similar provided idyntree functionality.

I super agree!