]> defiant.homedns.org Git - wt_open_manipulator.git/blobdiff - urdf/open_manipulator.urdf.xacro
replace id 12 with xm540-w270
[wt_open_manipulator.git] / urdf / open_manipulator.urdf.xacro
index f1a5529d28f818c3077a20a4ff85f60c99dd09dc..bbc923d1da4647757a1e925a8d0eeb59d8511473 100644 (file)
@@ -6,6 +6,7 @@
        <xacro:include filename="$(find open_manipulator_description)/urdf/open_manipulator.gazebo.xacro" />
        <!-- Import Rviz colors -->
        <xacro:include filename="$(find open_manipulator_description)/urdf/materials.xacro" />
+       <xacro:include filename="$(find realsense2_description)/urdf/_d435.urdf.xacro"/>
 
        <!-- Transmission macro -->
        <xacro:macro name="SimpleTransmission" params="joint n">
                </transmission>
        </xacro:macro>
 
+       <link name="open_manipulator_base">
+       </link>
+
+       <joint name="world_fixed" type="fixed">
+               <origin xyz="0 0 0" rpy="0 0 0"/>
+               <parent link="open_manipulator_base"/>
+               <child link="link1"/>
+       </joint>
+
        <!-- Link 1 -->
        <link name="link1">
                <visual>
@@ -59,9 +69,9 @@
   <joint name="joint1" type="revolute">
          <parent link="link1"/>
          <child link="link2"/>
-         <origin xyz="0.012 0.0 0.017" rpy="0 0 0"/>
+         <origin xyz="0.012 0.0 0.036" rpy="0 0 0"/> <!-- Height servo+horn -->
          <axis xyz="0 0 1"/>
-         <limit velocity="4.8" effort="1" lower="${-pi}" upper="${pi}" />
+         <limit velocity="3.1" effort="1" lower="${-pi}" upper="${pi}" />
   </joint>
 
   <!-- Transmission 1 -->
   <!--  Link 2 -->
   <link name="link2">
          <visual>
-                 <origin xyz="0 0 0.019" rpy="0 0 0"/>
                  <geometry>
-                         <mesh filename="package://open_manipulator_description/meshes/chain_link2.stl" scale="0.001 0.001 0.001"/>
+                         <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 -->
                  </geometry>
                  <material name="grey"/>
          </visual>
 
          <collision>
-                 <origin xyz="0 0 0.019" rpy="0 0 0"/>
                  <geometry>
-                         <mesh filename="package://open_manipulator_description/meshes/chain_link2.stl" scale="0.001 0.001 0.001"/>
+                         <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 -->
                  </geometry>
          </collision>
 
   <joint name="joint2" type="revolute">
          <parent link="link2"/>
          <child link="link3"/>
-         <origin xyz="0.0 0.0 0.0595" rpy="0 0 0"/>
+         <origin xyz="0.0 0.0 0.0515" rpy="0 0 0"/> <!-- fr13-s102k to middle of servo horn -->
          <axis xyz="0 1 0"/>
          <limit velocity="4.8" effort="1" lower="${-pi*0.64}" upper="${pi*0.64}" />
   </joint>
          <visual>
                  <origin xyz="0 0 0" rpy="0 0 0"/>
                  <geometry>
-                         <mesh filename="package://open_manipulator_description/meshes/chain_link3.stl" scale="0.001 0.001 0.001"/>
+                         <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 -->
                  </geometry>
                  <material name="grey"/>
          </visual>
          <collision>
                  <origin xyz="0 0 0" rpy="0 0 0"/>
                  <geometry>
-                         <mesh filename="package://open_manipulator_description/meshes/chain_link3.stl" scale="0.001 0.001 0.001"/>
+                         <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 -->
                  </geometry>
          </collision>
 
   <joint name="joint3" type="revolute">
          <parent link="link3"/>
          <child link="link4"/>
-         <origin xyz="0.024 0 0.128" rpy="0 0 0"/>
+         <origin xyz="0.024 0 ${0.128+0.004}" rpy="0 0 0"/> <!-- 4mm more for FR13-H101K-->
          <axis xyz="0 1 0"/>
          <limit velocity="4.8" effort="1" lower="${-pi*0.64}" upper="${pi*0.48}" />
   </joint>
          </inertial>
   </link>
 
+       <sensor_d435 parent="gripper_link">
+               <origin xyz="-0.02 -0.018 0.055" rpy="0 0 0"/>
+       </sensor_d435>
 </robot>