2 <!-- Open_Manipulator Chain -->
3 <robot name="open_manipulator" xmlns:xacro="http://ros.org/wiki/xacro">
5 <!-- Import all Gazebo-customization elements, including Gazebo colors -->
6 <xacro:include filename="$(find open_manipulator_description)/urdf/open_manipulator.gazebo.xacro" />
7 <!-- Import Rviz colors -->
8 <xacro:include filename="$(find open_manipulator_description)/urdf/materials.xacro" />
9 <xacro:include filename="$(find realsense2_description)/urdf/_d435.urdf.xacro"/>
11 <!-- Transmission macro -->
12 <xacro:macro name="SimpleTransmission" params="joint n">
13 <transmission name="tran${n}">
14 <type>transmission_interface/SimpleTransmission</type>
15 <joint name="${joint}">
16 <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
18 <actuator name="motor${n}">
19 <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
20 <mechanicalReduction>1</mechanicalReduction>
25 <link name="open_manipulator_base">
28 <joint name="world_fixed" type="fixed">
29 <origin xyz="0 0 0" rpy="0 0 0"/>
30 <parent link="open_manipulator_base"/>
37 <origin xyz="0 0 0" rpy="0 0 0"/>
39 <mesh filename="package://open_manipulator_description/meshes/chain_link1.stl" scale="0.001 0.001 0.001"/>
41 <material name="grey"/>
45 <origin xyz="0 0 0" rpy="0 0 0"/>
47 <mesh filename="package://open_manipulator_description/meshes/chain_link1.stl" scale="0.001 0.001 0.001"/>
52 <origin xyz="0 0 0" />
53 <mass value="0.082" />
54 <inertia ixx="0.1" ixy="0.0" ixz="0.0"
60 <origin xyz="3.0876154e-04 0.0000000e+00 -1.2176461e-04" />
61 <mass value="7.9119962e-02" />
62 <inertia ixx="1.2505234e-05" ixy="0.0" ixz="-1.7855208e-07"
63 iyy="2.1898364e-05" iyz="0.0"
64 izz="1.9267361e-05" />
69 <joint name="joint1" type="revolute">
70 <parent link="link1"/>
72 <origin xyz="0.012 0.0 0.036" rpy="0 0 0"/> <!-- Height servo+horn -->
74 <limit velocity="3.1" effort="1" lower="${-pi}" upper="${pi}" />
77 <!-- Transmission 1 -->
78 <xacro:SimpleTransmission n="1" joint="joint1" />
84 <mesh filename="package://open_manipulator_description/meshes/chain_link2.stl" scale="${0.001*33.5/28.5} ${0.001*44/34} ${0.001*58.5/46.5}"/> <!-- scale by XM540 -->
86 <material name="grey"/>
91 <mesh filename="package://open_manipulator_description/meshes/chain_link2.stl" scale="${0.001*33.5/28.5} ${0.001*44/34} ${0.001*58.5/46.5}"/> <!-- scale by XM540 -->
96 <origin xyz="0 0 0" />
97 <mass value="0.098" />
98 <inertia ixx="0.1" ixy="0.0" ixz="0.0"
104 <origin xyz="-3.0184870e-04 5.4043684e-04 ${0.018 + 2.9433464e-02}" />
105 <mass value="9.8406837e-02" />
106 <inertia ixx="3.4543422e-05" ixy="-1.6031095e-08" ixz="-3.8375155e-07"
107 iyy="3.2689329e-05" iyz="2.8511935e-08"
108 izz="1.8850320e-05" />
113 <joint name="joint2" type="revolute">
114 <parent link="link2"/>
115 <child link="link3"/>
116 <origin xyz="0.0 0.0 0.0515" rpy="0 0 0"/> <!-- fr13-s102k to middle of servo horn -->
118 <limit velocity="4.8" effort="1" lower="${-pi*0.64}" upper="${pi*0.64}" />
121 <!-- Transmission 2 -->
122 <xacro:SimpleTransmission n="2" joint="joint2" />
127 <origin xyz="0 0 0" rpy="0 0 0"/>
129 <mesh filename="package://open_manipulator_description/meshes/chain_link3.stl" scale="0.001 0.001 ${0.001*(1+0.004/0.128)}"/> <!-- scale by 4mm more, see joint3 -->
131 <material name="grey"/>
135 <origin xyz="0 0 0" rpy="0 0 0"/>
137 <mesh filename="package://open_manipulator_description/meshes/chain_link3.stl" scale="0.001 0.001 ${0.001*(1+0.004/0.128)}"/> <!-- scale by 4mm more, see joint3 -->
142 <origin xyz="0 0 0" />
143 <mass value="0.136" />
144 <inertia ixx="0.1" ixy="0.0" ixz="0.0"
150 <origin xyz="1.0308393e-02 3.7743363e-04 1.0170197e-01" />
151 <mass value="1.3850917e-01" />
152 <inertia ixx="3.3055381e-04" ixy="-9.7940978e-08" ixz="-3.8505711e-05"
153 iyy="3.4290447e-04" iyz="-1.5717516e-06"
154 izz="6.0346498e-05" />
159 <joint name="joint3" type="revolute">
160 <parent link="link3"/>
161 <child link="link4"/>
162 <origin xyz="0.024 0 ${0.128+0.004}" rpy="0 0 0"/> <!-- 4mm more for FR13-H101K-->
164 <limit velocity="4.8" effort="1" lower="${-pi*0.64}" upper="${pi*0.48}" />
167 <!-- Transmission 3 -->
168 <xacro:SimpleTransmission n="3" joint="joint3" />
173 <origin xyz="0 0 0" rpy="0 0 0"/>
175 <mesh filename="package://open_manipulator_description/meshes/chain_link4.stl" scale="0.001 0.001 0.001"/>
177 <material name="grey"/>
181 <origin xyz="0 0 0" rpy="0 0 0"/>
183 <mesh filename="package://open_manipulator_description/meshes/chain_link4.stl" scale="0.001 0.001 0.001"/>
188 <origin xyz="0 0 0" />
189 <mass value="0.131" />
190 <inertia ixx="0.1" ixy="0.0" ixz="0.0"
196 <origin xyz="9.0909590e-02 3.8929816e-04 2.2413279e-04" />
197 <mass value="1.3274562e-01" />
198 <inertia ixx="3.0654178e-05" ixy="-1.2764155e-06" ixz="-2.6874417e-07"
199 iyy="2.4230292e-04" iyz="1.1559550e-08"
200 izz="2.5155057e-04" />
205 <joint name="joint4" type="revolute">
206 <parent link="link4"/>
207 <child link="link5"/>
208 <origin xyz="0.124 0.0 0.0" rpy="0 0 0"/>
210 <limit velocity="4.8" effort="1" lower="${-pi*0.57}" upper="${pi*0.65}" />
213 <!-- Transmission 4 -->
214 <xacro:SimpleTransmission n="4" joint="joint4" />
219 <origin xyz="0 0 0" rpy="0 0 0"/>
221 <mesh filename="package://open_manipulator_description/meshes/chain_link5.stl" scale="0.001 0.001 0.001"/>
223 <material name="grey"/>
227 <origin xyz="0 0 0" rpy="0 0 0"/>
229 <mesh filename="package://open_manipulator_description/meshes/chain_link5.stl" scale="0.001 0.001 0.001"/>
234 <origin xyz="0 0 0" />
235 <mass value="0.141" />
236 <inertia ixx="0.1" ixy="0.0" ixz="0.0"
242 <origin xyz="4.4206755e-02 3.6839985e-07 8.9142216e-03" />
243 <mass value="1.4327573e-01" />
244 <inertia ixx="8.0870749e-05" ixy="0.0" ixz="-1.0157896e-06"
245 iyy="7.5980465e-05" iyz="0.0"
246 izz="9.3127351e-05" />
250 <!-- Gripper link -->
251 <link name="gripper_link">
253 <origin xyz="0.0 0.0 0" rpy="0 0 0"/>
255 <mesh filename="package://open_manipulator_description/meshes/chain_link_grip_l.stl" scale="0.001 0.001 0.001"/>
257 <material name="grey"/>
261 <origin xyz="0.0 0.0 0" rpy="0 0 0"/>
263 <mesh filename="package://open_manipulator_description/meshes/chain_link_grip_l.stl" scale="0.001 0.001 0.001"/>
268 <origin xyz="0 0 0" />
269 <mass value="0.017" />
270 <inertia ixx="1.0e-03" ixy="0.0" ixz="0.0"
271 iyy="1.0e-03" iyz="0.0"
276 <origin xyz="${0.028 + 8.3720668e-03} ${0.0246 + 9.9696160e-03} -4.2836895e-07" />
277 <mass value="3.2218127e-02" />
278 <inertia ixx="9.5568826e-06" ixy="2.8424644e-06" ixz="-3.2829197e-10"
279 iyy="2.2552871e-05" iyz="-3.1463634e-10"
280 izz="1.7605306e-05" />
284 <!-- Gripper joint -->
285 <joint name="gripper" type="prismatic">
286 <parent link="link5"/>
287 <child link="gripper_link"/>
288 <origin xyz="0.0817 0.021 0.0" rpy="0 0 0"/>
290 <limit velocity="4.8" effort="1" lower="-0.010" upper="0.019" />
293 <!-- Transmission 5 -->
294 <xacro:SimpleTransmission n="5" joint="gripper" />
296 <!-- Gripper link sub -->
297 <link name="gripper_link_sub">
299 <origin xyz="0.0 -0.0 0" rpy="0 0 0"/>
301 <mesh filename="package://open_manipulator_description/meshes/chain_link_grip_r.stl" scale="0.001 0.001 0.001"/>
303 <material name="grey"/>
307 <origin xyz="0.0 -0.0 0" rpy="0 0 0"/>
309 <mesh filename="package://open_manipulator_description/meshes/chain_link_grip_r.stl" scale="0.001 0.001 0.001"/>
314 <origin xyz="0 0 0" />
315 <mass value="0.017" />
316 <inertia ixx="1.0e-03" ixy="0.0" ixz="0.0"
317 iyy="1.0e-03" iyz="0.0"
322 <origin xyz="${0.028 + 8.3720668e-03} ${-0.0246 - 9.9696160e-03} -4.2836895e-07" />
323 <mass value="3.2218127e-02" />
324 <inertia ixx="9.5568826e-06" ixy="2.8424644e-06" ixz="-3.2829197e-10"
325 iyy="2.2552871e-05" iyz="-3.1463634e-10"
326 izz="1.7605306e-05" />
330 <!-- Gripper joint sub -->
331 <joint name="gripper_sub" type="prismatic">
332 <parent link="link5"/>
333 <child link="gripper_link_sub"/>
334 <origin xyz="0.0817 -0.021 0" rpy="0 0 0"/>
336 <limit velocity="4.8" effort="1" lower="-0.010" upper="0.019" />
337 <mimic joint="gripper" multiplier="1"/>
340 <!-- Transmission 6 -->
341 <xacro:SimpleTransmission n="6" joint="gripper_sub" />
343 <!-- end effector joint -->
344 <joint name="end_effector_joint" type="fixed">
345 <origin xyz="0.126 0.0 0.0" rpy="0 0 0"/>
346 <parent link="link5"/>
347 <child link="end_effector_link"/>
350 <!-- end effector link -->
351 <link name="end_effector_link">
353 <origin xyz="0 0 0" rpy="0 0 0"/>
355 <box size="0.01 0.01 0.01" />
357 <material name="red"/>
361 <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
362 <mass value="0.001"/>
363 <inertia ixx="1.0e-06" ixy="0.0" ixz="0.0" iyy="1.0e-06" iyz="0.0" izz="1.0e-06" />
367 <sensor_d435 parent="gripper_link">
368 <origin xyz="-0.02 -0.018 0.055" rpy="0 0 0"/>