Why the the forward kinematics formula chain is different with the result of the getTransform function?

Hello guys,

I made a test of the forward kinematics formula chain of the robot is constructed by extracting motion parameters from URDF, and the calculated results are inconsistent with the getTransform function. Can someone help me with this problem.My test codes are below

robot = importrobot('StepByStepChanged.urdf',DataFormat='column');
q0 = deg2rad([90, -90, 0, 90, 0, 90, 0])';
Tn=transl([0 0 0.1789])*trotx(0)*troty(0)*trotz(1.570796326794897)*trotz(-q0(1))...
*transl([0.002928236431969 2.430013955534704e-04 0.119599103269260])*trotx(0)*troty(3.062357948966975e-04)*trotz(0)*trotx(q0(2))...
*transl([-0.002914729213097  -0.001033720384413 0.155156998604446])*trotx(0.003609596843643)*troty(0)*trotz(0.022904943980785)*trotz(-q0(3))...
*transl([0.002968235617925 -0.065351124778835 0.304178518630177])*trotx(-0.003609596843643)*troty(0)*trotz(-0.022904943980785)*trotx(q0(4))...
*transl([-0.005205306143812  0.052692560433216  0.077902477491654])*trotx(0.004548981482162)*troty(-0.003199754330450)*trotz(-8.266717521666507e-05)*trotz(-q0(5))...
*transl([0.005280972133389  -9.031931320468978e-05  0.379510019774372])*trotx(3.137042207549150)*troty(-4.894462134554110e-05)*trotz(0.004660804199542)*trotx(-q0(6))...
*transl([-9.820067340677299e-04 -0.076367223695928 -0.106323102347568])*trotx(-3.130442252053242)*troty(-0.005548107438167)*trotz(1.567890031620816)*trotz(-q0(7))


