Adds teleop kit support for dex wrist

stretch_description/urdf/stretch_base_imu.xacro

@@ -8,14 +8,14 @@
         xyz="0.00300349280517617 0.00149777182047641 -0.00193103885249443"
         rpy="0 0 0" />
       <mass
-        value="0.00092" />
+        value="0.000901473198307758" />
       <inertia
-        ixx="3E-08"
-        ixy="0"
-        ixz="0"
-        iyy="5E-08"
-        iyz="0"
-        izz="8E-08" />
+        ixx="2.83324000746358E-08"
+        ixy="-2.10106175504755E-10"
+        ixz="1.86717014516205E-10"
+        iyy="5.35315990744214E-08"
+        iyz="9.31114208976591E-11"
+        izz="8.13221670783358E-08" />
     </inertial>
     <visual>
       <origin

stretch_description/urdf/stretch_description_standard.xacro

@@ -1,15 +1,16 @@
-<?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="stretch">
-
-  <xacro:include filename="stretch_gripper.xacro" />
-  <!--<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_aruco.xacro" />
-  <xacro:include filename="stretch_d435i.xacro" />
-  <xacro:include filename="stretch_laser_range_finder.xacro" />
-  <xacro:include filename="stretch_base_imu.xacro" />
-  <xacro:include filename="stretch_respeaker.xacro" />
-
-</robot>
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="stretch_description">
+
+  <xacro:include filename="stretch_gripper.xacro" />
+  <!-- <xacro:include filename="stretch_dex_wrist.xacro" /> -->
+  <!--<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_aruco.xacro" />
+  <xacro:include filename="stretch_d435i.xacro" />
+  <xacro:include filename="stretch_laser_range_finder.xacro" />
+  <xacro:include filename="stretch_base_imu.xacro" />
+  <xacro:include filename="stretch_respeaker.xacro" />
+
+</robot>

stretch_description/urdf/stretch_gripper.xacro

@@ -4,21 +4,20 @@
   <xacro:property name="scale_finger_length" value="0.9" />
 
   <link
-    name="link_gripper">
     name="link_gripper">
     <inertial>
       <origin
-        xyz="-0.013687 0.015548 -0.035364"
+        xyz="-0.0170229132730066 0.0131410320934285 -0.0371614759484659"
         rpy="0 0 0" />
       <mass
-        value="0.175929" />
+        value="0.101902711393094" />
       <inertia
-        ixx="0.000124"
-        ixy="3.7E-05"
-        ixz="5.7E-05"
-        iyy="0.000217"
-        iyz="1.1E-05"
-        izz="0.000221" />
+        ixx="0.001"
+        ixy="0"
+        ixz="0"
+        iyy="0.001"
+        iyz="0"
+        izz="0.001" />
     </inertial>
     <visual>
       <origin
@@ -65,17 +64,17 @@
     name="link_gripper_finger_left">
     <inertial>
       <origin
-        xyz="0.094071 0.011377 0"
+        xyz="0.0945047483510102 0.0124301080924361 -4.44089209850063E-16"
         rpy="0 0 0" />
       <mass
-        value="0.06" />
+        value="0.0476207785199474" />
       <inertia
-        ixx="1.1E-05"
-        ixy="3.6E-05"
+        ixx="0.001"
+        ixy="0"
         ixz="0"
-        iyy="0.000214"
+        iyy="0.001"
         iyz="0"
-        izz="0.000224" />
+        izz="0.001" />
     </inertial>
     <visual>
       <origin
@@ -121,17 +120,17 @@
     name="link_gripper_fingertip_left">
     <inertial>
       <origin
-        xyz="0 0 0.008126"
+        xyz="-3.16381381648689E-08 -2.91408530639359E-09 0.00812579670381812"
         rpy="0 0 0" />
       <mass
-        value="0.003822" />
+        value="0.00382159917455729" />
       <inertia
-        ixx="0"
+        ixx="0.001"
         ixy="0"
         ixz="0"
-        iyy="0"
+        iyy="0.001"
         iyz="0"
-        izz="1E-06" />
+        izz="0.001" />
     </inertial>
     <visual>
       <origin
@@ -176,17 +175,17 @@
     name="link_gripper_finger_right">
     <inertial>
       <origin
