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 (brawner@gmail.com)
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?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 (brawner@gmail.com)
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? 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 (brawner@gmail.com)
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? urdf, calibration, gettransform MATLAB Answers — New Questions
