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.
Why the the forward kinematics formula chain is different with the result of the getTransform function?
4 ビュー (過去 30 日間)
古いコメントを表示
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:
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1704311/image.jpeg)
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?
採用された回答
Karsh Tharyani
2024 年 5 月 30 日
編集済み: Karsh Tharyani
2024 年 5 月 30 日
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);
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)])')')
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'));
Hope that helps.
Karsh
0 件のコメント
その他の回答 (0 件)
参考
カテゴリ
Help Center および File Exchange で Coordinate Transformations についてさらに検索
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!