-        xyz="-0.093921 -0.011351 0"
+        xyz="-0.0945047483510099 -0.0124301080924345 0"
         rpy="0 0 0" />
       <mass
-        value="0.06" />
+        value="0.0476207785199481" />
       <inertia
-        ixx="1.1E-05"
-        ixy="3.6E-05"
+        ixx="0.001"
+        ixy="0"
         ixz="0"
-        iyy="0.000213"
+        iyy="0.001"
         iyz="0"
-        izz="0.000223" />
+        izz="0.001" />
     </inertial>
     <visual>
       <origin
@@ -232,17 +231,17 @@
     name="link_gripper_fingertip_right">
     <inertial>
       <origin
-        xyz="0 0 0.008126"
+        xyz="2.59386303963494E-08 6.70949018566347E-09 0.00812579516130402"
         rpy="0 0 0" />
       <mass
-        value="0.003822" />
+        value="0.00382160037319545" />
       <inertia
-        ixx="0"
+        ixx="0.001"
         ixy="0"
         ixz="0"
-        iyy="0"
+        iyy="0.001"
         iyz="0"
-        izz="1E-06" />
+        izz="0.001" />
     </inertial>
     <visual>
       <origin

stretch_description/urdf/stretch_gripper_with_puller.xacro

@@ -7,17 +7,17 @@
     name="link_gripper">
     <inertial>
       <origin
-        xyz="-0.013687 0.015548 -0.035364"
+        xyz="-0.0170229132730066 0.0131410320934285 -0.0371614759484659"
         rpy="0 0 0" />
       <mass
-        value="0.175929" />
+        value="0.101902711393094" />
       <inertia
-        ixx="0.000124"
-        ixy="3.7E-05"
-        ixz="5.7E-05"
-        iyy="0.000217"
-        iyz="1.1E-05"
-        izz="0.000221" />
+        ixx="2.79241618017607E-05"
+        ixy="-1.10819247449605E-05"
+        ixz="-1.50343284036654E-05"
+        iyy="3.67945130513069E-05"
+        iyz="-4.33280448535579E-06"
+        izz="4.14524691955231E-05" />
     </inertial>
     <visual>
       <origin
@@ -118,17 +118,17 @@
     name="link_gripper_finger_left">
     <inertial>
       <origin
-        xyz="0.094071 0.011377 0"
+        xyz="0.0945047483510102 0.0124301080924361 -4.44089209850063E-16"
         rpy="0 0 0" />
       <mass
-        value="0.06" />
+        value="0.0476207785199474" />
       <inertia
-        ixx="1.1E-05"
-        ixy="3.6E-05"
+        ixx="0"
+        ixy="0"
         ixz="0"
-        iyy="0.000214"
+        iyy="0"
         iyz="0"
-        izz="0.000224" />
+        izz="0" />
     </inertial>
     <visual>
       <origin
@@ -174,17 +174,17 @@
     name="link_gripper_fingertip_left">
     <inertial>
       <origin
-        xyz="0 0 0.008126"
+        xyz="-3.16381381648689E-08 -2.91408530639359E-09 0.00812579670381812"
         rpy="0 0 0" />
       <mass
-        value="0.003822" />
+        value="0.00382159917455729" />
       <inertia
         ixx="0"
         ixy="0"
         ixz="0"
         iyy="0"
         iyz="0"
-        izz="1E-06" />
+        izz="0" />
     </inertial>
     <visual>
       <origin
@@ -229,17 +229,17 @@
     name="link_gripper_finger_right">
     <inertial>
       <origin
-        xyz="-0.093921 -0.011351 0"
+        xyz="-0.0945047483510099 -0.0124301080924345 0"
         rpy="0 0 0" />
       <mass
-        value="0.06" />
+        value="0.0476207785199481" />
       <inertia
-        ixx="1.1E-05"
-        ixy="3.6E-05"
+        ixx="0"
+        ixy="0"
         ixz="0"
-        iyy="0.000213"
+        iyy="0"
         iyz="0"
-        izz="0.000223" />
+        izz="0" />
     </inertial>
     <visual>
       <origin
@@ -285,17 +285,17 @@
     name="link_gripper_fingertip_right">
     <inertial>
       <origin
-        xyz="0 0 0.008126"
+        xyz="2.59386303963494E-08 6.70949018566347E-09 0.00812579516130402"
         rpy="0 0 0" />
       <mass
