Skip to content

iCub Forward Kinematics

The iCub forward kinematics are described with respect to a (root) reference frame which is positioned at the level of the waist in the center of the robot as represented in the following pic (colored cylinders: the x axis is in red, the y axis is in green, the z axis is in blue).
img-3 img-4

The origin of the root reference frame is a point on the axis of rotation of the torso pitch. Among all points in this axis, the origin is located in the middle of the robot in between the two legs.

  1. The z axis of the root reference frame is parallel to gravity but pointing upwards.
  2. The x axis of the root reference frame points behind the robot.
  3. The y axis of the root reference frame points laterally and is chosen according to the right hand rule.

For simpler visualization the root reference frame of the forward kinematics is depicted in the picture below (light blue) (for the exact position refer the CAD pictures above):

img-4

Seven additional reference frames are defined with respect to this common root reference frame. The reference frames are located as shown in the CAD figure. The x axis is in red. The y axis is in green. The z axis is in blue.

  • Left hand reference frame (see the CAD picture)
  • Right hand reference frame (see the CAD picture)
  • Left Fingers reference frame (see the CAD picture)
  • Right Fingers reference frame (see the CAD picture)
  • Left foot reference frame (see the CAD picture)
  • Right foot reference frame (see the CAD picture)
  • Left eye reference frame (see the CAD picture)
  • Right eye reference frame (see the CAD picture)
  • Inertia sensor reference frame (see the CAD picture)

The roto-translation which converts a point in one of these reference frames to the root reference frame are given by the following SE(3) matrices:

End effector Reference frame SE(3) Matrix Description Matlab files
All -> Root - Whole body (all reference frames) ICubFwdKinNew.zip
Left arm -> Root T_RoLa iCub Left Arm Forward Kinematics ICubFwdKinNew.zip
Right arm -> Root T_RoRa iCub Right Arm Forward Kinematics ICubFwdKinNew.zip
Left hand -> Root T_RoLt, T_RoLi, T_RoLm iCub Left Fingers Forward Kinematics -
Right hand -> Root T_RoRt, T_RoRi, T_RoRm iCub Right Fingers Forward Kinematics -
Left foot V1 -> Root T_RoLf iCub Left Leg Forward Kinematics V1 ICubFwdKinNew.zip
Left foot V2.5 -> Root T_RoLf iCub Left Leg Forward Kinematics V2.5 ICubFwdKinNewV2.5.zip
Right foot V1 -> Root T_RoRf iCub Right Leg Forward Kinematics V1 ICubFwdKinNew.zip
Right foot V2.5 -> Root T_RoRf iCub Right Leg Forward Kinematics V2.5 ICubFwdKinNewV2.5.zip
Left eye V1 -> Root T_RoLe iCub Head Forward Kinematics (left eye) V1 ICubFwdKinNew.zip
Left eye V2 -> Root T_RoLe iCub Head Forward Kinematics (left eye) V2 ICubFwdKinNewV2.zip
Right eye V1 -> Root T_RoRe iCub Head Forward Kinematics (right eye) V1 ICubFwdKinNew.zip
Right eye V2 -> Root T_RoRe iCub Head Forward Kinematics (right eye) V2 ICubFwdKinNewV2.zip
Inertia sensor V1 -> Root T_RoIs iCub Inertial Sensor Forward Kinematics V1 ICubFwdKinNew.zip
Inertia sensor V2 -> Root T_RoIs iCub Inertial Sensor Forward Kinematics V2 ICubFwdKinNewV2.zip

Each of these matrices is constructed with two steps. The first consists in a rigid roto-translation from the points in the root reference frame to points in the 0th reference frame as defined by the Denavit-Hartenberg convention. The second step corresponds to the Denavit-Hartenberg description of the forward kinematic, i.e. the roto-translation from the 0th reference frame to the nth reference frame being n the number of degrees of freedom.

Finally, the following image shows the root reference frame and all other coordinate systems at each joint of the robot (no hands included), along with the fixed transformation matrices from limbs to base reference frames. The Upper/Lower Torso division has been made according to the iDyn library convention (which is different the one found in iKin).

iCub Reference Frames