在ROS(Kinetic) 中利用URDF 编写 Denso 机器人的一个简单模型
Denso的URDF 模型主要是参照图1过来的,然后根据实际的情况进行了相应的修改。
在写模型过程中遇到的问题,在RVIZ 中的坐标系 红色为X,绿色为Y , 蓝色为Z 。rpy的数值是 沿着坐标系的箭头的方向,顺时针为正,逆时针为负。
有一点必须要注意的是:用URDF 生成的物体,如果没有做任何的旋转和位置变换的话,那么物体的坐标系的原点在自身物体的中心位置,而物体自身的坐标系和大地的参考坐标系是重合的,所以当你加载一个没有做任何变换的模型的时候,会看到物体的一半是在网格线的下面的。
在Link 属性中的 origin 条目中的 rpy 是连杆相对于自身的坐标系的旋转变换,三个系数依次对应 XYZ,这里和机器人学中的rpy值不太一样(重点);而 XYZ 则是是连杆的物体中心相对于自身的坐标系的原点的位置平移变换。
<link name="base_link">
<visual>
<geometry>
<box size="0.1 0.1 0.15"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0.075"/>
<material name="blue"/>
</visual>
</link>
从这里可以看出base_link 相对于自身的坐标系是有一个位置变换的,沿着Z 方向上移动了 0.075,刚好是base_link的高度的一半,所以在图中看到的base_link是全部露出来的,而不是一半在网格线的下面的。
在Joint 属性中的 origin 条目中的 rpy 是 child link 的坐标系相对于 parent link坐标系的旋转变换;而 XYZ 则是 child link 的坐标系的原点相对于 parent link坐标系的原点的位置平移变换,先进行旋转再进行平移,XYZ轴的旋转按照顺序依次进行。另外还有一点值得注意的地方是这里的旋转和机器人学中的DH旋转变换不太一样,机器人学中的DH变换是相对于运动坐标系,而URDF中则是相对于静止坐标系进行旋转的。最后当joint属性中的 upper 和 lower 的平均值不为零时,child link 会相对于joint 中指定的旋转轴旋转 (upper+lower)/2 弧度。有的时候当你发现结果不对的时候,可以想想是不是设置关节角度上下限出了问题
一般来说,直接在URDF中修改成DH的坐标系显得很繁琐,正常应该是在Solidwork中建好模型之后,然后导出为URDF模型
<joint name="joint1" type="revolute">
<parent link="base_link"/>
<child link="shoulder_link"/>
<origin rpy="0 0 0" xyz="0 0 0.15"/>
<axis xyz="0 0 1"/>
<limit effort="300" lower="-2.79252675555" upper="2.79252675555" velocity="1"/>
<dynamics damping="50" friction="1"/>
</joint>
从这里可以看出shoulder_link 相对于 base_link 坐标系是有一个位置变换的,沿着Z 方向上移动了 0.15,刚好是base_link的高度,所以在图中看到的shoulder_link的坐标系是在base_link的上表面。
<?xml version="1.0"?>
<robot name="myfirstrobot">
<material name="blue">
<color rgba="0 0 0.8 0.6"/>
</material>
<material name="white">
<color rgba="1 1 1 0.6"/>
</material>
<material name="orange">
<color rgba="1 0.4 0.1 0.6"/>
</material>
<!-- Base Link -->
<link name="base_link">
<visual>
<geometry>
<box size="0.1 0.1 0.15"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0.075"/>
<material name="blue"/>
</visual>
</link>
<joint name="joint1" type="revolute">
<parent link="base_link"/>
<child link="shoulder_link"/>
<origin rpy="0 0 0" xyz="0 0 0.15"/>
<axis xyz="0 0 1"/>
<limit effort="300" lower="-2.79252675555" upper="2.79252675555" velocity="1"/>
<dynamics damping="50" friction="1"/>
</joint>
<link name="shoulder_link">
<visual>
<geometry>
<cylinder length="0.125" radius="0.08"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0.0625"/>
<material name="white"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0.0625"/>
<geometry>
<cylinder length="0.125" radius="0.08"/>
</geometry>
</collision>
<inertial>
<mass value="1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="0.5" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<gazebo reference="shoulder_link">
<material>Gazebo/White</material>
</gazebo>
<joint name="joint2" type="revolute">
<parent link="shoulder_link"/>
<child link="elbow_link"/>
<origin rpy="1.570795 0 0" xyz="0 0 0.125"/>
<axis xyz="0 0 1"/>
<limit effort="300" lower="-2.094395066" upper="2.094395066" velocity="1"/>
<dynamics damping="50" friction="1"/>
</joint>
<link name="elbow_link">
<visual>
<geometry>
<box size="0.06 0.06 0.21"/>
</geometry>
<origin rpy="-1.570795 0 0" xyz="0 0.105 0"/>
<material name="orange"/>
</visual>
<collision>
<origin rpy="-1.570795 0 0" xyz="0 0.105 0"/>
<geometry>
<box size="0.06 0.06 0.21"/>
</geometry>
</collision>
<inertial>
<mass value="1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="0.5" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<gazebo reference="elbow_link">
<material>Gazebo/orange</material>
</gazebo>
<joint name="joint3" type="revolute">
<parent link="elbow_link"/>
<child link="wrist_link"/>
<origin rpy="0 0 1.570795" xyz="0 0.21 0"/>
<axis xyz="0 0 1"/>
<limit effort="300" lower="-1.2217304" upper="1.2217304" velocity="1"/>
<dynamics damping="50" friction="1"/>
</joint>
<link name="wrist_link">
<visual>
<geometry>
<box size="0.06 0.06 0.176"/>
</geometry>
<origin rpy="1.570795 0 0" xyz="0.03 0 0"/>
<material name="orange"/>
</visual>
<collision>
<origin rpy="1.570795 0 0" xyz="0.03 0 0"/>
<geometry>
<box size="0.06 0.06 0.176"/>
</geometry>
</collision>
<inertial>
<mass value="1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="0.5" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<gazebo reference="wrist_link">
<material>Gazebo/orange</material>
</gazebo>
<joint name="joint4" type="revolute">
<parent link="wrist_link"/>
<child link="shoulder_r_link"/>
<origin rpy="1.570795 -3.1415926 0" xyz="0.03 -0.088 0"/>
<axis xyz="0 0 1"/>
<limit effort="300" lower="-2.79252675555" upper="2.79252675555" velocity="1"/>
<dynamics damping="50" friction="1"/>
</joint>
<link name="shoulder_r_link">
<visual>
<geometry>
<cylinder length="0.122" radius="0.03"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0.061"/>
<material name="red"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0.061"/>
<geometry>
<cylinder length="0.122" radius="0.03"/>
</geometry>
</collision>
<inertial>
<mass value="1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="0.5" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<gazebo reference="shoulder_r_link">
<material>Gazebo/red</material>
</gazebo>
<joint name="joint5" type="revolute">
<parent link="shoulder_r_link"/>
<child link="elbow_r_link"/>
<origin rpy="1.570795 0 -1.570795" xyz="0 0 0.119"/>
<axis xyz="1 0 0"/>
<limit effort="300" lower="-2.094395066" upper="2.094395066" velocity="1"/>
<dynamics damping="50" friction="1"/>
</joint>
<link name="elbow_r_link">
<visual>
<geometry>
<cylinder length="0.070" radius="0.03"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 -0.005"/>
<material name="orange"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 -0.005"/>
<geometry>
<cylinder length="0.070" radius="0.03"/>
</geometry>
</collision>
<inertial>
<mass value="1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="0.5" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<gazebo reference="elbow_r_link">
<material>Gazebo/orange</material>
</gazebo>
<joint name="joint6" type="continuous">
<parent link="elbow_r_link"/>
<child link="wrist_r_link"/>
<origin rpy="-1.570795 0 -1.570795" xyz="0 0 -0.04"/>
<axis xyz="0 1 0"/>
<dynamics damping="50" friction="1"/>
</joint>
<link name="wrist_r_link">
<visual>
<geometry>
<cylinder length="0.020" radius="0.03"/>
</geometry>
<origin rpy="1.570795 0 0" xyz="0 0 0"/>
<material name="red"/>
</visual>
<collision>
<origin rpy="1.570795 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.020" radius="0.03"/>
</geometry>
</collision>
<inertial>
<mass value="1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="0.5" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<gazebo reference="wrist_r_link">
<material>Gazebo/red</material>
</gazebo>
</robot>