-        value="0.003822" />
+        value="0.00382160037319545" />
       <inertia
         ixx="0"
         ixy="0"
         ixz="0"
         iyy="0"
         iyz="0"
-        izz="1E-06" />
+        izz="0" />
     </inertial>
     <visual>
       <origin

stretch_description/urdf/stretch_laser_range_finder.xacro

@@ -5,17 +5,17 @@
     name="laser">
     <inertial>
       <origin
-        xyz="0.011979 -0.000523 -0.01998"
+        xyz="0 0 -0.000755956127492408"
         rpy="0 0 0" />
       <mass
-        value="0.216007" />
+        value="0.0749979022894495" />
       <inertia
-        ixx="9.5E-05"
-        ixy="2E-06"
-        ixz="-1.9E-05"
-        iyy="0.000184"
-        iyz="-1E-06"
-        izz="0.000216" />
+        ixx="0"
+        ixy="0"
+        ixz="0"
+        iyy="0"
+        iyz="0"
+        izz="0" />
     </inertial>
     <visual>
       <origin

stretch_description/urdf/stretch_main.xacro

@@ -10,17 +10,17 @@
     name="base_link">
     <inertial>
       <origin
-        xyz="-0.087526 -0.001626 0.081009"
+        xyz="-0.11587 0.0019426 0.093621"
         rpy="0 0 0" />
       <mass
-        value="17.384389" />
+        value="1.1912" />
       <inertia
-        ixx="0.160002"
-        ixy="0.006758"
-        ixz="0.004621"
-        iyy="0.138068"
-        iyz="0.002208"
-        izz="0.228992" />
+        ixx="0.0034667"
+        ixy="-5.0568E-06"
+        ixz="0.00042861"
+        iyy="0.0052744"
+        iyz="-5.766E-05"
+        izz="0.0047945" />
     </inertial>
     <visual>
       <origin
@@ -51,17 +51,17 @@
     name="link_right_wheel">
     <inertial>
       <origin
-        xyz="0 0 0.02765"
+        xyz="1.1719E-11 2.0783E-11 0.037544"
         rpy="0 0 0" />
       <mass
-        value="0.20773" />
+        value="0.0042721" />
       <inertia
-        ixx="5.4E-05"
+        ixx="0.001"
         ixy="0"
         ixz="0"
-        iyy="5.4E-05"
+        iyy="0.001"
         iyz="0"
-        izz="5.1E-05" />
+        izz="0.001" />
     </inertial>
     <visual>
       <origin
@@ -107,17 +107,17 @@
     name="link_left_wheel">
     <inertial>
       <origin
-        xyz="0 0 -0.02765"
+        xyz="-2.0783E-11 -1.1719E-11 -0.037544"
         rpy="0 0 0" />
       <mass
-        value="0.20773" />
+        value="0.0042721" />
       <inertia
-        ixx="5.4E-05"
+        ixx="0.001"
         ixy="0"
         ixz="0"
-        iyy="5.4E-05"
+        iyy="0.001"
         iyz="0"
-        izz="5.1E-05" />
+        izz="0.001" />
     </inertial>
     <visual>
       <origin
@@ -214,17 +214,17 @@
     name="link_mast">
     <inertial>
       <origin
-        xyz="0.000337 0.722201 0.002652"
+        xyz="2.0817E-17 0.7075 -2.7756E-17"
         rpy="0 0 0" />
       <mass
-        value="1.764017" />
+        value="1.8285" />
       <inertia
-        ixx="0.291286"
-        ixy="0.004149"
-        ixz="-0.000174"
-        iyy="0.001226"
-        iyz="0.000354"
-        izz="0.291429" />
+        ixx="0.0709854511954588"
+        ixy="-0.00433428742758457"
+        ixz="-0.000186110788697573"
+        iyy="0.000437922053342648"
+        iyz="-0.00288788257713431"
+        izz="0.071104808501661" />
     </inertial>
     <visual>
       <origin
@@ -269,17 +269,17 @@
     name="link_lift">
     <inertial>
       <origin
-        xyz="-0.020369 0.03438 0.02236"
+        xyz="-0.031727 0.038403 0.013361"
         rpy="0 0 0" />
       <mass
-        value="1.987167" />
+        value="0.27218" />
       <inertia