jointPositions=q0';
T_urdf1=robot.getTransform(jointPositions','Link1','base_link')*robot.getTransform(jointPositions','Link2','Link1')...
       *robot.getTransform(jointPositions','Link3','Link2')*robot.getTransform(jointPositions','Link4','Link3')...
       *robot.getTransform(jointPositions','Link5','Link4')*robot.getTransform(jointPositions','Link6','Link5')...
       *robot.getTransform(jointPositions','Link7','Link6')

T_urdf=robot.getTransform(q0,'Link7','base_link')

StepByStepChanged.urdf is as follows:

<?xml version="1.0" encoding="utf-8"?>
<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner ([e-mail]) 
     Commit Version: 1.6.0-4-g7f85cfe  Build Version: 1.6.7995.38578
     For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
<robot
  name="Diana7Med">
<link name="world"/>

<joint name="fixed" type="fixed">
<parent link="world"/>
<child link="base_link"/>
</joint>

  <link
    name="base_link">
    <inertial>
      <origin
        xyz="-0.00024914 -0.00023302 0.076031"
        rpy="0 0 0" />
      <mass
        value="1.732" />
      <inertia
        ixx="0.0085914"
        ixy="-2.9842E-05"
        ixz="-2.6598E-05"
        iyy="0.0085918"
        iyz="-2.4568E-05"
        izz="0.006987" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0.013"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://Diana7Med/meshes/base_link.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0.013"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://Diana7Med/meshes/base_link.STL" />
      </geometry>
    </collision>
  </link>

 <joint
    name="joint1"
    type="revolute">
    <origin
      xyz="0 0 0.17890"
      rpy="0 0 1.570796326794897" />
    <parent
      link="base_link" />
    <child
      link="Link1" />
    <axis
      xyz="0 0 -1" />
    <limit
      lower="-3.1241"
      upper="3.1241"
      effort="254"
      velocity="1.5708" />
  </joint>


  <link
    name="Link1">
    <inertial>
      <origin
        xyz="0.043475 -3.8174E-06 0.096019"
        rpy="0 0 0" />
      <mass
        value="2.727" />
      <inertia
        ixx="0.011033"
        ixy="-4.8083E-07"
        ixz="-0.0022068"
        iyy="0.011337"
        iyz="-3.3735E-07"
        izz="0.008112" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://Diana7Med/meshes/Link1.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://Diana7Med/meshes/Link1.STL" />
      </geometry>
    </collision>
  </link>

<joint
    name="joint2"
    type="revolute">
    <origin
      xyz="0.002928236431969 2.430013955534704e-04 0.119599103269260"
      rpy="0 3.062357948966975e-04 0" />
    <parent
      link="Link1" />
    <child
      link="Link2" />
    <axis
      xyz="1 0 0" />
    <limit
      lower="-1.5708"
      upper="1.5708"
      effort="253"
      velocity="1.5708" />
  </joint>

  <link
    name="Link2">
    <inertial>
      <origin
        xyz="-0.036812 7.2275E-07 0.04165"
        rpy="0 0 0" />
      <mass
        value="3.1817" />
      <inertia
        ixx="0.016888"
        ixy="8.9891E-08"
        ixz="-0.0043558"
        iyy="0.018072"
        iyz="-2.2014E-07"
        izz="0.0096122" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://Diana7Med/meshes/Link2.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://Diana7Med/meshes/Link2.STL" />
      </geometry>
    </collision>
  </link>

 <joint
    name="joint3"
    type="revolute">
    <origin
      xyz="-0.002914729213097 -0.001033720384413 0.155156998604446"
      rpy="0.003609596843643 0 0.022904943980785" />
    <parent
      link="Link2" />
    <child
      link="Link3" />
    <axis
      xyz="0 0 -1" />
    <limit
      lower="-3.1241"
      upper="3.1241"
      effort="111"
      velocity="1.5708" />
  </joint>

<link
    name="Link3">
    <inertial>
      <origin
        xyz="0 -0.014808 0.17679"
        rpy="0 0 0" />
      <mass
        value="3.9362" />
      <inertia
        ixx="0.054706"
        ixy="0.0029594"
        ixz="-0.0079553"
        iyy="0.052971"
        iyz="0.010083"
        izz="0.013339" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://Diana7Med/meshes/Link3.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://Diana7Med/meshes/Link3.STL" />
      </geometry>
    </collision>
  </link>

<joint
    name="joint4"
    type="revolute">
    <origin
      xyz="0.002968235617925 -0.065351124778835 0.304178518630177"
      rpy="-0.003609596843643 0 -0.022904943980785" />
    <parent
      link="Link3" />
    <child
      link="Link4" />
    <axis
      xyz="1 0 0" />
    <limit
      lower="0"
      upper="3.0543"
      effort="111"
      velocity="1.5708" />
  </joint>

  <link
    name="Link4">
    <inertial>
      <origin
        xyz="-0.041116 0.015134 0.012416"
        rpy="0 0 0" />
      <mass
        value="1.5847" />
      <inertia
        ixx="0.0043925"
        ixy="-0.00067906"
        ixz="-0.00067352"
        iyy="0.0038733"
        iyz="-0.00083279"
        izz="0.0042261" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://Diana7Med/meshes/Link4.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://Diana7Med/meshes/Link4.STL" />
      </geometry>
    </collision>
  </link>

 <joint
    name="joint5"
    type="revolute">
    <origin
      xyz="-0.005205306143812 0.052692560433216 0.077902477491654"
      rpy="0.004548981482162 -0.003199754330450 -8.266717521666507e-05" />
    <parent
      link="Link4" />
    <child
      link="Link5" />
    <axis
      xyz="0 0 -1" />
    <limit
      lower="-3.1241"
      upper="3.1241"
      effort="53"
      velocity="2.9671" />
  </joint>

 <link
    name="Link5">
    <inertial>
      <origin
        xyz="-0.028829 0.0020991 0.15664"
        rpy="0 0 0" />
      <mass
        value="1.9268" />
      <inertia
        ixx="0.027058"
        ixy="0.00012672"
        ixz="0.007118"
        iyy="0.029321"
        iyz="-0.00025937"
        izz="0.0041403" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://Diana7Med/meshes/Link5.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://Diana7Med/meshes/Link5.STL" />
      </geometry>
    </collision>
  </link>

<joint
    name="joint6"
    type="revolute">
    <origin
      xyz="0.005280972133389 -9.031931320468978e-05 0.379510019774372"
      rpy="3.137042207549150 -4.894462134554110e-05 0.004660804199542" />
    <parent
      link="Link5" />
    <child
      link="Link6" />
    <axis
      xyz="-1 0 0" />
    <limit
      lower="-3.1241"
      upper="3.1241"
      effort="53"
      velocity="2.9671" />
  </joint>

<link
    name="Link6">
    <inertial>
      <origin
        xyz="0 -0.0122 1.2021"
        rpy="0 0 0" />
      <mass
        value="4.434E-12" />
      <inertia
        ixx="7.2173E-20"
        ixy="-9.8454E-20"
        ixz="-1.1913E-20"
        iyy="1.6421E-19"
        iyz="-7.0312E-21"
        izz="2.2322E-19" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://Diana7Med/meshes/Link6.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://Diana7Med/meshes/Link6.STL" />
      </geometry>
    </collision>
  </link>

<joint
    name="joint7"
    type="revolute">
    <origin
      xyz="-9.820067340677299e-04 -0.076367223695928 -0.106323102347568"
      rpy="-3.130442252053242 -0.005548107438167 1.567890031620816" />
    <parent
      link="Link6" />
    <child
      link="Link7" />
    <axis
      xyz="0 0 -1" />
    <limit
      lower="-3.1241"
      upper="3.1241"
      effort="53"
      velocity="2.9671" />
  </joint>

  <link
    name="Link7">
    <inertial>
      <origin
        xyz="1.2416E-09 -0.00041281 -0.015665"
        rpy="0 0 0" />
      <mass
        value="0.1669" />
      <inertia
        ixx="8.8902E-05"
        ixy="-9.3654E-10"
        ixz="-1.1631E-12"
        iyy="8.7425E-05"
        iyz="-2.6369E-07"
        izz="0.0001438" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 -0.0369"
        rpy="3.1416 0 0" />
      <geometry>
        <mesh
          filename="package://Diana7Med/meshes/Link7.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 -0.0369"
        rpy="3.1416 0 0" />
      <geometry>
        <mesh
          filename="package://Diana7Med/meshes/Link7.STL" />
      </geometry>
    </collision>
  </link>
  
<joint
    name="joint8"
    type="fixed">
    <origin
      xyz="0.05 0 0"
      rpy="0 0 0" />
    <parent
      link="Link7" />
    <child
      link="Link8" />
  </joint>


  <link
    name="Link8">
    <inertial>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <mass
        value="0" />
      <inertia
        ixx="0"
        ixy="0"
        ixz="-0"
        iyy="0"
        iyz="0"
        izz="0" />
    </inertial>
  </link>

</robot>

The results of the two calculation methods are as follows:

So as the figure shows the results of the two calculations are different, can someone tell me why? I have done several tests and have not been able to find out what is the cause of this result. Hope someone help me with this. And by the way,I wonder if there is any current experience or paper on the calibration of motion and dynamics in URDF?

[snow John]

The rpy order in URDF is computed in getTransform according to the rotz(y)roty(p)rotx(r) order, so the transformation order in Tn is wrong.

Answer 1

[Karsh Tharyani]

Hi @snow John,

You are correct. As per the URDF specification, https://wiki.ros.org/urdf/XML/joint , "rpy" is about a fixed axis. That means, the rotation sequences are given with respect to a fixed axis with rotations applied consequently to the intermediate frames. Here is some code to clarify this,

rpy=[-2*pi/3,-pi/7,pi/20];

% Let H be the homogeneous transformation matrix between the new frame "rolled-pitched-yawed" and the fixed frame.
% H transforms points in the new frame to the original fixed frame. In our
% example, all homogeneous transformations are rotations. Hence their
% inverse is just the transpose. Remember that rotation matrices are
% orthogonal.

% The fixed X axis (1,0,0) is (1,0,0) in the new frame when "roll"ed.
H=axang2tform([1,0,0,rpy(1)]);

% Find the rotation matrix along the original fixed Y-axis i.e., (0,1,0).
% (0,1,0) in the orignal fixed frame 
% expressed in the new frame's coordinate system is H'*(0,1,0,0)
ay=H'*[0,1,0,0]';
Ry=axang2tform([ay(1:3)',rpy(2)]);

% Now rotate frame H by Ry 
H=H*Ry;

% Find the rotation matrix along the original fixed Z-axis i.e., (0,0,1).
% (0,0,1) in the orignal fixed frame 
% expressed in the new frame's coordinate system is H'(0,0,1,0)
az=H'*[0,0,1,0]';
Rz=axang2tform([az(1:3)',rpy(3)]);

% Now rotate frame H by Rz
H=H*Rz;

% Here is the resulting frame expressed in original fixed frame.
disp(H);

    0.8899    0.4493    0.0788         0
    0.1409   -0.4351    0.8893         0
    0.4339   -0.7803   -0.4505         0
         0         0         0    1.0000

Now, if you carefully note, we are doing (RotX'*RotY'*RotZ')'

disp(...
    (axang2tform([1,0,0,rpy(1)])'*...
    axang2tform([0,1,0,rpy(2)])'*...
    axang2tform([0,0,1,rpy(3)])')')

    0.8899    0.4493    0.0788         0
    0.1409   -0.4351    0.8893         0
    0.4339   -0.7803   -0.4505         0
         0         0         0    1.0000

Which is same as, like you pointed out, as the Euler-Angles sequence ZYX for "Yaw", "Pitch", and "Roll".

disp(eul2tform([rpy(3),rpy(2),rpy(1)],'ZYX'));

    0.8899    0.4493    0.0788         0
    0.1409   -0.4351    0.8893         0
    0.4339   -0.7803   -0.4505         0
         0         0         0    1.0000

Hope that helps.

Karsh