Add end effector frame to Panda arm

[diegoferigo]

Our Panda model (#2) does not have any end effector frame, very useful for cartesian control.

This PR adds a fake link attached with a fixed joint to the panda_link7 link:

• Position: the center of the gripper's fingers
• Orientation: X axis pointing forward, Y axis aligned with finger's prismatic joints

Unfortunately I couldn't find a way to avoid adding a dummy mass to the fake end_effector_frame link. @traversaro do you have any smarter workaround?

cc @paolo-viceconte @eleramp

[traversaro]

Unfortunately I couldn't find a way to avoid adding a dummy mass to the fake end_effector_frame link. @traversaro do you have any smarter workaround?

Just to clarify, this is yet another problem that you have due to URDF's implicit constraint that link frames can only lie on the axis of the parent joint, using SDF, you do not have this constraints so you could directly place the link6's frame in the desired location.

Regarding the "dummy mass" the best long term solution is for the the URDF fake links to be converted to SDF 1.7 frames by the URDF --> SDF converted presend in sdformat (see https://bitbucket.org/osrf/sdformat/issues/199/urdf-to-sdf-conversion-ignores-links#comment-56105477 and the evergreen https://discourse.ros.org/t/urdf-ng-link-and-frame-concepts/56). However, a more short term solution that does not change the total inertial parameters of the robot is to decompose the inertia of the link7 in two parts, in a way that the total inertia is still the original one. In particular, let's say that $E$ is your original end effector frame, and $D$ is an additional dummy frame, and ${}_E I_7$ is the total inertia of the link 7, then you can decompose this inertia in two parts (the one associated with frame E and the one associated with the frame T as (given that the inertia is linear:

$$ {}_E I_7 = {}_E I_E + {}_E X^D {}_D I_D {}^E X_D $$ so you can compute the modified inertia of link 7, once you selected the dummy inertia $ I_T$ as: $$ {}_E I_E = {}_E I_6 - {}_E X^D {}_D I_D {}^E X_D $$

This can be easily done with iDynTree bindings.

Note that as we leave all the collision shapes in link7, all the external forces will be measured as forces on link7 and not on link8.

[traversaro]

Additional comment: when assigning frames for industrial robots, it may be convenient to follow the guidelines used in ROS-Industrial models:

• http://wiki.ros.org/Industrial/Tutorials/WorkingWithRosIndustrialRobotSupportPackages#Standardised_links_.2BAC8_frames
• http://wiki.ros.org/Industrial/Tutorials/Create%20a%20URDF%20for%20an%20Industrial%20Robot#Additional.2BAC8-Standard_Frames

[diegoferigo]

@traversaro Thank you a lot for the ROS industrial links, very interesting. If I have to extract the relevant change for this PR, probably the only change we should do is having the EE with the X axis pointing forward (right now it's Z pointing forward).

It is not a big change, and I can take care of it in short time.

However, a more short term solution that does not change the total inertial parameters of the robot is to decompose the inertia of the link6 in two parts, in a way that the total inertia is still the original one.

Great explanation :heart_eyes: For the aims of our first experiments, I think that these very small masses can be left as they are. I will open a new issue linking your comment to keep track of this activity that we can implement in another PR.

Since we got the inertial parameters from outside the repo, we are not even sure that they are correct and having this small additional mass does not block our current experiments.

However, a more short term solution that does not change the total inertial parameters of the robot is to decompose the inertia of the link6 in two parts, in a way that the total inertia is still the original one. [...] Note that as we leave all the collision shapes in link6, all the external forces will be measured as forces on link6 and not on link7.

I'm not sure I got this right. What's the role of link6 in this case? Here below you can find the current kinematic tree if it helps:

Model: 
  Links: 
    [0] panda_link0
    [1] panda_link1
    [2] panda_link2
    [3] panda_link3
    [4] panda_link4
    [5] panda_link5
    [6] panda_link6
    [7] panda_link7
    [8] panda_link8
    [9] panda_hand
    [10] panda_leftfinger
    [11] panda_rightfinger
  Frames: 
    [12] world --> panda_link0
  Joints: 
    [0] panda_joint1 (dofs: 1) : panda_link0<-->panda_link1
    [1] panda_joint2 (dofs: 1) : panda_link1<-->panda_link2
    [2] panda_joint3 (dofs: 1) : panda_link2<-->panda_link3
    [3] panda_joint4 (dofs: 1) : panda_link3<-->panda_link4
    [4] panda_joint5 (dofs: 1) : panda_link4<-->panda_link5
    [5] panda_joint6 (dofs: 1) : panda_link5<-->panda_link6
    [6] panda_joint7 (dofs: 1) : panda_link6<-->panda_link7
    [7] panda_finger_joint1 (dofs: 1) : panda_hand<-->panda_leftfinger
    [8] panda_finger_joint2 (dofs: 1) : panda_hand<-->panda_rightfinger
    [9] panda_joint8 (dofs: 0) : panda_link7<-->panda_link8
    [10] panda_hand_joint (dofs: 0) : panda_link8<-->panda_hand

and this is the tree with the edits of this PR:

Model: 
  Links: 
    [0] panda_link0
    [1] panda_link1
    [2] panda_link2
    [3] panda_link3
    [4] panda_link4
    [5] panda_link5
    [6] panda_link6
    [7] panda_link7
    [8] end_effector_frame        <-- New link
    [9] panda_link8
    [10] panda_hand
    [11] panda_leftfinger
    [12] panda_rightfinger
  Frames: 
    [13] world --> panda_link0
  Joints: 
    [0] panda_joint1 (dofs: 1) : panda_link0<-->panda_link1
    [1] panda_joint2 (dofs: 1) : panda_link1<-->panda_link2
    [2] panda_joint3 (dofs: 1) : panda_link2<-->panda_link3
    [3] panda_joint4 (dofs: 1) : panda_link3<-->panda_link4
    [4] panda_joint5 (dofs: 1) : panda_link4<-->panda_link5
    [5] panda_joint6 (dofs: 1) : panda_link5<-->panda_link6
    [6] panda_joint7 (dofs: 1) : panda_link6<-->panda_link7
    [7] panda_finger_joint1 (dofs: 1) : panda_hand<-->panda_leftfinger
    [8] panda_finger_joint2 (dofs: 1) : panda_hand<-->panda_rightfinger
    [9] end_effector_frame_fixed_joint (dofs: 0) : panda_link7<-->end_effector_frame      <-- new fixed joint
    [10] panda_joint8 (dofs: 0) : panda_link7<-->panda_link8
    [11] panda_hand_joint (dofs: 0) : panda_link8<-->panda_hand

[traversaro]

I'm not sure I got this right. What's the role of link6 in this case? Here below you can find the current kinematic tree if it helps:

Sorry, I am too used to work with 6 dofs industrial arms. I actually meant with link6 the last "real" link (that is actually link7) and link7 the additional fake one (that is actually link8), I will edit the comment.

[diegoferigo]

Thanks. Just to be precise, panda_link7, panda_link8, panda_hand are all lumped together in the final SDF (that will only have panda_link7). The only fixed joint that persists is the new end_effector_frame_fixed_joint that connects the new end_effector_frame with panda_link7.

[diegoferigo]

I modified the model to have the X axis pointing forward. I updated the first post and image accordingly.

Thanks @traversaro for pointing this out, now imagining the pose of the EE wrt the base is way more easier.