-        ixx="0.006518"
-        ixy="-0.001539"
-        ixz="0.000371"
-        iyy="0.004464"
-        iyz="0.000392"
-        izz="0.008092" />
+        ixx="0.00052571"
+        ixy="0.00014899"
+        ixz="-1.9258E-05"
+        iyy="0.00030679"
+        iyz="-6.2451E-06"
+        izz="0.00037324" />
     </inertial>
     <visual>
       <origin
@@ -318,7 +318,7 @@
       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 -->
+  <!-- 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"/>
@@ -328,17 +328,17 @@
     name="link_arm_l4">
     <inertial>
       <origin
-        xyz="-0.000168 -0.000254 -0.125235"
+        xyz="-1.0146E-06 -1.9719E-05 -0.094738"
         rpy="0 0 0" />
       <mass
-        value="0.235626" />
+        value="0.068095" />
       <inertia
-        ixx="0.001518"
-        ixy="-1E-06"
-        ixz="3E-06"
-        iyy="0.001519"
-        iyz="3E-06"
-        izz="0.000176" />
+        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
@@ -383,17 +383,17 @@
     name="link_arm_l3">
     <inertial>
       <origin
-        xyz="-1E-06 -0.000146 -0.11908"
+        xyz="-5.13853606326845E-07 -1.99844969271112E-05 -0.0971104963726614"
         rpy="0 0 0" />
       <mass
-        value="0.146817" />
+        value="0.0628927381893134" />
       <inertia
-        ixx="0.001081"
+        ixx="0.001"
         ixy="0"
         ixz="0"
-        iyy="0.001081"
-        iyz="-1E-06"
-        izz="7.7E-05" />
+        iyy="0.001"
+        iyz="0"
+        izz="0.001" />
     </inertial>
     <visual>
       <origin
@@ -440,17 +440,17 @@
     name="link_arm_l2">
     <inertial>
       <origin
-        xyz="0 -7.1E-05 -0.115635"
+        xyz="-5.17421949435687E-07 -2.02045301450349E-05 -0.0968815475684904"
         rpy="0 0 0" />
       <mass
-        value="0.130565" />
+        value="0.0571386353275368" />
       <inertia
-        ixx="0.000943"
+        ixx="0.001"
         ixy="0"
         ixz="0"
-        iyy="0.000943"
+        iyy="0.001"
         iyz="0"
-        izz="5.7E-05" />
+        izz="0.001" />
     </inertial>
     <visual>
       <origin
@@ -497,17 +497,17 @@
     name="link_arm_l1">
     <inertial>
       <origin
-        xyz="-1E-06 -0.000121 -0.113457"
+        xyz="-5.257E-07 -2.0482E-05 -0.096543"
         rpy="0 0 0" />
       <mass
-        value="0.113658" />
+        value="0.051382" />
       <inertia
-        ixx="0.0008"
+        ixx="0.001"
         ixy="0"
         ixz="0"
-        iyy="0.0008"
+        iyy="0.001"
         iyz="0"
-        izz="4E-05" />
+        izz="0.001" />
     </inertial>
     <visual>
       <origin
@@ -554,17 +554,17 @@
     name="link_arm_l0">
     <inertial>
       <origin
-        xyz="0.033681 -0.000847 -0.031723"
+        xyz="0.0270582141286185 -0.00189876414654466 -0.0377809018481181"
         rpy="0 0 0" />
       <mass
-        value="0.427734" />
+        value="0.085003260946398" />
       <inertia
-        ixx="0.001689"
-        ixy="-2.8E-05"
-        ixz="0.000483"
-        iyy="0.002107"
-        iyz="-1.1E-05"
-        izz="0.000571" />
+        ixx="0.001"
+        ixy="0"
+        ixz="0"
+        iyy="0.001"
+        iyz="0"
+        izz="0.001" />
     </inertial>
     <visual>
       <origin
@@ -611,17 +611,17 @@
     name="link_wrist_yaw">
     <inertial>
       <origin
-        xyz="0 -3.9E-05 -0.016495"
+        xyz="2.20122392535771E-11 2.9317167880849E-05 -0.018966592644729"
         rpy="0 0 0" />
       <mass
-        value="0.054422" />
+        value="0.0404746907425003" />
       <inertia
-        ixx="9E-06"
+        ixx="0.001"
         ixy="0"
         ixz="0"
