Dev branch for adding mitski URDF

stretch_description/urdf/stretch_description.xacro

@@ -5,7 +5,8 @@
   <!--<xacro:include filename="stretch_gripper_with_puller.xacro" />-->
   <!--<xacro:include filename="stretch_dry_erase_marker.xacro" />-->
     
-  <xacro:include filename="stretch_main.xacro" />
+  <!--<xacro:include filename="stretch_main.xacro" />-->
+  <xacro:include filename="stretch_mitski.xacro" />
   <xacro:include filename="stretch_aruco.xacro" />
   <xacro:include filename="stretch_d435i.xacro" />
   <xacro:include filename="stretch_laser_range_finder.xacro" />

stretch_description/urdf/stretch_mitsksi.xacro

@@ -0,0 +1,791 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="stretch_main">
+  <xacro:property name="M_PI" value="3.1415926535897931" />
+  <xacro:property name="joint_damping" value="21.75"/>
+  <xacro:property name="joint_friction" value="10.48"/>
+  <xacro:property name="joint_spring_stiffness" value="0"/>
+  <xacro:property name="joint_spring_reference" value="0"/>
+
+  <link
+    name="base_link">
+    <inertial>
+      <origin
+        xyz="-0.11587 0.0019426 0.093621"
+        rpy="0 0 0" />
+      <mass
+        value="1.1912" />
+      <inertia
+        ixx="0.0034667"
+        ixy="-5.0568E-06"
+        ixz="0.00042861"
+        iyy="0.0052744"
+        iyz="-5.766E-05"
+        izz="0.0047945" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/base_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.79216 0.81961 0.93333 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/base_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <link
+    name="link_right_wheel">
+    <inertial>
+      <origin
+        xyz="1.1719E-11 2.0783E-11 0.037544"
+        rpy="0 0 0" />
+      <mass
+        value="0.0042721" />
+      <inertia
+        ixx="0"
+        ixy="0"
+        ixz="0"
+        iyy="0"
+        iyz="0"
+        izz="0" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_right_wheel.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.79216 0.81961 0.93333 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_right_wheel.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint
+    name="joint_right_wheel"
+    type="revolute">
+    <origin
+      xyz="0 -0.17035 0.0508"
+      rpy="-1.5708 1.2717E-16 4.8006E-17" />
+    <parent
+      link="base_link" />
+    <child
+      link="link_right_wheel" />
+    <axis
+      xyz="0 0 -1" />
+    <dynamics damping="${joint_damping}" friction="${joint_friction}" spring_reference="${joint_spring_reference}" spring_stiffness="{joint_spring_stiffness}"/>
+  </joint>
+
+  <link
+    name="link_left_wheel">
+    <inertial>
+      <origin
+        xyz="-2.0783E-11 -1.1719E-11 -0.037544"
+        rpy="0 0 0" />
+      <mass
+        value="0.0042721" />
+      <inertia
+        ixx="0"
+        ixy="0"
+        ixz="0"
+        iyy="0"
+        iyz="0"
+        izz="0" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_left_wheel.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.79216 0.81961 0.93333 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_left_wheel.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint
+    name="joint_left_wheel"
+    type="revolute">
+    <origin
+      xyz="0 0.17035 0.0508"
+      rpy="-1.5708 2.6317E-16 -8.2057E-19" />
+    <parent
+      link="base_link" />
+    <child
+      link="link_left_wheel" />
+    <axis
+      xyz="0 0 -1" />
+    <dynamics damping="${joint_damping}" friction="${joint_friction}" spring_reference="${joint_spring_reference}" spring_stiffness="{joint_spring_stiffness}"/>
+  </joint>
+
+  <link
+    name="link_mast">
+    <inertial>
+      <origin
+        xyz="2.0817E-17 0.7075 -2.7756E-17"
+        rpy="0 0 0" />
+      <mass
+        value="1.8285" />
+      <inertia
+        ixx="0"
+        ixy="0"
+        ixz="0"
+        iyy="0"
+        iyz="0"
+        izz="0" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_mast.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.79216 0.81961 0.93333 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_mast.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint
+    name="joint_mast"
+    type="fixed">
+    <origin
+      xyz="-0.067 0.135 0.0284"
+      rpy="1.5708 0 4.8006E-17" />
+    <parent
+      link="base_link" />
+    <child
+      link="link_mast" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+
+  <link
+    name="link_lift">
+    <inertial>
+      <origin
+        xyz="-0.031727 0.038403 0.013361"
+        rpy="0 0 0" />
+      <mass
+        value="0.27218" />
+      <inertia
+        ixx="0.00052571"
+        ixy="0.00014899"
+        ixz="-1.9258E-05"
+        iyy="0.00030679"
+        iyz="-6.2451E-06"
+        izz="0.00037324" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_lift.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.79216 0.81961 0.93333 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_lift.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint
+    name="joint_lift"
+    type="prismatic">
+    <origin
+      xyz="-0.037385 0.1666 0"
+      rpy="1.5708 1.5708 0" />
+    <parent
+      link="link_mast" />
+    <child
+      link="link_lift" />
+    <axis
+      xyz="0 0 1" />
+  <!-- for now: hand copied range_m: from lift: from ~/repos/stretch_fleet/stretch-re1-1001/stretch_re1_factory_params.yaml -->
+  <!--<limit effort="100" lower="0.0" upper="1.095" velocity="1.0"/>-->
+  <!-- copied value did not reach the top of mesh model with GUI sliders and RViz -->
+  <limit effort="100" lower="0.0" upper="1.1" velocity="1.0"/>
+  </joint>
+
+  <link
+    name="link_arm_l4">
+    <inertial>
+      <origin
+        xyz="-1.0146E-06 -1.9719E-05 -0.094738"
+        rpy="0 0 0" />
+      <mass
+        value="0.068095" />
+      <inertia
+        ixx="0.0001256"
+        ixy="-5.6914E-12"
+        ixz="6.0647E-09"
+        iyy="0.0001256"
+        iyz="1.1787E-07"
+        izz="1.1091E-10" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_arm_l4.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.79216 0.81961 0.93333 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_arm_l4.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint
+    name="joint_arm_l4"
+    type="fixed">
+    <origin
+      xyz="-0.2547 0 0"
+      rpy="1.5708 2.4721E-15 -1.5708" />
+    <parent
+      link="link_lift" />
+    <child
+      link="link_arm_l4" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+
+  <link
+    name="link_arm_l3">
+    <inertial>
+      <origin
+        xyz="-5.13853606326845E-07 -1.99844969271112E-05 -0.0971104963726614"
+        rpy="0 0 0" />
+      <mass
+        value="0.0628927381893134" />
+      <inertia
+        ixx="0.000116972687184809"
+        ixy="-2.81979926511074E-12"
+        ixz="2.91222027599461E-09"
+        iyy="0.000116972577591313"
+        iyz="1.13260384471775E-07"
+        izz="1.09738504441915E-10" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_arm_l3.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_arm_l3.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint
+    name="joint_arm_l3"
+    type="prismatic">
+    <origin
+      xyz="0 0 0.013"
+      rpy="7.68831233799385E-30 2.36716479416092E-30 2.29652732251143E-17" />
+    <parent
+      link="link_arm_l4" />
+    <child
+      link="link_arm_l3" />
+    <axis
+      xyz="0 0 1" />
+        <!-- 0.13 = 0.52/4-->
+    <limit effort="100" lower="0.0" upper="0.13" velocity="1.0"/>
+  </joint>
+
+  <link
+    name="link_arm_l2">
+    <inertial>
+      <origin
+        xyz="-5.17421949435687E-07 -2.02045301450349E-05 -0.0968815475684904"
+        rpy="0 0 0" />
+      <mass
+        value="0.0571386353275368" />
+      <inertia
+        ixx="0.000107176369887422"
+        ixy="-2.57283392153217E-12"
+        ixz="2.65737742559071E-09"
+        iyy="0.000107176269488109"
+        iyz="1.0376649507793E-07"
+        izz="1.0053108970507E-10" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_arm_l2.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_arm_l2.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint
+    name="joint_arm_l2"
+    type="prismatic">
+    <origin
+      xyz="0 0 0.013"
+      rpy="0 1.57655765344625E-30 -1.66533453693773E-16" />
+    <parent
+      link="link_arm_l3" />
+    <child
+      link="link_arm_l2" />
+    <axis
+      xyz="0 0 1" />
+    <!-- 0.13 = 0.52/4-->
+    <limit effort="100" lower="0.0" upper="0.13" velocity="1.0"/>
+  </joint>
+
+  <link
+    name="link_arm_l1">
+    <inertial>
+      <origin
+        xyz="-5.257E-07 -2.0482E-05 -0.096543"
+        rpy="0 0 0" />
+      <mass
+        value="0.051382" />
+      <inertia
+        ixx="9.7585E-05"
+        ixy="-2.3365E-12"
+        ixz="2.4191E-09"
+        iyy="9.7585E-05"
+        iyz="9.4252E-08"
+        izz="9.1094E-11" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_arm_l1.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.79216 0.81961 0.93333 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_arm_l1.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint
+    name="joint_arm_l1"
+    type="prismatic">
+    <origin
+      xyz="0 0 0.0129999999999981"
+      rpy="-7.63746778746202E-30 -7.88860905221012E-31 1.11022302462516E-16" />
+    <parent
+      link="link_arm_l2" />
+    <child
+      link="link_arm_l1" />
+    <axis
+      xyz="0 0 1" />
+    <!-- 0.13 = 0.52/4-->
+    <limit effort="100" lower="0.0" upper="0.13" velocity="1.0"/>
+  </joint>
+
+  <link
+    name="link_arm_l0">
+    <inertial>
+      <origin
+        xyz="0.0270582141286185 -0.00189876414654466 -0.0377809018481181"
+        rpy="0 0 0" />
+      <mass
+        value="0.085003260946398" />
+      <inertia
+        ixx="0.000207559662463324"
+        ixy="7.32310021353946E-06"
+        ixz="-8.77838232968467E-05"
+        iyy="0.000270258467719226"
+        iyz="6.1781873091689E-06"
+        izz="8.15211614844496E-05" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_arm_l0.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_arm_l0.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint
+    name="joint_arm_l0"
+    type="prismatic">
+    <origin
+      xyz="0 0 -0.0137499999991968"
+      rpy="7.63746778746202E-30 -3.80121128864402E-15 2.62707547767438E-15" />
+    <parent
+      link="link_arm_l1" />
+    <child
+      link="link_arm_l0" />
+    <axis
+      xyz="0 0 1" />
+    <!-- 0.13 = 0.52/4-->
+    <limit effort="100" lower="0.0" upper="0.13" velocity="1.0"/>
+  </joint>
+
+
+  <link
+    name="link_wrist_yaw">
+    <inertial>
+      <origin
+        xyz="2.20122392535771E-11 2.9317167880849E-05 -0.018966592644729"
+        rpy="0 0 0" />
+      <mass
+        value="0.0404746907425003" />
+      <inertia
+        ixx="2.25219236648594E-09"
+        ixy="-4.79073130840543E-19"
+        ixz="2.84594420739213E-17"
+        iyy="2.25155434794669E-09"
+        iyz="3.79016281460022E-11"
+        izz="6.3801853924987E-13" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_wrist_yaw.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.898039215686275 0.917647058823529 0.929411764705882 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_wrist_yaw.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint
+    name="joint_wrist_yaw"
+    type="revolute">
+    <origin
+      xyz="0.0830000000000654 -0.0307500000000129 0"
+      rpy="-1.5707963267949 -6.34174103868745E-15 3.14159265358978" />
+    <parent
+      link="link_arm_l0" />
+    <child
+      link="link_wrist_yaw" />
+    <axis
+      xyz="0 0 -1" />
+    <!--
+      stowed to front ~225 deg: 3.15159 x 1.25 = 3.9395
+      using 4.0
+      stowed to back ~100 deg:  100 / 180 x 3.14159 = 1.7453
+      using -1.75
+    -->
+    <limit effort="100" lower="-1.75" upper="4.0" velocity="1.0"/>
+  </joint>
+
+  <link
+    name="link_head">
+    <inertial>
+      <origin
+        xyz="0.0406850995527703 0.0396956343318414 0.0226500246461012"
+        rpy="0 0 0" />
+      <mass
+        value="0.133027236718691" />
+      <inertia
+        ixx="5.12726410844311E-05"
+        ixy="-1.28409095844797E-05"
+        ixz="-8.63651073052237E-06"
+        iyy="5.06358489467333E-05"
+        iyz="-4.02909801959325E-06"
+        izz="2.71795471864185E-05" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_head.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_head.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint
+    name="joint_head"
+    type="fixed">
+    <origin
+      xyz="0 1.33 0"
+      rpy="1.5707963267949 -1.5707963267949 0" />
+    <parent
+      link="link_mast" />
+    <child
+      link="link_head" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+
+  <link
+    name="link_head_pan">
+    <inertial>
+      <origin
+        xyz="-0.000326562615178591 0.00850012613776489 0.000130487222982367"
+        rpy="0 0 0" />
+      <mass
+        value="0.0273764496535409" />
+      <inertia
+        ixx="1.78931787015902E-05"
+        ixy="1.09301758992101E-07"
+        ixz="-2.39230585375427E-07"
+        iyy="1.4743405056612E-05"
+        iyz="6.73888414083794E-06"
+        izz="3.15753948501286E-06" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_head_pan.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_head_pan.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint
+    name="joint_head_pan"
+    type="revolute">
+    <origin
+      xyz="0.135 0.0731000000000001 -0.00319621125547975"
+      rpy="0 0 1.5707963267949" />
+    <parent
+      link="link_head" />
+    <child
+      link="link_head_pan" />
+    <axis
+      xyz="0 0 1" />
+    <!-- unconstrained range for now -->
+    <limit effort="100" lower="-3.9" upper="1.5" velocity="1.0"/>
+  </joint>
+
+  <link
+    name="link_head_tilt">
+    <inertial>
+      <origin
+        xyz="0.00704566394917504 -0.0212256210929691 0.0302058990060359"
+        rpy="0 0 0" />
+      <mass
+        value="0.090217113313934" />
+      <inertia
+        ixx="3.00855252262414E-05"
+        ixy="4.02701944747726E-06"
+        ixz="-9.82945288189658E-07"
+        iyy="4.33262186405403E-05"
+        iyz="2.27705774750325E-06"
+        izz="3.69710351876137E-05" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_head_tilt.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_head_tilt.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint
+    name="joint_head_tilt"
+    type="revolute">
+    <origin
+      xyz="-0.00130000001785262 0.0277624999926072 -0.0533107920897029"
+      rpy="1.5707963267949 3.36459255518345E-15 -8.42914893687103E-17" />
+    <parent
+      link="link_head_pan" />
+    <child
+      link="link_head_tilt" />
+    <axis
+      xyz="0 0 1" />
+    <!-- unconstrained range for now -->
+    <limit effort="100" lower="-1.53" upper="0.79" velocity="1.0"/>
+  </joint>
+
+</robot>