[Bug]: Changing Root Joint doesn't do anything

[raabuchanan]

🐞 Bug

Maybe I am misunderstanding the purpose of root_joint but I want to use Pinocchio to get forward kinematics and dynamics expressed in a different frame from the base_link frame. Specifically I want it in the imu_link frame. However when I build the model and get the forward kinematics like below, it's always expressed in the base_link frame.

Reproduction steps

std::string root_joint = "joint_imu"; // I also tried a link in the urdf "imu_link"
pinocchio::Model model_;

pinocchio::urdf::buildModel(urdf_file_, JointModelFreeFlyer(), root_joint, robot_model_);

pinocchio::Data data_(model_);

 forwardKinematics(model_, data_, q_);
 updateFramePlacements(model_, data_);

data_.oMf[limb_frame_idx_[leg]].translation(); // right gives the foot position relative to the base_link frame not imu_link or joint_imu

Relevant log output / Error message

System Info

No response

Checklist

• I have checked that all my packages are installed from the same package manager (conda-forge, Nix, PyPI, robotpkg, ROS, ...)
• I have checked that there is no similar issue/discussion in the repo
• I have provided a minimal and working example to reproduce the bug
• I have used Markdown code blocks for both code and stack traces/compiler errors.

[jorisv]

Hello @raabuchanan,

joint_imu is a joint already defined in your urdf file ?
Here, you're creating a model from urdf_file_ whit a freeflyer root joint named joint_imu. This joint is added to your model and support the urdf defined root joint.
I don't think it's what you want to achieve.

[raabuchanan]

Yes, that's correct joint_imu and imu_link are already in the URDF with a translation relative to base_link. I don't want to make a new joint unless I can define it to be aligned with the existing imu_link. How can I do this?

[jorisv]

If you build your model from the ModelGraph API it's quite easy : https://github.com/stack-of-tasks/pinocchio/blob/devel/examples/model-graph.cpp#L66-L67

Unfortunately, there is no function to convert a Model to a ModelGraph right now. We plan to work on it (Or to make all parsers able to output ModelGraph) in few months.

A way to achieve that is to play with appendModel. You can create your model (modelB) with a fixe base, and then append a model with only a freeflyier (modelA) to the modelB joint_imu frame.

You can find one example here

[raabuchanan]

Thank you, I'll try out appendModel