-        iyy="9E-06"
+        iyy="0.001"
         iyz="0"
-        izz="7E-06" />
+        izz="0.001" />
     </inertial>
     <visual>
       <origin
@@ -661,9 +661,9 @@
     <axis
       xyz="0 0 -1" />
     <!--
-      stowed to front ~225 deg: 3.15159 x 1.25 = 3.9395
+      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
+      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"/>
@@ -673,17 +673,17 @@
     name="link_head">
     <inertial>
       <origin
-        xyz="0.047462 0.04451 0.016376"
+        xyz="0.0406850995527703 0.0396956343318414 0.0226500246461012"
         rpy="0 0 0" />
       <mass
-        value="0.681871" />
+        value="0.133027236718691" />
       <inertia
-        ixx="0.001023"
-        ixy="0.000641"
-        ixz="6.6E-05"
-        iyy="0.002097"
-        iyz="6.5E-05"
-        izz="0.00281" />
+        ixx="0.001"
+        ixy="0"
+        ixz="0"
+        iyy="0.001"
+        iyz="0"
+        izz="0.001" />
     </inertial>
     <visual>
       <origin
@@ -728,17 +728,17 @@
     name="link_head_pan">
     <inertial>
       <origin
-        xyz="-0.000422 0.013934 -0.0161"
+        xyz="-0.000326562615178591 0.00850012613776489 0.000130487222982367"
         rpy="0 0 0" />
       <mass
-        value="0.062462" />
+        value="0.0273764496535409" />
       <inertia
-        ixx="4.1E-05"
-        ixy="-1E-06"
-        ixz="1E-06"
-        iyy="3.2E-05"
-        iyz="-1.5E-05"
-        izz="2.2E-05" />
+        ixx="0.001"
+        ixy="0"
+        ixz="0"
+        iyy="0.001"
+        iyz="0"
+        izz="0.001" />
     </inertial>
     <visual>
       <origin
@@ -785,17 +785,17 @@
     name="link_head_tilt">
     <inertial>
       <origin
-        xyz="0.003907 -0.02418 0.030154"
+        xyz="0.00704566394917504 -0.0212256210929691 0.0302058990060359"
         rpy="0 0 0" />
       <mass
-        value="0.283548" />
+        value="0.090217113313934" />
       <inertia
-        ixx="0.000316"
-        ixy="-6E-06"
-        ixz="1E-06"
-        iyy="0.000143"
-        iyz="-2E-06"
-        izz="0.00031" />
+        ixx="0.001"
+        ixy="0"
+        ixz="0"
+        iyy="0.001"
+        iyz="0"
+        izz="0.001" />
     </inertial>
     <visual>
       <origin

stretch_description/urdf/stretch_respeaker.xacro

@@ -5,17 +5,17 @@
     name="respeaker_base">
     <inertial>
       <origin
-        xyz="-0.001594 -0.001282 -0.008098"
+        xyz="-0.003809 -0.0023075 -0.012854"
         rpy="0 0 0" />
       <mass
-        value="0.025175" />
+        value="0.015643" />
       <inertia
-        ixx="1E-05"
-        ixy="2E-06"
-        ixz="1E-06"
-        iyy="1.2E-05"
-        iyz="0"
-        izz="1.5E-05" />
+        ixx="1.0075E-06"
+        ixy="-5.4396E-08"
+        ixz="-2.8652E-07"
+        iyy="1.0569E-06"
+        iyz="-1.8463E-07"
+        izz="1.1947E-07" />
     </inertial>
     <visual>
       <origin

stretch_description/urdf/stretch_teleop_kit.xacro

