Adding new Testig Data

stretch_camera_testrig/config/testrig_marker_info.yaml

@@ -3,7 +3,7 @@ testrig_aruco_marker_info:
   - - 1.0
     - 0.0
     - 0.0
-    - -0.195
+    - 0.009
   - - 0.0
     - -1.0
     - 0.0

stretch_camera_testrig/data/Testrig_Data_Analyse_rough.ipynb

@@ -227,12 +227,12 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 45,
+   "execution_count": 6,
    "metadata": {},
    "outputs": [],
    "source": [
-    "setup_1_pose = np.array([0.009,-0.06418,0.97625-(4/1000)])\n",
-    "setup_2_pose = np.array([-0.195,-0.06418,0.97625-(4/1000)])"
+    "setup_1_pose = np.array([0.009,-0.06418,0.97625-(4.2/1000)])\n",
+    "setup_2_pose = np.array([-0.195,-0.06418,0.97625-(4.2/1000)])"
    ]
   },
   {
@@ -244,22 +244,26 @@
   },
   {
    "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "setup_1_data = get_dict('testrig_collected_data_202205031614.yaml')\n",
-    "get_avg_pos_vals(setup_1_data)[data_keys[0]]"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
+   "execution_count": 17,
    "metadata": {},
-   "outputs": [],
+   "outputs": [
+    {
+     "name": "stdout",
+     "output_type": "stream",
+     "text": [
+      "('Average Pose=', array([ 0.01414346, -0.0522339 ,  0.96775044]))\n",
+      "('dist_true=', 0.9742080244485773)\n",
+      "('dist_detected=', 0.9692622666331148)\n",
+      "('error (mm) =', 4.945757815462581)\n"
+     ]
+    }
+   ],
    "source": [
+    "setup_data = get_dict('testrig_collected_data_202205031614.yaml')\n",
+    "avg_pos = get_avg_pos_vals(setup_data)[data_keys[0]]\n",
+    "print('Average Pose=',avg_pos)\n",
     "p1 = setup_1_pose\n",
-    "p2 = get_avg_pos_vals(setup_1_data)[data_keys[0]]\n",
+    "p2 = avg_pos\n",
     "dist1 = np.linalg.norm(p1)\n",
     "print('dist_true=',dist1)\n",
     "dist2 = np.linalg.norm(p2)\n",
@@ -277,22 +281,26 @@
   },
   {
    "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "setup_2_data = get_dict('testrig_collected_data_202205031621.yaml')\n",
-    "get_avg_pos_vals(setup_2_data)[data_keys[0]]"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
+   "execution_count": 18,
    "metadata": {},
-   "outputs": [],
+   "outputs": [
+    {
+     "name": "stdout",
+     "output_type": "stream",
+     "text": [
+      "('Average Pose=', array([-0.18590926, -0.05297513,  0.96615088]))\n",
+      "('dist_true=', 0.9934914568832487)\n",
+      "('dist_detected=', 0.9853000282469121)\n",
+      "('error (mm) =', 8.191428636336596)\n"
+     ]
+    }
+   ],
    "source": [
+    "setup_data = get_dict('testrig_collected_data_202205031621.yaml')\n",
+    "avg_pos = get_avg_pos_vals(setup_data)[data_keys[0]]\n",
+    "print('Average Pose=',avg_pos)\n",
     "p1 = setup_2_pose\n",
-    "p2 = get_avg_pos_vals(setup_2_data)[data_keys[0]]\n",
+    "p2 = avg_pos\n",
     "dist1 = np.linalg.norm(p1)\n",
     "print('dist_true=',dist1)\n",
     "dist2 = np.linalg.norm(p2)\n",
@@ -310,43 +318,26 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 43,
-   "metadata": {},
-   "outputs": [
-    {
-     "data": {
-      "text/plain": [
-       "array([ 0.01413084, -0.05231626,  0.96759951])"
-      ]
-     },
-     "execution_count": 43,
-     "metadata": {},
-     "output_type": "execute_result"
-    }
-   ],
-   "source": [
-    "setup_1_data = get_dict('testrig_collected_data_202205031705.yaml')\n",
-    "get_avg_pos_vals(setup_1_data)[data_keys[0]]"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": 44,
+   "execution_count": 19,
    "metadata": {},
    "outputs": [
     {
      "name": "stdout",
      "output_type": "stream",
      "text": [
-      "('dist_true=', 0.9783987606799182)\n",
+      "('Average Pose=', array([ 0.01413084, -0.05231626,  0.96759951]))\n",
+      "('dist_true=', 0.9742080244485773)\n",
       "('dist_detected=', 0.9691158304372177)\n",
-      "('error (mm) =', 9.282930242700438)\n"
+      "('error (mm) =', 5.09219401135963)\n"
      ]
     }
    ],
    "source": [
+    "setup_data = get_dict('testrig_collected_data_202205031705.yaml')\n",
+    "avg_pos = get_avg_pos_vals(setup_data)[data_keys[0]]\n",
+    "print('Average Pose=',avg_pos)\n",
     "p1 = setup_1_pose\n",
-    "p2 = get_avg_pos_vals(setup_1_data)[data_keys[0]]\n",
+    "p2 = avg_pos\n",
     "dist1 = np.linalg.norm(p1)\n",
     "print('dist_true=',dist1)\n",
     "dist2 = np.linalg.norm(p2)\n",
@@ -364,43 +355,26 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 36,
-   "metadata": {},
-   "outputs": [
-    {
-     "data": {
-      "text/plain": [
-       "array([ 0.01413084, -0.05231626,  0.96759951])"
-      ]
-     },
-     "execution_count": 36,
-     "metadata": {},
-     "output_type": "execute_result"
-    }
-   ],
-   "source": [
-    "setup_2_data = get_dict('testrig_collected_data_202205031648.yaml')\n",
-    "get_avg_pos_vals(setup_1_data)[data_keys[0]]"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": 37,
+   "execution_count": 20,
    "metadata": {},
    "outputs": [
     {
      "name": "stdout",
      "output_type": "stream",
      "text": [
-      "('dist_true=', 0.997601190306026)\n",
+      "('Average Pose=', array([-0.18595072, -0.05305209,  0.96639527]))\n",
+      "('dist_true=', 0.9934914568832487)\n",
       "('dist_detected=', 0.985551625090867)\n",
-      "('error (mm) =', 12.049565215158985)\n"
+      "('error (mm) =', 7.939831792381669)\n"
      ]
     }
    ],
    "source": [
+    "setup_data = get_dict('testrig_collected_data_202205031648.yaml')\n",
+    "avg_pos = get_avg_pos_vals(setup_data)[data_keys[0]]\n",
+    "print('Average Pose=',avg_pos)\n",
     "p1 = setup_2_pose\n",
-    "p2 = get_avg_pos_vals(setup_2_data)[data_keys[0]]\n",
+    "p2 = avg_pos\n",
     "dist1 = np.linalg.norm(p1)\n",
     "print('dist_true=',dist1)\n",
     "dist2 = np.linalg.norm(p2)\n",

stretch_core/config/HighAccuracyPreset.json

@@ -0,0 +1,48 @@
+{
+    "ignoreSAD": "0",
+    "param-censusenablereg-udiameter": "9",
+    "param-censusenablereg-vdiameter": "9",
+    "param-censususize": "9",
+    "param-censusvsize": "9",
+    "param-disableraucolor": "0",
+    "param-disablesadcolor": "0",
+    "param-disablesadnormalize": "0",
+    "param-disablesloleftcolor": "0",
+    "param-disableslorightcolor": "1",
+    "param-lambdaad": "751",
+    "param-lambdacensus": "6",
+    "param-leftrightthreshold": "10",
+    "param-maxscorethreshb": "2893",
+    "param-medianthreshold": "796",
+    "param-minscorethresha": "4",
+    "param-neighborthresh": "108",
+    "param-raumine": "6",
+    "param-rauminn": "3",
+    "param-rauminnssum": "7",
+    "param-raumins": "2",
+    "param-rauminw": "2",
+    "param-rauminwesum": "12",
+    "param-regioncolorthresholdb": "0.785714",
+    "param-regioncolorthresholdg": "0.565558",
+    "param-regioncolorthresholdr": "0.985323",
+    "param-regionshrinku": "3",
+    "param-regionshrinkv": "0",
+    "param-robbinsmonrodecrement": "25",
+    "param-robbinsmonroincrement": "2",
+    "param-rsmdiffthreshold": "1.65625",
+    "param-rsmrauslodiffthreshold": "0.71875",
+    "param-rsmremovethreshold": "0.809524",
+    "param-scanlineedgetaub": "13",
+    "param-scanlineedgetaug": "15",
+    "param-scanlineedgetaur": "30",
+    "param-scanlinep1": "155",
+    "param-scanlinep1onediscon": "160",
+    "param-scanlinep1twodiscon": "59",
+    "param-scanlinep2": "190",
+    "param-scanlinep2onediscon": "507",
+    "param-scanlinep2twodiscon": "493",
+    "param-secondpeakdelta": "647",
+    "param-texturecountthresh": "0",
+    "param-texturedifferencethresh": "1722",
+    "param-usersm": "1"
+}

stretch_core/launch/d435i_basic.launch

@@ -1,50 +1,48 @@
 <launch>
 
+  <arg name="initial_preset" default="$(find stretch_core)/config/HighAccuracyPreset.json" doc="depth image visual preset (pass in filepath to json defined preset)" />
+
   <!-- REDUCE RATE AND USE NUC TIME -->
   <node name="d435i_accel_correction" pkg="stretch_core" type="d435i_accel_correction" output="screen" />
 
   <!-- REALSENSE D435i -->
   <include file="$(find realsense2_camera)/launch/rs_camera.launch">
-    <!-- "The D435i depth camera generates and transmits the gyro and
+    <arg name="enable_confidence" value="false"/>
+    <arg name="enable_infra1" value="false"/>
+    <arg name="enable_infra2" value="false"/>
+    <arg name="enable_infra" value="false"/>
+    <arg name="enable_color" value="true"/>
+    <arg name="enable_depth" value="true"/>
+    <arg name="enable_accel" value="true"/>
+    <arg name="enable_gyro" value="true"/>
+
+    <!-- The D435i depth camera generates and transmits the gyro and
          accelerometer samples independently, as the inertial sensors
          exhibit different FPS rates (200/400Hz for gyro, 63/250Hz for
-         accelerometer)."
-         https://realsense.intel.com/how-to-getting-imu-data-from-d435i-and-t265/
-         https://github.com/intel-ros/realsense
-         -->
+         accelerometer). -->
     <arg name="accel_fps" value="63"/>
     <arg name="gyro_fps" value="200"/>
     <arg name="depth_fps" value="15"/>
-    <arg name="enable_infra1" value="false"/>
-    <arg name="enable_infra2" value="false"/>
-    <arg name="enable_accel" value="true"/>
+    <arg name="color_fps" value="15"/>
 
     <arg name="depth_width" value="$(arg depth_width)"/>
     <arg name="depth_height" value="$(arg depth_height)"/>
     <arg name="color_width" value="$(arg color_width)"/>
     <arg name="color_height" value="$(arg color_height)"/>
-  
-    <arg name="color_fps" value="15"/>
 
     <!-- publish depth streams aligned to other streams -->
     <arg name="align_depth" value="true"/>
 
     <!-- publish an RGBD point cloud -->
     <arg name="filters" value="pointcloud"/>
-    
-    <!-- "tf_prefix: By default all frame's ids have the same prefix -
-         camera_. This allows changing it per camera."
-         https://github.com/intel-ros/realsense -->
+    <arg name="enable_pointcloud" value="true"/>
 
-    <!-- "enable_sync: gathers closest frames of different sensors,
+    <!-- enable_sync: gathers closest frames of different sensors,
          infra red, color and depth, to be sent with the same
          timetag. This happens automatically when such filters as
-         pointcloud are enabled."
-         https://github.com/intel-ros/realsense -->
+         pointcloud are enabled. -->
     <arg name="enable_sync" value="true"/>
-    
-    <!-- "You can have a full depth to pointcloud, coloring the
-         regions beyond the texture with zeros..." -->
+
     <!-- Set to true in order to make use of the full field of view of
          the depth image instead of being restricted to the field of
          view associated with the narrower RGB camera. Note that
@@ -52,13 +50,16 @@
          colors set to 0,0,0. -->
     <arg name="allow_no_texture_points" value="true"/>
 
-    <!-- "initial_reset: On occasions the device was not closed
+    <!-- initial_reset: On occasions the device was not closed
          properly and due to firmware issues needs to reset. If set to
-         true, the device will reset prior to usage.";
-         https://github.com/intel-ros/realsense -->
-    <!--<arg name="initial_reset" value="true"/>-->
-    
+         true, the device will reset prior to usage. -->;
+    <arg name="initial_reset" value="true"/>
+
+    <!-- publishes a single imu message with gyro/accel filled out -->
     <!--<arg name="unite_imu_method" value="copy"/> linear_interpolation-->
+
+     <!-- configure the depth image to the high accuracy visual preset -->
+    <arg name="json_file_path" value="$(arg initial_preset)" />
   </include>
 
 </launch>

stretch_core/launch/d435i_basic.launch.backup

@@ -0,0 +1,64 @@
+<launch>
+
+  <!-- REDUCE RATE AND USE NUC TIME -->
+  <node name="d435i_accel_correction" pkg="stretch_core" type="d435i_accel_correction" output="screen" />
+
+  <!-- REALSENSE D435i -->
+  <include file="$(find realsense2_camera)/launch/rs_camera.launch">
+    <!-- "The D435i depth camera generates and transmits the gyro and
+         accelerometer samples independently, as the inertial sensors
+         exhibit different FPS rates (200/400Hz for gyro, 63/250Hz for
+         accelerometer)."
+         https://realsense.intel.com/how-to-getting-imu-data-from-d435i-and-t265/
+         https://github.com/intel-ros/realsense
+         -->
+    <arg name="accel_fps" value="63"/>
+    <arg name="gyro_fps" value="200"/>
+    <arg name="depth_fps" value="15"/>
+    <arg name="enable_infra1" value="false"/>
+    <arg name="enable_infra2" value="false"/>
+    <arg name="enable_accel" value="true"/>
+
+    <arg name="depth_width" value="$(arg depth_width)"/>
+    <arg name="depth_height" value="$(arg depth_height)"/>
+    <arg name="color_width" value="$(arg color_width)"/>
+    <arg name="color_height" value="$(arg color_height)"/>
+  
+    <arg name="color_fps" value="15"/>
+
+    <!-- publish depth streams aligned to other streams -->
+    <arg name="align_depth" value="true"/>
+
+    <!-- publish an RGBD point cloud -->
+    <arg name="filters" value="pointcloud"/>
+    
+    <!-- "tf_prefix: By default all frame's ids have the same prefix -
+         camera_. This allows changing it per camera."
+         https://github.com/intel-ros/realsense -->
+
+    <!-- "enable_sync: gathers closest frames of different sensors,
+         infra red, color and depth, to be sent with the same
+         timetag. This happens automatically when such filters as
+         pointcloud are enabled."
+         https://github.com/intel-ros/realsense -->
+    <arg name="enable_sync" value="true"/>
+    
+    <!-- "You can have a full depth to pointcloud, coloring the
+         regions beyond the texture with zeros..." -->
+    <!-- Set to true in order to make use of the full field of view of
+         the depth image instead of being restricted to the field of
+         view associated with the narrower RGB camera. Note that
+         points out of the RGB camera's field of view will have their
+         colors set to 0,0,0. -->
+    <arg name="allow_no_texture_points" value="true"/>
+
+    <!-- "initial_reset: On occasions the device was not closed
+         properly and due to firmware issues needs to reset. If set to
+         true, the device will reset prior to usage."
+         https://github.com/intel-ros/realsense -->
+    <!--<arg name="initial_reset" value="true"/>-->
+    
+    <!--<arg name="unite_imu_method" value="copy"/> linear_interpolation-->
+  </include>
+
+</launch>

stretch_core/launch/d435i_high_resolution.launch

@@ -1,16 +1,18 @@
 <launch>
-  
+
+  <arg name="initial_preset" default="$(find stretch_core)/config/HighAccuracyPreset.json" doc="depth image visual preset (pass in filepath to json defined preset)" />
+
   <!-- REALSENSE D435i -->
   <include file="$(find stretch_core)/launch/d435i_basic.launch" pass_all_args="true">
-    <!--
-	HIGHEST RESOLUTION, but also has the highest minimum depth
-	(280mm Min-Z) below which objects generate bad noise, such as
-	when the arm and gripper are raised close to the camera.
-    -->
+    <!-- HIGHEST RESOLUTION, but also has the highest minimum depth
+         (280mm Min-Z) below which objects generate bad noise, such as
+         when the arm and gripper are raised close to the camera. -->
     <arg name="depth_width" value="1280"/>
     <arg name="depth_height" value="720"/>
     <arg name="color_width" value="1280"/>
     <arg name="color_height" value="720"/>
+
+    <arg name="initial_preset" value="$(arg initial_preset)" />
   </include>
-  
+
 </launch>

stretch_core/launch/d435i_high_resolution.launch.backup

@@ -0,0 +1,16 @@
+<launch>
+  
+  <!-- REALSENSE D435i -->
+  <include file="$(find stretch_core)/launch/d435i_basic.launch" pass_all_args="true">
+    <!--
+	HIGHEST RESOLUTION, but also has the highest minimum depth
+	(280mm Min-Z) below which objects generate bad noise, such as
+	when the arm and gripper are raised close to the camera.
+    -->
+    <arg name="depth_width" value="1280"/>
+    <arg name="depth_height" value="720"/>
+    <arg name="color_width" value="1280"/>
+    <arg name="color_height" value="720"/>
+  </include>
+  
+</launch>

stretch_description/urdf/stretch_dex_wrist.xacro

@@ -0,0 +1,434 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="stretch_dex_wrist">
+
+  <xacro:property name="scale_finger_length" value="0.9" />
+
+  <link
+    name="link_wrist_yaw_bottom">
+    <inertial>
+      <origin
+        xyz="-0.012839101377342 -0.0382787718640742 -0.0228400332263617"
+        rpy="0 0 0" />
+      <mass
+        value="0.0988906816399982" />
+      <inertia
+        ixx="2.60067866573596E-05"
+        ixy="-6.73176267521354E-06"
+        ixz="-2.43476436723672E-06"
+        iyy="5.99482946819923E-06"
+        iyz="-3.39642410492401E-06"
+        izz="2.56907114334732E-05" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_wrist_yaw_bottom.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_wrist_yaw_bottom.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_wrist_yaw_bottom"
+    type="fixed">
+    <origin
+      xyz="0 0 0"
+      rpy="-3.14159265358979 1.13367999021379E-14 1.57079632679489" />
+    <parent
+      link="link_wrist_yaw" />
+    <child
+      link="link_wrist_yaw_bottom" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+  <link
+    name="link_wrist_pitch">
+    <inertial>
+      <origin
+        xyz="-0.00310609611067142 -0.0150777141465843 0.0204734587925901"
+        rpy="0 0 0" />
+      <mass
+        value="0.0701267146295583" />
+      <inertia
+        ixx="2.55965614980905E-06"
+        ixy="-1.47551515167608E-06"
+        ixz="-6.31436085977252E-08"
+        iyy="3.43968637386282E-06"
+        iyz="-4.17813567208843E-07"
+        izz="4.53568668211393E-06" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_wrist_pitch.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_wrist_pitch.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_wrist_pitch"
+    type="revolute">
+    <origin
+      xyz="0 -0.0195500000000002 -0.0247499999999984"
+      rpy="1.5707963267949 -8.12895570882604E-15 -3.14159265358979" />
+    <parent
+      link="link_wrist_yaw_bottom" />
+    <child
+      link="link_wrist_pitch" />
+    <axis
+      xyz="0 0 -1" />
+    <limit effort="100" lower="-1.57" upper="0.56" velocity="1.0"/>
+  </joint>
+  <link
+    name="link_wrist_roll">
+    <inertial>
+      <origin
+        xyz="9.63118473862323E-15 -6.38378239159465E-15 0.00768048802649798"
+        rpy="0 0 0" />
+      <mass
+        value="0.00585666394358811" />
+      <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_wrist_roll.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_wrist_roll.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_wrist_roll"
+    type="revolute">
+    <origin
+      xyz="-0.0188587444076125 -0.0239999999998942 0.01955"
+      rpy="3.14159265358979 1.5707963267949 0" />
+    <parent
+      link="link_wrist_pitch" />
+    <child
+      link="link_wrist_roll" />
+    <axis
+      xyz="0 0 1" />
+    <limit effort="100" lower="-3.14" upper="3.14" velocity="1.0"/>
+  </joint>
+  <link
+    name="link_straight_gripper">
+    <inertial>
+      <origin
+        xyz="0.00150764845432383 -0.00711581846201287 0.0399737901417758"
+        rpy="0 0 0" />
+      <mass
+        value="0.0496384234458284" />
+      <inertia
+        ixx="5.61461154156397E-06"
+        ixy="8.29518962984231E-07"
+        ixz="-2.41382921888194E-06"
+        iyy="1.11504692003467E-05"
+        iyz="9.76174898123369E-07"
+        izz="6.63803357903882E-06" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_straight_gripper.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_straight_gripper.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_straight_gripper"
+    type="fixed">
+    <origin
+      xyz="0 0 0.0155"
+      rpy="3.54987407349455E-30 3.24021254484265E-20 -3.14159265358979" />
+    <parent
+      link="link_wrist_roll" />
+    <child
+      link="link_straight_gripper" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+  <link
+    name="link_gripper_finger_right">
+    <inertial>
+      <origin
+        xyz="-0.094981 -0.0080152 -2.2204E-16"
+        rpy="0 0 0" />
+      <mass
+        value="0.047621" />
+      <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_finger_right.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_finger_right.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_gripper_finger_right"
+    type="revolute">
+    <origin
+      xyz="-0.018599 0.003 0.033689"
+      rpy="1.5708 1.5708 0" />
+    <parent
+      link="link_straight_gripper" />
+    <child
+      link="link_gripper_finger_right" />
+    <axis
+      xyz="0 0 1" />
+    <limit effort="100" lower="-0.6" upper="0.6" velocity="1.0"/>
+  </joint>
+  <link
+    name="link_gripper_fingertip_right">
+    <inertial>
+      <origin
+        xyz="2.83785970833783E-08 6.75131661687089E-09 0.00812578923434215"
+        rpy="0 0 0" />
+      <mass
+        value="0.00382160881468841" />
+      <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_fingertip_right.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.250980392156863 0.250980392156863 0.250980392156863 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_gripper_fingertip_right.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_gripper_fingertip_right"
+    type="fixed">
+    <origin
+      xyz="-0.190596948563868 -0.015 0"
+      rpy="-1.57079632679483 -3.43320051448326E-14 0.540456056432235" />
+    <parent
+      link="link_gripper_finger_right" />
+    <child
+      link="link_gripper_fingertip_right" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+  <link
+    name="link_gripper_finger_left">
+    <inertial>
+      <origin
+        xyz="0.0949811095686165 -0.00801522758203194 1.38777878078145E-15"
+        rpy="0 0 0" />
+      <mass
+        value="0.0476207785199479" />
+      <inertia
+        ixx="0"
+        ixy="0"
+        ixz="0"
+        iyy="0"
+        iyz="0"
+        izz="0" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="3.141592653589793 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_gripper_finger_left.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="1.5707963267948966 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_gripper_finger_left.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_gripper_finger_left"
+    type="revolute">
+    <origin
+      xyz="0.018599 0.003 0.033689"
+      rpy="1.5708 -1.5708 0" />
+    <parent
+      link="link_straight_gripper" />
+    <child
+      link="link_gripper_finger_left" />
+    <axis
+      xyz="0 0 -1" />
+    <limit effort="100" lower="-0.6" upper="0.6" velocity="1.0"/>
+  </joint>
+  <link
+    name="link_gripper_fingertip_left">
+    <inertial>
+      <origin
+        xyz="-2.59496317767116E-08 -6.65612598371723E-09 0.00812579036862837"
+        rpy="0 0 0" />
+      <mass
+        value="0.00382160686584851" />
+      <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_fingertip_left.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.250980392156863 0.250980392156863 0.250980392156863 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_gripper_fingertip_left.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_gripper_fingertip_left"
+    type="fixed">
+    <origin
+      xyz="0.190596948563868 -0.015 0"
+      rpy="1.57079632679496 4.51275387511463E-14 2.60113659715756" />
+    <parent
+      link="link_gripper_finger_left" />
+    <child
+      link="link_gripper_fingertip_left" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+</robot>

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>