Pinocchio: End-effector position and rotation

[alejandroastudillo]

Hi guys, thank you very much for all your effort building this amazing framework.

I just noticed that after building a model from an URDF (Kinova Gen3 robot, KUKA iiwa 14, etc), the variable model.njoints is missing one fixed joint which represents the end-effector. Then, for a 7-dof robot, model.njoints = 8 including the universe, but it should be 9 if the end-effector fixed joint is included. I have checked this with both the kinova gen3 and kuka iiwa14 urdf.

Due to this, I haven't been able to get the pose of the end-effector after using the pinocchio::forwardKinematics(model,data,q) function and printing the vector of joint placements data.oMi[k], since the oMi vector doesn't contain information about the fixed joint.

Is that a bug, or is there any other way to get the pose of the end-effector (and not of the last actuator)?

[jcarpent]

@alejandroastudillo

First of all, thanks a lot for using our tools.
Concerning your issue, in Pinocchio, we do not append fixed in the vector of joints (model.joints[]).
We want to avoid to have additional computation burdens when adding a fixed joint.

The idea we follow in Pinocchio is to append the fixed joints as a frame (model.frames[]). You can then retrieve its id with id = model.getFrameId("name") and then get its parent joint in the kinematic free with model.frames[id].parent.

You can also look at the notion of a frame which contains also the fixed transform between a frame and its parent joint.

I hope these details answer your issue.

[gabrielebndn]

Hi @alejandroastudillo,
as @jcarpent just said, "fixed" joints are not represented as joints in Pinocchio, but as frames.
You can get the corresponding frame id with frameId = model.getFrameId("name"). Then, the frame placement is found in data.oMf[frameId].

Notice frame placements are not computed by forwardKinematics. To compute frame placements, you have three options:

• after running forwardKinematics(model,data,q), run updateFramePlacements(model,data). This will fill data.oMf
• instead of forwardKinematics(model,data,q), run framesForwardKinematics(model,data). This is equivalent to forwardKinematics + updateFramePlacements
• after running forwardKinematics(model,data,q), run updateFramePlacement(model,data,frameId). This will set the correct value for data.oMf[frameId]. It will spare you the computation of the placement for all frames you do not need.

All the above methods are contained in the header pinocchio/algorithm/frames.hpp

[alejandroastudillo]

Thank you very much @jcarpent and @gabrielebndn
I was able to get the end-effector pose with that.

On the other hand, I have another two questions. I don't know if this is the correct place to ask them or should I open a new issue?

The first one is still related to the forward kinematics function. Since the dimension of the configuration vector representation nq is not the same as the dimension of the velocity vector space nv = Degrees of Freedom, is it possible to know to which joint does every index of the q vector corresponds? (so that one can apply the FK function with a custom q vector).

My second question is more about what I am working with, and the possibilities that Pinocchio can give to me. I am trying to interface Pinocchio with CasADi in order to generate symbolic expressions for the FK and FD of a robot based on its URDF, and use those expressions in the solution of an optimal control problem. So far I am scripting the use of custom scalar types in order to use symbolic variables as arguments in some Pinocchio methods. But I would like to know if you have already try to use symbolic variables with Pinocchio (maybe with CppAD)?

[gabrielebndn]

@alejandroastudillo you can easily find the index of joint i in the configuration vector through model.joints[i].idx_q(). The index in the velocity vector is model.joints[i].idx_v(). model.joints[i].nq() and model.joints[i].nv() give the size of the configuration and of the velocity for joint i.

[jcarpent]

My second question is more about what I am working with, and the possibilities that Pinocchio can give to me. I am trying to interface Pinocchio with CasADi in order to generate symbolic expressions for the FK and FD of a robot based on its URDF, and use those expressions in the solution of an optimal control problem. So far I am scripting the use of custom scalar types in order to use symbolic variables as arguments in some Pinocchio methods. But I would like to know if you have already try to use symbolic variables with Pinocchio (maybe with CppAD)?

Yes, sure. You can use Pinocchio with CppaAD and CppADCodeGen.
If you have installed CppAD and the counterpart CppADCodeGen, you can activate the compilation of the tests by setting ON the two Cmake variables: BUILD_WITH_AUTODIFF_SUPPORT and BUILD_WITH_CODEGEN_SUPPORT

Currently, I do not have concrete examples of how to use it, but the unit tests should be sufficient at first glance. I will try to make concrete the use of Pinocchio with these two frameworks. If you need more details, you can also have a look here, on Pinocchio's paper with benchmarks: https://hal-laas.archives-ouvertes.fr/hal-01866228

[alejandroastudillo]

thanks for your answers. You really helped me a lot.

[alejandroastudillo]

Hi, one small question to add here: Is it possible to retrieve the joint type ('Continuous, 'Revolute', etc) from the model or data structs?

[jmirabel]

There is a one to one mapping between C++ joint types and URDFs.
Have a look here: https://github.com/stack-of-tasks/pinocchio/blob/8e8f0711b1efa18e8dbc3ee0eea5693cb2cdd2b4/src/parsers/urdf/model.hxx#L223

[jcarpent]

@alejandroastudillo In the field model.joints[id].shortname()you can retrieve this information with the high-level interface (id is the id of the joint).
Another solution would be to do a dynamic cast, but this is not recommended.

[alejandroastudillo]

@jmirabel @jcarpent thank you very much. The shortname was what I was looking for.