@@ -0,0 +1,215 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="stretch_teleop_kit">
+  <link
+    name="link_gripper_teleop_mount">
+    <inertial>
+      <origin
+        xyz="3.8266E-05 -0.0044089 0.014934"
+        rpy="0 0 0" />
+      <mass
+        value="0.032535" />
+      <inertia
+        ixx="1.0206E-05"
+        ixy="7.894E-09"
+        ixz="-8.5844E-11"
+        iyy="7.437E-06"
+        iyz="-2.9587E-06"
+        izz="2.8384E-06" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_gripper_teleop_mount.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_gripper_teleop_mount.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_gripper_teleop_mount"
+    type="fixed">
+    <origin
+      xyz="-0.10807 0 -0.069789"
+      rpy="-0.5236 -4.3792E-15 1.5708" />
+    <parent
+      link="link_gripper" />
+    <child
+      link="link_gripper_teleop_mount" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+  <link
+    name="link_gripper_teleop_camera">
+    <inertial>
+      <origin
+        xyz="0.0040022 5.9321E-05 -0.00011952"
+        rpy="0 0 0" />
+      <mass
+        value="0.0056257" />
+      <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_gripper_teleop_camera.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.25098 0.25098 0.25098 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_gripper_teleop_camera.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_gripper_teleop_camera"
+    type="fixed">
+    <origin
+      xyz="0 0.0025 0.029"
+      rpy="1.8934E-15 5.5511E-17 1.5708" />
+    <parent
+      link="link_gripper_teleop_mount" />
+    <child
+      link="link_gripper_teleop_camera" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+  <link
+    name="link_head_teleop_mount">
+    <inertial>
+      <origin
+        xyz="0.032675 0.036403 -0.0035184"
+        rpy="0 0 0" />
+      <mass
+        value="0.072645" />
+      <inertia
+        ixx="9.371E-05"
+        ixy="-3.7238E-05"
+        ixz="1.2184E-05"
+        iyy="2.7972E-05"
+        iyz="2.6994E-05"
+        izz="9.9358E-05" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_head_teleop_mount.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_head_teleop_mount.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_head_teleop_mount"
+    type="fixed">
+    <origin
+      xyz="0.079 0.0731 0.052"
+      rpy="0 0 0" />
+    <parent
+      link="link_head" />
+    <child
+      link="link_head_teleop_mount" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+  <link
+    name="link_head_teleop_camera">
+    <inertial>
+      <origin
+        xyz="0.0040022 5.9321E-05 -0.00011952"
+        rpy="0 0 0" />
+      <mass
+        value="0.0056257" />
+      <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_head_teleop_camera.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.25098 0.25098 0.25098 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_head_teleop_camera.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_head_teleop_camera"
+    type="fixed">
+    <origin
+      xyz="0.055997 0.088091 -0.0245"
+      rpy="${-M_PI/2} ${M_PI/2} 0" />
+    <parent
+      link="link_head_teleop_mount" />
+    <child
+      link="link_head_teleop_camera" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+</robot>

stretch_gazebo/urdf/stretch_gazebo_dex_wrist.urdf.xacro

@@ -9,6 +9,7 @@
   <xacro:include filename="$(find stretch_gazebo)/urdf/stretch_laser_range_finder.xacro" />
   <xacro:include filename="$(find stretch_gazebo)/urdf/stretch_respeaker.xacro" />
   <xacro:include filename="$(find stretch_gazebo)/urdf/stretch_dex_wrist.xacro" />
+  <!-- <xacro:include filename="$(find stretch_gazebo)/urdf/stretch_teleop_kit_dex_wrist.xacro" />  -->
 
   <gazebo>
     <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
@@ -69,6 +70,83 @@
 
   <!-- Sensors -->
 
+<!--Teleop Cameras-->
+
+  <gazebo reference="link_gripper_teleop_camera">
+    <sensor type="camera" name="gripper_teleop_camera">
+      <update_rate>30.0</update_rate>
+      <camera name="gripper_teleop_camera">
+        <horizontal_fov>2.26</horizontal_fov>
+        <image>
+          <width>640</width>
+          <height>480</height>
+          <format>R8G8B8</format>
+        </image>
+        <clip>
+          <near>0.02</near>
+          <far>300</far>
+        </clip>
+        <noise>
+          <type>gaussian</type>
+          <mean>0.0</mean>
+          <stddev>0.007</stddev>
+        </noise>
+      </camera>
+      <plugin name="camera_controller" filename="libgazebo_ros_camera.so">
+        <alwaysOn>true</alwaysOn>
+        <updateRate>0.0</updateRate>
+        <cameraName>teleop/gripper_camera</cameraName>
+        <imageTopicName>image_raw</imageTopicName>
+        <cameraInfoTopicName>camera_info</cameraInfoTopicName>
+        <frameName>camera_link</frameName>
+        <hackBaseline>0.07</hackBaseline>
+        <distortionK1>0.0</distortionK1>
+        <distortionK2>0.0</distortionK2>
+        <distortionK3>0.0</distortionK3>
+        <distortionT1>0.0</distortionT1>
+        <distortionT2>0.0</distortionT2>
+      </plugin>
+    </sensor>
+  </gazebo>
+
+  <gazebo reference="link_head_teleop_camera">
+    <sensor type="camera" name="head_teleop_camera">
+      <update_rate>30.0</update_rate>
+      <camera name="head_teleop_camera">
+        <horizontal_fov>2.26</horizontal_fov>
+        <image>
+          <width>640</width>
+          <height>480</height>
+          <format>R8G8B8</format>
+        </image>
+        <clip>
+          <near>0.02</near>
+          <far>300</far>
+        </clip>
+        <noise>
+          <type>gaussian</type>
+          <mean>0.0</mean>
+          <stddev>0.007</stddev>
+        </noise>
+      </camera>
+      <plugin name="camera_controller" filename="libgazebo_ros_camera.so">
+        <alwaysOn>true</alwaysOn>
+        <updateRate>0.0</updateRate>
+        <cameraName>teleop/head_camera</cameraName>
+        <imageTopicName>image_raw</imageTopicName>
+        <cameraInfoTopicName>camera_info</cameraInfoTopicName>
+        <frameName>camera_link</frameName>
+        <hackBaseline>0.07</hackBaseline>
+        <distortionK1>0.0</distortionK1>
+        <distortionK2>0.0</distortionK2>
+        <distortionK3>0.0</distortionK3>
+        <distortionT1>0.0</distortionT1>
+        <distortionT2>0.0</distortionT2>
+      </plugin>
+    </sensor>
+  </gazebo>
+
+
   <!-- Realsense D435i -->
   <gazebo reference="camera_color_frame">
     <sensor name="color" type="camera">

stretch_gazebo/urdf/stretch_gazebo_standard_gripper.urdf.xacro

@@ -9,6 +9,7 @@
   <xacro:include filename="$(find stretch_gazebo)/urdf/stretch_laser_range_finder.xacro" />
   <xacro:include filename="$(find stretch_gazebo)/urdf/stretch_respeaker.xacro" />
   <xacro:include filename="$(find stretch_gazebo)/urdf/stretch_gripper.xacro" />
+  <xacro:include filename="$(find stretch_gazebo)/urdf/stretch_teleop_kit_standard_gripper.xacro" /> 
 
   <gazebo>
     <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">

stretch_gazebo/urdf/stretch_teleop_kit_dex_wrist.xacro

@@ -0,0 +1,215 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="stretch_teleop_kit">
+  <link
+    name="link_gripper_teleop_mount">
+    <inertial>
+      <origin
+        xyz="3.8266E-05 -0.0044089 0.014934"
+        rpy="0 0 0" />
+      <mass
+        value="0.032535" />
+      <inertia
+        ixx="1.0206E-05"
+        ixy="7.894E-09"
+        ixz="-8.5844E-11"
+        iyy="7.437E-06"
+        iyz="-2.9587E-06"
+        izz="2.8384E-06" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_gripper_teleop_mount.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_gripper_teleop_mount.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_gripper_teleop_mount"
+    type="fixed">
+    <origin
+      xyz="-0.10807 0 -0.069789"
+      rpy="-0.5236 -4.3792E-15 1.5708" />
+    <parent
+      link="link_straight_gripper" />
+    <child
+      link="link_gripper_teleop_mount" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+  <link
+    name="link_gripper_teleop_camera">
+    <inertial>
+      <origin
+        xyz="0.0040022 5.9321E-05 -0.00011952"
+        rpy="0 0 0" />
+      <mass
+        value="0.0056257" />
+      <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_gripper_teleop_camera.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.25098 0.25098 0.25098 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_gripper_teleop_camera.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_gripper_teleop_camera"
+    type="fixed">
+    <origin
+      xyz="0 0.0025 0.029"
+      rpy="1.8934E-15 5.5511E-17 1.5708" />
+    <parent
+      link="link_gripper_teleop_mount" />
+    <child
+      link="link_gripper_teleop_camera" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+  <link
+    name="link_head_teleop_mount">
+    <inertial>
+      <origin
+        xyz="0.032675 0.036403 -0.0035184"
+        rpy="0 0 0" />
+      <mass
+        value="0.072645" />
+      <inertia
+        ixx="9.371E-05"
+        ixy="-3.7238E-05"
+        ixz="1.2184E-05"
+        iyy="2.7972E-05"
+        iyz="2.6994E-05"
+        izz="9.9358E-05" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_head_teleop_mount.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_head_teleop_mount.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_head_teleop_mount"
+    type="fixed">
+    <origin
+      xyz="0.079 0.0731 0.052"
+      rpy="0 0 0" />
+    <parent
+      link="link_head" />
+    <child
+      link="link_head_teleop_mount" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+  <link
+    name="link_head_teleop_camera">
+    <inertial>
+      <origin
+        xyz="0.0040022 5.9321E-05 -0.00011952"
+        rpy="0 0 0" />
+      <mass
+        value="0.0056257" />
+      <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_head_teleop_camera.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.25098 0.25098 0.25098 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_head_teleop_camera.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_head_teleop_camera"
+    type="fixed">
+    <origin
+      xyz="0.055997 0.088091 -0.0245"
+      rpy="${-M_PI/2} ${M_PI/2} 0" />
+    <parent
+      link="link_head_teleop_mount" />
+    <child
+      link="link_head_teleop_camera" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+</robot>

stretch_gazebo/urdf/stretch_teleop_kit_standard_gripper.xacro

@@ -0,0 +1,215 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="stretch_teleop_kit">
+  <link
+    name="link_gripper_teleop_mount">
+    <inertial>
+      <origin
+        xyz="3.8266E-05 -0.0044089 0.014934"
+        rpy="0 0 0" />
+      <mass
+        value="0.032535" />
+      <inertia
+        ixx="1.0206E-05"
+        ixy="7.894E-09"
+        ixz="-8.5844E-11"
+        iyy="7.437E-06"
+        iyz="-2.9587E-06"
+        izz="2.8384E-06" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_gripper_teleop_mount.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_gripper_teleop_mount.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_gripper_teleop_mount"
+    type="fixed">
+    <origin
+      xyz="-0.10807 0 -0.069789"
+      rpy="-0.5236 -4.3792E-15 1.5708" />
+    <parent
+      link="link_gripper" />
+    <child
+      link="link_gripper_teleop_mount" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+  <link
+    name="link_gripper_teleop_camera">
+    <inertial>
+      <origin
+        xyz="0.0040022 5.9321E-05 -0.00011952"
+        rpy="0 0 0" />
+      <mass
+        value="0.0056257" />
+      <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_gripper_teleop_camera.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.25098 0.25098 0.25098 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_gripper_teleop_camera.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_gripper_teleop_camera"
+    type="fixed">
+    <origin
+      xyz="0 0.0025 0.029"
+      rpy="1.8934E-15 5.5511E-17 1.5708" />
+    <parent
+      link="link_gripper_teleop_mount" />
+    <child
+      link="link_gripper_teleop_camera" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+  <link
+    name="link_head_teleop_mount">
+    <inertial>
+      <origin
+        xyz="0.032675 0.036403 -0.0035184"
+        rpy="0 0 0" />
+      <mass
+        value="0.072645" />
+      <inertia
+        ixx="9.371E-05"
+        ixy="-3.7238E-05"
+        ixz="1.2184E-05"
+        iyy="2.7972E-05"
+        iyz="2.6994E-05"
+        izz="9.9358E-05" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_head_teleop_mount.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_head_teleop_mount.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_head_teleop_mount"
+    type="fixed">
+    <origin
+      xyz="0.079 0.0731 0.052"
+      rpy="0 0 0" />
+    <parent
+      link="link_head" />
+    <child
+      link="link_head_teleop_mount" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+  <link
+    name="link_head_teleop_camera">
+    <inertial>
+      <origin
+        xyz="0.0040022 5.9321E-05 -0.00011952"
+        rpy="0 0 0" />
+      <mass
+        value="0.0056257" />
+      <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_head_teleop_camera.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.25098 0.25098 0.25098 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_head_teleop_camera.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_head_teleop_camera"
+    type="fixed">
+    <origin
+      xyz="0.055997 0.088091 -0.0245"
+      rpy="${-M_PI/2} ${M_PI/2} 0" />
+    <parent
+      link="link_head_teleop_mount" />
+    <child
+      link="link_head_teleop_camera" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+</robot>