Merge branch 'dev/noetic' into feature/dock_robot

hello_helpers/README.md

@@ -14,14 +14,144 @@
 
 ## Typical Usage
 
-```
+```python
 import hello_helpers.fit_plane as fp
 ```
+```python
+import hello_helpers.hello_misc as hm
+```
+```python
+import hello_helpers.hello_ros_viz as hr
 ```
+
+# API
+
+## Classes
+
+### [HelloNode](./src/hello_helpers/hello_misc.py)
+
+This class is a convenience class for creating a ROS node for Stretch. The most common way to use this class is to extend it. In your extending class, the main funcion would call `HelloNode`'s main function. This would look like:
+
+```python
+import hello_helpers.hello_misc as hm
+
+class MyNode(hm.HelloNode):
+    def __init__(self):
+        hm.HelloNode.__init__(self)
+
+    def main(self):
+        hm.HelloNode.main(self, 'my_node', 'my_node', wait_for_first_pointcloud=False)
+        # my_node's main logic goes here
+
+node = MyNode()
+node.main()
+```
+
+There is also a one-liner class method for instantiating a `HelloNode` for easy prototyping. One example where this is handy is sending pose commands from iPython:
+
+```python
+# roslaunch the stretch launch file beforehand
+
 import hello_helpers.hello_misc as hm
+temp = hm.HelloNode.quick_create('temp')
+temp.move_to_pose({'joint_lift': 0.4})
 ```
+
+#### Methods
+
+##### `move_to_pose(pose, return_before_done=False, custom_contact_thresholds=False, custom_full_goal=False)`
+
+This method takes in a dictionary that describes a desired pose for the robot and communicates with [stretch_driver](../stretch_core/README.md#stretchdrivernodesstretchdriver) to execute it. The basic format of this dictionary is string/number key/value pairs, where the keys are joint names and the values are desired position goals. For example, `{'joint_lift': 0.5}` would put the lift at 0.5m in its joint range. A full list of command-able joints is published to the `/stretch/joint_states` topic. Used within a node extending `HelloNode`, calling this method would look like:
+
+```python
+self.move_to_pose({'joint_lift': 0.5})
 ```
-import hello_helpers.hello_ros_viz as hr
+
+Internally, this dictionary is converted into a [FollowJointTrajectoryGoal](http://docs.ros.org/en/diamondback/api/control_msgs/html/msg/FollowJointTrajectoryGoal.html) that is sent to a [FollowJointTrajectory action](http://docs.ros.org/en/noetic/api/control_msgs/html/action/FollowJointTrajectory.html) server in stretch_driver. This method waits by default for the server to report that the goal has completed executing. However, you can return before the goal has completed by setting the `return_before_done` argument to True. This can be useful for preempting goals.
+
+There are two additional arguments that enable you to customize how the pose is executed. If you set `custom_contact_thresholds` to True, this method expects a different format dictionary: string/tuple key/value pairs, where the keys are still joint names, but the values are `(position_goal, effort_threshold)`. The addition of a effort threshold enables you to detect when a joint has made contact with something in the environment, which is useful for manipulation or safe movements. For example, `{'joint_arm': (0.5, 20)}` commands the telescoping arm fully out (the arm is nearly fully extended at 0.5 meters) but with a low enough effort threshold (20% of the arm motor's max effort) that the motor will stop when the end of arm has made contact with something. Again, in a node, this would look like:
+
+```python
+self.move_to_pose({'joint_arm': (0.5, 40)}, custom_contact_thresholds=True)
+```
+
+If you set `custom_full_goal` to True, the dictionary format is string/tuple key/value pairs, where keys are joint names again, but values are `(position_goal, velocity, acceleration, effort_threshold)`. The velocity and acceleration components allow you to customize the trajectory profile the joint follows while moving to the goal position. In the following example, the arm telescopes out slowly until contact is detected:
+
+```python
+self.move_to_pose({'joint_arm': (0.5, 0.01, 0.01, 40)}, custom_full_goal=True)
+```
+
+##### `get_robot_floor_pose_xya(floor_frame='odom')`
+
+Returns the current estimated x, y position and angle of the robot on the floor. This is typically called with respect to the odom frame or the map frame. x and y are in meters and the angle is in radians.
+
+##### `main(node_name, node_topic_namespace, wait_for_first_pointcloud=True)`
+
+When extending the `HelloNode` class, call this method at the very beginning of your `main()` method. This method handles setting up a few ROS components, including registering the node with the ROS server, creating a TF listener, creating a [FollowJointTrajectory](http://docs.ros.org/en/noetic/api/control_msgs/html/action/FollowJointTrajectory.html) client for the [`move_to_pose()`](#movetoposepose-returnbeforedonefalse-customcontactthresholdsfalse-customfullgoalfalse) method, subscribing to depth camera point cloud topic, and connecting to the quick-stop service.
+
+Since it takes up to 30 seconds for the head camera to start streaming data, the `wait_for_first_pointcloud` argument will get the node to wait until it has seen camera data, which is helpful if your node is processing camera data.
+
+##### `quick_create(name, wait_for_first_pointcloud=False)`
+
+A class level method for quick testing. This allows you to avoid having to extend `HelloNode` to use it.
+
+```python
+# roslaunch the stretch launch file beforehand
+
+import hello_helpers.hello_misc as hm
+temp = hm.HelloNode.quick_create('temp')
+temp.move_to_pose({'joint_lift': 0.4})
+```
+
+#### Subscribed Topics
+
+##### /camera/depth/color/points ([sensor_msgs/PointCloud2](http://docs.ros.org/en/noetic/api/sensor_msgs/html/msg/PointCloud2.html))
+
+Provides a point cloud as currently seen by the Realsense depth camera in Stretch's head. Accessible from the `self.point_cloud` attribute.
+
+```python
+# roslaunch the stretch launch file beforehand
+
+import hello_helpers.hello_misc as hm
+temp = hm.HelloNode.quick_create('temp', wait_for_first_pointcloud=True)
+print(temp.point_cloud)
+```
+
+#### Subscribed Services
+
+##### /stop_the_robot ([std_srvs/Trigger](https://docs.ros.org/en/noetic/api/std_srvs/html/srv/Trigger.html))
+
+Provides a service to quickly stop any motion currently executing on the robot.
+
+```python
+# roslaunch the stretch launch file beforehand
+
+from std_srvs.srv import TriggerRequest
+import hello_helpers.hello_misc as hm
+temp = hm.HelloNode.quick_create('temp')
+temp.stop_the_robot_service(TriggerRequest())
+```
+
+#### Other
+
+##### TF Listener
+
+Provides a [tf2](http://wiki.ros.org/tf2) listener that buffers [transforms](https://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/TransformStamped.html) under the `self.tf2_buffer` attribute. For example, we can do forward kinematics from the base_link to the link between the gripper fingers, link_grasp_center, using:
+
+```python
+# roslaunch the stretch launch file beforehand
+
+import rospy
+import hello_helpers.hello_misc as hm
+temp = hm.HelloNode.quick_create('temp')
+rate = rospy.Rate(10.0)
+while True:
+    try:
+        t = temp.tf2_buffer.lookup_transform('link_grasp_center', 'base_link', rospy.Time())
+        print(t.transform.translation, t.transform.rotation)
+    except:
+        continue
+    rate.sleep()
 ```
 
 ## License

hello_helpers/src/hello_helpers/hello_misc.py

@@ -5,6 +5,7 @@ import os
 import sys
 import glob
 import math
+import numbers
 
 import rospy
 import tf2_ros
@@ -69,40 +70,56 @@ class HelloNode:
         self.joint_state = None
         self.point_cloud = None
 
+    @classmethod
+    def quick_create(cls, name, wait_for_first_pointcloud=False):
+        i = cls()
+        i.main(name, name, wait_for_first_pointcloud)
+        return i
+
     def joint_states_callback(self, joint_state):
         self.joint_state = joint_state
 
     def point_cloud_callback(self, point_cloud):
         self.point_cloud = point_cloud
     
-    def move_to_pose(self, pose, return_before_done=False, custom_contact_thresholds=False):
-        joint_names = [key for key in pose]
+    def move_to_pose(self, pose, return_before_done=False, custom_contact_thresholds=False, custom_full_goal=False):
         point = JointTrajectoryPoint()
         point.time_from_start = rospy.Duration(0.0)
-
         trajectory_goal = FollowJointTrajectoryGoal()
         trajectory_goal.goal_time_tolerance = rospy.Time(1.0)
-        trajectory_goal.trajectory.joint_names = joint_names
-        if not custom_contact_thresholds: 
-            joint_positions = [pose[key] for key in joint_names]
-            point.positions = joint_positions
-            trajectory_goal.trajectory.points = [point]
+        trajectory_goal.trajectory.header.stamp = rospy.Time.now()
+        trajectory_goal.trajectory.joint_names = list(pose.keys())
+        trajectory_goal.trajectory.points = [point]
+
+        # populate goal
+        if custom_full_goal:
+            pose_correct = all([len(joint_goal)==4 for joint_goal in pose.values()])
+            if not pose_correct:
+                rospy.logerr(f"{self.node_name}'s HelloNode.move_to_pose: Not sending trajectory due to improper pose. The 'custom_full_goal' option requires tuple with 4 values (pose_target, velocity, acceleration, contact_threshold_effort) for each joint name, but pose = {pose}")
+                return
+            point.positions = [joint_goal[0] for joint_goal in pose.values()]
+            point.velocities = [joint_goal[1] for joint_goal in pose.values()]
+            point.accelerations = [joint_goal[2] for joint_goal in pose.values()]
+            point.effort = [joint_goal[3] for joint_goal in pose.values()]
+        elif custom_contact_thresholds:
+            pose_correct = all([len(joint_goal)==2 for joint_goal in pose.values()])
+            if not pose_correct:
+                rospy.logerr(f"{self.node_name}'s HelloNode.move_to_pose: Not sending trajectory due to improper pose. The 'custom_contact_thresholds' option requires tuple with 2 values (pose_target, contact_threshold_effort) for each joint name, but pose = {pose}")
+                return
+            point.positions = [joint_goal[0] for joint_goal in pose.values()]
+            point.effort = [joint_goal[1] for joint_goal in pose.values()]
         else:
-            pose_correct = all([len(pose[key])==2 for key in joint_names])
+            pose_correct = all([isinstance(joint_position, numbers.Real) for joint_position in pose.values()])
             if not pose_correct:
-                rospy.logerr("HelloNode.move_to_pose: Not sending trajectory due to improper pose. custom_contact_thresholds requires 2 values (pose_target, contact_threshold_effort) for each joint name, but pose = {0}".format(pose))
+                rospy.logerr(f"{self.node_name}'s HelloNode.move_to_pose: Not sending trajectory due to improper pose. The default option requires 1 value, pose_target as integer, for each joint name, but pose = {pose}")
                 return
-            joint_positions = [pose[key][0] for key in joint_names]
-            joint_efforts = [pose[key][1] for key in joint_names]
-            point.positions = joint_positions
-            point.effort = joint_efforts
-            trajectory_goal.trajectory.points = [point]
-        trajectory_goal.trajectory.header.stamp = rospy.Time.now()
+            point.positions = [joint_position for joint_position in pose.values()]
+
+        # send goal
         self.trajectory_client.send_goal(trajectory_goal)
-        if not return_before_done: 
+        if not return_before_done:
             self.trajectory_client.wait_for_result()
-            #print('Received the following result:')
-            #print(self.trajectory_client.get_result())
+            rospy.logdebug(f"{self.node_name}'s HelloNode.move_to_pose: got result {self.trajectory_client.get_result()}")
 
     def get_robot_floor_pose_xya(self, floor_frame='odom'):
         # Returns the current estimated x, y position and angle of the

stretch_core/nodes/command_groups.py

@@ -247,9 +247,11 @@ class GripperCommandGroup(SimpleCommandGroup):
 
 class ArmCommandGroup(SimpleCommandGroup):
     def __init__(self, range_m=None, calibrated_retracted_offset_m=None, node=None):
-        SimpleCommandGroup.__init__(self, 'wrist_extension', range_m, acceptable_joint_error=0.008, node=node)
+        SimpleCommandGroup.__init__(self, 'joint_arm', range_m, acceptable_joint_error=0.008, node=node)
         self.telescoping_joints = ['joint_arm_l3', 'joint_arm_l2', 'joint_arm_l1', 'joint_arm_l0']
         self.is_telescoping = False
+        self.wrist_extension_name = 'wrist_extension'
+        self.is_named_wrist_extension = False
         if calibrated_retracted_offset_m is None:
             calibrated_retracted_offset_m = node.controller_parameters['arm_retracted_offset']
         self.retracted_offset_m = calibrated_retracted_offset_m
@@ -278,16 +280,26 @@ class ArmCommandGroup(SimpleCommandGroup):
         self.is_telescoping = False
         self.index = None
         active_telescoping_joint_names = list(set(commanded_joint_names) & set(self.telescoping_joints))
-        if self.name in commanded_joint_names:
-            if len(active_telescoping_joint_names) == 0:
-                self.index = commanded_joint_names.index(self.name)
-                self.active = True
-            else:
-                err_str = ("Received a command for the wrist_extension joint and one or more telescoping_joints. "
+        if self.name in commanded_joint_names or self.wrist_extension_name in commanded_joint_names:
+            if self.name in commanded_joint_names and self.wrist_extension_name in commanded_joint_names:
+                err_str = ("Received a command for the joint_arm and wrist_extension joints. "
+                           "These are mutually exclusive options. The joint names in the received command = "
+                           "{0}").format(commanded_joint_names)
+                invalid_joints_callback(err_str)
+                return False
+            if len(active_telescoping_joint_names) != 0:
+                err_str = ("Received a command for the joint_arm/wrist_extension joint and one or more telescoping_joints. "
                            "These are mutually exclusive options. The joint names in the received command = "
                            "{0}").format(commanded_joint_names)
                 invalid_joints_callback(err_str)
                 return False
+            if self.name in commanded_joint_names:
+                self.index = commanded_joint_names.index(self.name)
+                self.active = True
+            else:
+                self.index = commanded_joint_names.index(self.wrist_extension_name)
+                self.is_named_wrist_extension = True
+                self.active = True
         elif len(active_telescoping_joint_names) != 0:
             if len(active_telescoping_joint_names) == len(self.telescoping_joints):
                 self.active = True
@@ -328,14 +340,14 @@ class ArmCommandGroup(SimpleCommandGroup):
             if goal_pos is None:
                 err_str = ("Received goal point with positions array length={0}. "
                            "This joint ({1})'s index is {2}. Length of array must cover all joints listed "
-                           "in commanded_joint_names.").format(len(point.positions), self.name, self.index)
+                           "in commanded_joint_names.").format(len(point.positions), self.name if not self.is_named_wrist_extension else self.wrist_extension_name, self.index)
                 invalid_goal_callback(err_str)
                 return False
 
             self.goal['position'] = hm.bound_ros_command(self.range, goal_pos, fail_out_of_range_goal)
             if self.goal['position'] is None:
                 err_str = ("Received {0} goal point that is out of bounds. "
-                            "Range = {1}, but goal point = {2}.").format(self.name, self.range, goal_pos)
+                            "Range = {1}, but goal point = {2}.").format(self.name if not self.is_named_wrist_extension else self.wrist_extension_name, self.range, goal_pos)
                 invalid_goal_callback(err_str)
                 return False
 
@@ -343,7 +355,7 @@ class ArmCommandGroup(SimpleCommandGroup):
 
     def init_execution(self, robot, robot_status, **kwargs):
         if self.active:
-            _, extension_error_m = self.update_execution(robot_status)
+            _, extension_error_m = self.update_execution(robot_status, force_single=True)
             robot.arm.move_by(extension_error_m,
                               v_m=self.goal['velocity'],
                               a_m=self.goal['acceleration'],
@@ -354,15 +366,22 @@ class ArmCommandGroup(SimpleCommandGroup):
 
     def update_execution(self, robot_status, **kwargs):
         success_callback = kwargs['success_callback'] if 'success_callback' in kwargs.keys() else None
+        force_single = kwargs['force_single'] if 'force_single' in kwargs.keys() else False
         self.error = None
         if self.active:
             if success_callback and robot_status['arm']['motor']['in_guarded_event']:
-                success_callback("{0} contact detected.".format(self.name))
+                success_callback("{0} contact detected.".format(self.name if not self.is_named_wrist_extension else self.wrist_extension_name))
                 return True
             arm_backlash_correction = self.retracted_offset_m if self.retracted else 0.0
             extension_current = robot_status['arm']['pos'] + arm_backlash_correction
             self.error = self.goal['position'] - extension_current
-            return (self.telescoping_joints, self.error) if self.is_telescoping else (self.name, self.error)
+            if force_single:
+                return self.name, self.error
+            else:
+                if self.is_telescoping:
+                    return [(telescoping_joint, self.error / len(self.telescoping_joints)) for telescoping_joint in self.telescoping_joints]
+                else:
+                    return [(self.name, self.error), (self.wrist_extension_name, self.error)]
 
         return None
 

stretch_core/nodes/detect_aruco_markers

@@ -511,8 +511,8 @@ class ArucoMarkerCollection:
         self.show_debug_images = show_debug_images
         
         self.marker_info = marker_info
-        self.aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)
-        self.aruco_detection_parameters =  aruco.DetectorParameters_create()
+        self.aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_6X6_250)
+        self.aruco_detection_parameters = aruco.DetectorParameters()
         # Apparently available in OpenCV 3.4.1, but not OpenCV 3.2.0.
         self.aruco_detection_parameters.cornerRefinementMethod = aruco.CORNER_REFINE_SUBPIX
         self.aruco_detection_parameters.cornerRefinementWinSize = 2

stretch_core/nodes/joint_trajectory_server.py

@@ -77,7 +77,7 @@ class JointTrajectoryAction:
 
         num_valid_points = sum([c.get_num_valid_commands() for c in self.command_groups])
         if num_valid_points <= 0:
-            err_str = ("Received a command without any valid joint names."
+            err_str = ("Received a command without any valid joint names. "
                        "Received joint names = {0}").format(commanded_joint_names)
             self.invalid_joints_callback(err_str)
             self.node.robot_mode_rwlock.release_read()

stretch_core/nodes/stretch_driver

@@ -189,7 +189,7 @@ class StretchDriverNode:
             joint_state.velocity.append(vel)
             joint_state.effort.append(effort)
 
-        # add telescoping joints to joint state
+        # add telescoping joints and wrist_extension to joint state
         arm_cg = self.joint_trajectory_action.arm_cg
         joint_state.name.extend(arm_cg.telescoping_joints)
         pos, vel, effort = arm_cg.joint_state(robot_status)
@@ -197,6 +197,10 @@ class StretchDriverNode:
             joint_state.position.append(pos / len(arm_cg.telescoping_joints))
             joint_state.velocity.append(vel / len(arm_cg.telescoping_joints))
             joint_state.effort.append(effort)
+        joint_state.name.append(arm_cg.wrist_extension_name)
+        joint_state.position.append(pos)
+        joint_state.velocity.append(vel)
+        joint_state.effort.append(effort)
 
         # add gripper joints to joint state
         gripper_cg = self.joint_trajectory_action.gripper_cg
@@ -453,8 +457,6 @@ class StretchDriverNode:
         self.linear_velocity_mps = 0.0 # m/s ROS SI standard for cmd_vel (REP 103)
         self.angular_velocity_radps = 0.0 # rad/s ROS SI standard for cmd_vel (REP 103)
 
-        self.max_arm_height = 1.1
-
         self.odom_pub = rospy.Publisher('odom', Odometry, queue_size=1)
 
         self.power_pub = rospy.Publisher('battery', BatteryState, queue_size=1)

stretch_demos/nodes/grasp_object

@@ -119,103 +119,106 @@ class GraspObjectNode(hm.HelloNode):
         self.look_at_surface(scan_time_s = 3.0)
         
         grasp_target = self.manipulation_view.get_grasp_target(self.tf2_buffer)
-        
-        if grasp_target is not None: 
-            pregrasp_lift_m = self.manipulation_view.get_pregrasp_lift(grasp_target, self.tf2_buffer)
-
-            if (self.lift_position is None):
-                return TriggerResponse(
-                    success=False,
-                    message='lift position unavailable'
-                )
-
-            if actually_move:
-                rospy.loginfo('Raise tool to pregrasp height.')
-                lift_to_pregrasp_m = max(self.lift_position + pregrasp_lift_m, 0.1)
-                lift_to_pregrasp_m = min(lift_to_pregrasp_m, max_lift_m)
-                pose = {'joint_lift': lift_to_pregrasp_m}
-                self.move_to_pose(pose)
+        if grasp_target is None:
+            return TriggerResponse(
+                success=False,
+                message='Failed to find grasp target'
+            )
 
-            pregrasp_yaw = self.manipulation_view.get_pregrasp_yaw(grasp_target, self.tf2_buffer)
-            print('pregrasp_yaw = {0:.2f} rad'.format(pregrasp_yaw))
-            print('pregrasp_yaw = {0:.2f} deg'.format(pregrasp_yaw * (180.0/np.pi)))
+        pregrasp_lift_m = self.manipulation_view.get_pregrasp_lift(grasp_target, self.tf2_buffer)
 
-            if actually_move:
-                rospy.loginfo('Rotate the gripper for grasping.')
-                pose = {'joint_wrist_yaw': pregrasp_yaw}
-                self.move_to_pose(pose)
-                
-                rospy.loginfo('Open the gripper.')
-                pose = {'gripper_aperture': 0.125}
-                self.move_to_pose(pose)
+        if (self.lift_position is None):
+            return TriggerResponse(
+                success=False,
+                message='lift position unavailable'
+            )
 
-            pregrasp_mobile_base_m, pregrasp_wrist_extension_m = self.manipulation_view.get_pregrasp_planar_translation(grasp_target, self.tf2_buffer)
+        if actually_move:
+            rospy.loginfo('Raise tool to pregrasp height.')
+            lift_to_pregrasp_m = max(self.lift_position + pregrasp_lift_m, 0.1)
+            lift_to_pregrasp_m = min(lift_to_pregrasp_m, max_lift_m)
+            pose = {'joint_lift': lift_to_pregrasp_m}
+            self.move_to_pose(pose)
+
+        pregrasp_yaw = self.manipulation_view.get_pregrasp_yaw(grasp_target, self.tf2_buffer)
+        print('pregrasp_yaw = {0:.2f} rad'.format(pregrasp_yaw))
+        print('pregrasp_yaw = {0:.2f} deg'.format(pregrasp_yaw * (180.0/np.pi)))
+
+        if actually_move:
+            rospy.loginfo('Rotate the gripper for grasping.')
+            pose = {'joint_wrist_yaw': pregrasp_yaw}
+            self.move_to_pose(pose)
             
-            print('pregrasp_mobile_base_m = {0:.3f} m'.format(pregrasp_mobile_base_m))
-            print('pregrasp_wrist_extension_m = {0:.3f} m'.format(pregrasp_wrist_extension_m))
-
-            if actually_move:
-                rospy.loginfo('Drive to pregrasp location.')
-                self.drive(pregrasp_mobile_base_m)
-
-                if pregrasp_wrist_extension_m > 0.0:
-                    extension_m = max(self.wrist_position + pregrasp_wrist_extension_m, min_extension_m)
-                    extension_m = min(extension_m, max_extension_m)
-                    rospy.loginfo('Extend tool above surface.')
-                    pose = {'wrist_extension': extension_m} 
-                    self.move_to_pose(pose)
-                else:
-                    print('negative wrist extension for pregrasp, so not extending or retracting.')
-
-            grasp_mobile_base_m, grasp_lift_m, grasp_wrist_extension_m = self.manipulation_view.get_grasp_from_pregrasp(grasp_target, self.tf2_buffer)
-            print('grasp_mobile_base_m = {0:3f} m, grasp_lift_m = {1:3f} m, grasp_wrist_extension_m = {2:3f} m'.format(grasp_mobile_base_m, grasp_lift_m, grasp_wrist_extension_m))
-
-            if actually_move: 
-                rospy.loginfo('Move the grasp pose from the pregrasp pose.')
-
-                lift_m = max(self.lift_position + grasp_lift_m, 0.1)
-                lift_m = min(lift_m, max_lift_m)
-                
-                extension_m = max(self.wrist_position + grasp_wrist_extension_m, min_extension_m)
+            rospy.loginfo('Open the gripper.')
+            pose = {'gripper_aperture': 0.125}
+            self.move_to_pose(pose)
+
+        pregrasp_mobile_base_m, pregrasp_wrist_extension_m = self.manipulation_view.get_pregrasp_planar_translation(grasp_target, self.tf2_buffer)
+
+        print('pregrasp_mobile_base_m = {0:.3f} m'.format(pregrasp_mobile_base_m))
+        print('pregrasp_wrist_extension_m = {0:.3f} m'.format(pregrasp_wrist_extension_m))
+
+        if actually_move:
+            rospy.loginfo('Drive to pregrasp location.')
+            self.drive(pregrasp_mobile_base_m)
+
+            if pregrasp_wrist_extension_m > 0.0:
+                extension_m = max(self.wrist_position + pregrasp_wrist_extension_m, min_extension_m)
                 extension_m = min(extension_m, max_extension_m)
-                
-                pose = {'translate_mobile_base': grasp_mobile_base_m,
-                        'joint_lift': lift_m,  
-                        'wrist_extension': extension_m}
+                rospy.loginfo('Extend tool above surface.')
+                pose = {'wrist_extension': extension_m}
                 self.move_to_pose(pose)
+            else:
+                print('negative wrist extension for pregrasp, so not extending or retracting.')
 
-                rospy.loginfo('Attempt to close the gripper on the object.')
-                gripper_aperture_m = grasp_target['width_m'] - 0.18
-                pose = {'gripper_aperture': gripper_aperture_m}
-                self.move_to_pose(pose)
-                
-                # Lifting appears to happen before the gripper has
-                # finished unless there is this sleep. Need to look
-                # into this issue.
-                rospy.sleep(3.0)
-
-                rospy.loginfo('Attempt to lift the object.')
-                object_lift_height_m = 0.1
-
-                lift_m = max(self.lift_position + object_lift_height_m, 0.2)
-                lift_m = min(lift_m, max_lift_m)
-                
-                pose = {'joint_lift': lift_m}
-                self.move_to_pose(pose)
+        grasp_mobile_base_m, grasp_lift_m, grasp_wrist_extension_m = self.manipulation_view.get_grasp_from_pregrasp(grasp_target, self.tf2_buffer)
+        print('grasp_mobile_base_m = {0:3f} m, grasp_lift_m = {1:3f} m, grasp_wrist_extension_m = {2:3f} m'.format(grasp_mobile_base_m, grasp_lift_m, grasp_wrist_extension_m))
 
-                rospy.loginfo('Open the gripper a little to avoid overtorquing and overheating the gripper motor.')
-                pose = {'gripper_aperture': gripper_aperture_m + 0.005}
-                self.move_to_pose(pose)
+        if actually_move:
+            rospy.loginfo('Move the grasp pose from the pregrasp pose.')
 
+            lift_m = max(self.lift_position + grasp_lift_m, 0.1)
+            lift_m = min(lift_m, max_lift_m)
 
-            if actually_move:
-                rospy.loginfo('Retract the tool.')
-                pose = {'wrist_extension': 0.01}
-                self.move_to_pose(pose)
+            extension_m = max(self.wrist_position + grasp_wrist_extension_m, min_extension_m)
+            extension_m = min(extension_m, max_extension_m)
 
-                rospy.loginfo('Reorient the wrist.')
-                pose = {'joint_wrist_yaw': 0.0}
-                self.move_to_pose(pose)
+            pose = {'translate_mobile_base': grasp_mobile_base_m,
+                    'joint_lift': lift_m,
+                    'wrist_extension': extension_m}
+            self.move_to_pose(pose)
+
+            rospy.loginfo('Attempt to close the gripper on the object.')
+            gripper_aperture_m = grasp_target['width_m'] - 0.18
+            pose = {'gripper_aperture': gripper_aperture_m}
+            self.move_to_pose(pose)
+
+            # Lifting appears to happen before the gripper has
+            # finished unless there is this sleep. Need to look
+            # into this issue.
+            rospy.sleep(3.0)
+
+            rospy.loginfo('Attempt to lift the object.')
+            object_lift_height_m = 0.1
+
+            lift_m = max(self.lift_position + object_lift_height_m, 0.2)
+            lift_m = min(lift_m, max_lift_m)
+
+            pose = {'joint_lift': lift_m}
+            self.move_to_pose(pose)
+
+            rospy.loginfo('Open the gripper a little to avoid overtorquing and overheating the gripper motor.')
+            pose = {'gripper_aperture': gripper_aperture_m + 0.005}
+            self.move_to_pose(pose)
+
+        if actually_move:
+            rospy.loginfo('Retract the tool.')
+            pose = {'wrist_extension': 0.01}
+            self.move_to_pose(pose)
+
+            rospy.loginfo('Reorient the wrist.')
+            pose = {'joint_wrist_yaw': 0.0}
+            self.move_to_pose(pose)
 
         return TriggerResponse(
             success=True,

stretch_demos/nodes/handover_object

@@ -163,12 +163,12 @@ class HandoverObjectNode(hm.HelloNode):
 
             
     def trigger_handover_object_callback(self, request):
-        with self.move_lock: 
+        with self.move_lock:
             # First, retract the wrist in preparation for handing out an object.
             pose = {'wrist_extension': 0.005}
             self.move_to_pose(pose)
 
-            if self.handover_goal_ready: 
+            if self.handover_goal_ready:
                 pose = {'joint_lift': self.lift_goal_m}
                 self.move_to_pose(pose)
                 tolerance_distance_m = 0.01
@@ -177,9 +177,14 @@ class HandoverObjectNode(hm.HelloNode):
                 self.move_to_pose(pose)
                 self.handover_goal_ready = False
 
-            return TriggerResponse(
-                success=True,
-                message='Completed object handover!'
+                return TriggerResponse(
+                    success=True,
+                    message='Completed object handover!'
+                )
+            else:
+                return TriggerResponse(
+                    success=False,
+                    message='Failed to find handover goal'
                 )
 
     

stretch_description/README.md

@@ -2,7 +2,13 @@
 
 ## Overview
 
-*stretch_description* provides materials for a [URDF](http://wiki.ros.org/urdf) kinematic model of the Stretch RE1 mobile manipulator from Hello Robot Inc.
+*stretch_description* provides materials for a [URDF](http://wiki.ros.org/urdf) kinematic model of the Stretch mobile manipulator from Hello Robot Inc.
+
+## Quick View
+
+```
+roslaunch stretch_description display.launch
+```
 
 ## Details
 
@@ -10,7 +16,7 @@ The *meshes directory* contains [STL mesh files](https://en.wikipedia.org/wiki/S
 
 The *urdf directory* contains [xacro files](http://wiki.ros.org/xacro) representing various parts of the robot that are used to generate the robot's URDF. 
 
-stretch_ros expects a URDF with the name stretch.urdf to reside within the urdf directory. The file stretch.urdf serves as the URDF for the robot and must be generated. Typically, it is a calibrated urdf file for the particular Stretch RE1 robot being used. To generate this file, please read the documentation within stretch_ros/stretch_calibration. 
+stretch_ros expects a URDF with the name stretch.urdf to reside within the urdf directory. The file stretch.urdf serves as the URDF for the robot and must be generated. Typically, it is a calibrated urdf file unique to the particular Stretch robot being used. To generate this file, please read the documentation within stretch_ros/stretch_calibration.
 
 The xacro_to_urdf.sh will usually only be indirectly run as part of various scripts and launch files within stretch_ros/stretch_calibration. 
 
@@ -20,7 +26,7 @@ Sometimes a stretch_uncalibrated.urdf file will reside with the urdf directory.
 
 Sometimes a URDF is useful outside of ROS, such as for simulations and analysis. Running the *export_urdf.sh* script in the urdf directory will export a full URDF model of the robot based on stretch.urdf. 
 
-The exported URDF will be found within an exported_urdf directory. It is also copied to a directory for your specific robot found under ~/stretch_user. The exported URDF includes meshes and controller calibration YAML files. The exported URDF can be visualized using stretch_urdf_show.py, which is part of the stretch_body Python code. 
+The exported URDF will be found within an exported_urdf directory. It is also copied to a directory for your specific robot found under ~/stretch_user. The exported URDF includes meshes and controller calibration YAML files. The exported URDF can be visualized using `stretch_robot_urdf_visualizer.py`, which is part of the stretch_body Python code.
 
 ## Changing the Tool
 
@@ -30,7 +36,7 @@ If you wish to remove the default gripper and add a different tool, you will typ
 
 As an example we provide the xacro `stretch_dry_erase_marker.xacro` and its dependent mesh files with stretch_ros. 
 
-Some of the tools found in the [Stretch Body Tool Share](https://github.com/hello-robot/stretch_tool_share/) include URDF data. To integrate these tools into the URDF for your Stretch
+Some of the tools found in the [Stretch Tool Share](https://github.com/hello-robot/stretch_tool_share/) include URDF data. To integrate these tools into the URDF for your Stretch
 
 ```bash
 >>$ cd ~/repos
@@ -54,8 +60,6 @@ Now visualize the new tool
 >>$ roslaunch stretch_calibration simple_test_head_calibration.launch
 ```
 
-
-
 ## License and Patents
 
 Patents are pending that cover aspects of the Stretch RE1 mobile manipulator.

stretch_description/batch/guthrie/urdf/stretch_base_imu.xacro

@@ -0,0 +1,61 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="stretch_base_imu">
+
+<link
+    name="base_imu">
+    <inertial>
+      <origin
+        xyz="0.00300349280517617 0.00149777182047641 -0.00193103885249443"
+        rpy="0 0 0" />
+      <mass
+        value="0.000901473198307758" />
+      <inertia
+        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
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/base_imu.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0 0.772549019607843 0.207843137254902 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/base_imu.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_base_imu"
+    type="fixed">
+    <origin
+      xyz="-0.12838 0.0031592 0.1474"
+      rpy="-3.1416 0 -1.5708" />
+    <parent
+      link="base_link" />
+    <child
+      link="base_imu" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+
+</robot>
+
+
+

stretch_description/batch/guthrie/urdf/stretch_description.xacro

@@ -9,6 +9,7 @@
   <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/batch/hank/urdf/stretch_base_imu.xacro

@@ -0,0 +1,61 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="stretch_base_imu">
+
+<link
+    name="base_imu">
+    <inertial>
+      <origin
+        xyz="0.00300349280517617 0.00149777182047641 -0.00193103885249443"
+        rpy="0 0 0" />
+      <mass
+        value="0.000901473198307758" />
+      <inertia
+        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
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/base_imu.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0 0.772549019607843 0.207843137254902 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/base_imu.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_base_imu"
+    type="fixed">
+    <origin
+      xyz="-0.12838 0.0031592 0.1474"
+      rpy="-3.1416 0 -1.5708" />
+    <parent
+      link="base_link" />
+    <child
+      link="base_imu" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+
+</robot>
+
+
+

stretch_description/batch/hank/urdf/stretch_description.xacro

@@ -9,6 +9,7 @@
   <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/batch/irma/urdf/stretch_base_imu.xacro

@@ -0,0 +1,61 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="stretch_base_imu">
+
+<link
+    name="base_imu">
+    <inertial>
+      <origin
+        xyz="0.00300349280517617 0.00149777182047641 -0.00193103885249443"
+        rpy="0 0 0" />
+      <mass
+        value="0.000901473198307758" />
+      <inertia
+        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
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/base_imu.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0 0.772549019607843 0.207843137254902 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/base_imu.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_base_imu"
+    type="fixed">
+    <origin
+      xyz="-0.12838 0.0031592 0.1474"
+      rpy="-3.1416 0 -1.5708" />
+    <parent
+      link="base_link" />
+    <child
+      link="base_imu" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+
+</robot>
+
+
+

stretch_description/batch/irma/urdf/stretch_description.xacro

@@ -9,6 +9,7 @@
   <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/batch/joplin/urdf/stretch_base_imu.xacro

@@ -0,0 +1,61 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="stretch_base_imu">
+
+<link
+    name="base_imu">
+    <inertial>
+      <origin
+        xyz="0.00300349280517617 0.00149777182047641 -0.00193103885249443"
+        rpy="0 0 0" />
+      <mass
+        value="0.000901473198307758" />
+      <inertia
+        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
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/base_imu.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0 0.772549019607843 0.207843137254902 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/base_imu.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_base_imu"
+    type="fixed">
+    <origin
+      xyz="-0.12838 0.0031592 0.1474"
+      rpy="-3.1416 0 -1.5708" />
+    <parent
+      link="base_link" />
+    <child
+      link="base_imu" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+
+</robot>
+
+
+

stretch_description/batch/joplin/urdf/stretch_description.xacro

@@ -9,6 +9,7 @@
   <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/batch/kendrick/urdf/stretch_base_imu.xacro

@@ -0,0 +1,61 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="stretch_base_imu">
+
+<link
+    name="base_imu">
+    <inertial>
+      <origin
+        xyz="0.00300349280517617 0.00149777182047641 -0.00193103885249443"
+        rpy="0 0 0" />
+      <mass
+        value="0.000901473198307758" />
+      <inertia
+        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
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/base_imu.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0 0.772549019607843 0.207843137254902 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/base_imu.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_base_imu"
+    type="fixed">
+    <origin
+      xyz="-0.12838 0.0031592 0.1474"
+      rpy="-3.1416 0 -1.5708" />
+    <parent
+      link="base_link" />
+    <child
+      link="base_imu" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+
+</robot>
+
+
+

stretch_description/batch/kendrick/urdf/stretch_description.xacro

@@ -9,6 +9,7 @@
   <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/batch/louis/urdf/stretch_base_imu.xacro

@@ -0,0 +1,61 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="stretch_base_imu">
+
+<link
+    name="base_imu">
+    <inertial>
+      <origin
+        xyz="0.00300349280517617 0.00149777182047641 -0.00193103885249443"
+        rpy="0 0 0" />
+      <mass
+        value="0.000901473198307758" />
+      <inertia
+        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
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/base_imu.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0 0.772549019607843 0.207843137254902 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/base_imu.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_base_imu"
+    type="fixed">
+    <origin
+      xyz="-0.12838 0.0031592 0.1474"
+      rpy="-3.1416 0 -1.5708" />
+    <parent
+      link="base_link" />
+    <child
+      link="base_imu" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+
+</robot>
+
+
+

stretch_description/batch/louis/urdf/stretch_description.xacro

@@ -9,6 +9,7 @@
   <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/batch/mitski/urdf/stretch_base_imu.xacro

@@ -0,0 +1,61 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="stretch_base_imu">
+
+<link
+    name="base_imu">
+    <inertial>
+      <origin
+        xyz="0.00300349280517617 0.00149777182047641 -0.00193103885249443"
+        rpy="0 0 0" />
+      <mass
+        value="0.000901473198307758" />
+      <inertia
+        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
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/base_imu.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0 0.772549019607843 0.207843137254902 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/base_imu.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_base_imu"
+    type="fixed">
+    <origin
+      xyz="-0.12838 0.0031592 0.1474"
+      rpy="-3.1416 0 -1.5708" />
+    <parent
+      link="base_link" />
+    <child
+      link="base_imu" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+
+</robot>
+
+
+

stretch_description/batch/mitski/urdf/stretch_description.xacro

@@ -9,6 +9,7 @@
   <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/batch/mitski/urdf/stretch_main.xacro

@@ -99,8 +99,8 @@
     <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}"/>
+      xyz="0 0 1" />
+    <dynamics damping="${joint_damping}" friction="${joint_friction}" spring_reference="${joint_spring_reference}" spring_stiffness="${joint_spring_stiffness}"/>
   </joint>
 
   <link
@@ -155,8 +155,8 @@
     <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}"/>
+      xyz="0 0 1" />
+    <dynamics damping="${joint_damping}" friction="${joint_friction}" spring_reference="${joint_spring_reference}" spring_stiffness="${joint_spring_stiffness}"/>
   </joint>
 
   <link name="caster_link">

stretch_description/batch/nina/urdf/stretch_aruco.xacro

@@ -0,0 +1,283 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="stretch_aruco">
+
+  <link
+    name="link_aruco_right_base">
+    <inertial>
+      <origin
+        xyz="1.3878E-17 0 -0.000125"
+        rpy="0 0 0" />
+      <mass
+        value="3.59999999990368E-06" />
+      <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_aruco_right_base.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_aruco_right_base.STL" />
+      </geometry>
+    </collision>
+  </link>
+  
+  <joint
+    name="joint_aruco_right_base"
+    type="fixed">
+    <origin
+      xyz="-0.0015028 -0.1304972 0.1597482"
+      rpy="0 0 -1.5707963267949" />
+    <parent
+      link="base_link" />
+    <child
+      link="link_aruco_right_base" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+
+  
+  <link
+    name="link_aruco_left_base">
+    <inertial>
+      <origin
+        xyz="2.7756E-17 0 -0.000125"
+        rpy="0 0 0" />
+      <mass
+        value="3.59999999990368E-06" />
+      <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_aruco_left_base.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_aruco_left_base.STL" />
+      </geometry>
+    </collision>
+  </link>
+  
+  <joint
+    name="joint_aruco_left_base"
+    type="fixed">
+    <origin
+      xyz="-0.00500000000000014 0.1304972 0.1597482"
+      rpy="0 0 -1.5707963267949" />
+    <parent
+      link="base_link" />
+    <child
+      link="link_aruco_left_base" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+  
+  <link
+    name="link_aruco_shoulder">
+    <inertial>
+      <origin
+        xyz="-2.77555756156289E-17 2.56739074444567E-16 -0.000125000000000042"
+        rpy="0 0 0" />
+      <mass
+        value="0" />
+      <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_aruco_shoulder.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_aruco_shoulder.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint
+    name="joint_aruco_shoulder"
+    type="fixed">
+    <origin
+      xyz="-0.0133768876375287 0.0558540528812078 0.0861368272417975"
+      rpy="-1.53998860117704E-29 3.55962409571165E-15 0.0" />
+    <parent
+      link="link_lift" />
+    <child
+      link="link_aruco_shoulder" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+
+  <link
+    name="link_aruco_top_wrist">
+    <inertial>
+      <origin
+        xyz="-1.3531E-15 -3.4972E-15 -0.000125"
+        rpy="0 0 0" />
+      <mass
+        value="0" />
+      <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_aruco_top_wrist.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_aruco_top_wrist.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint
+    name="joint_aruco_top_wrist"
+    type="fixed">
+    <origin
+      xyz="0.0472500000000019 0.0292850000000015 0"
+      rpy="1.5707963267949 -8.03728587323464E-15 3.14159265358979" />
+    <parent
+      link="link_arm_l0" />
+    <child
+      link="link_aruco_top_wrist" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+
+  <link
+    name="link_aruco_inner_wrist">
+    <inertial>
+      <origin
+        xyz="8.32667268468867E-17 1.77635683940025E-15 -0.000125000000000264"
+        rpy="0 0 0" />
+      <mass
+        value="0" />
+      <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_aruco_inner_wrist.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_aruco_inner_wrist.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint
+    name="joint_aruco_inner_wrist"
+    type="fixed">
+    <origin
+      xyz="0.0472499999999947 -0.0119000000000034 -0.0272499999991938"
+      rpy="3.14159265358979 4.23377442363088E-14 3.14159265358979" />
+    <parent
+      link="link_arm_l0" />
+    <child
+      link="link_aruco_inner_wrist" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+
+</robot>
+
+  
+

stretch_description/batch/nina/urdf/stretch_base_imu.xacro

@@ -0,0 +1,61 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="stretch_base_imu">
+
+<link
+    name="base_imu">
+    <inertial>
+      <origin
+        xyz="0.00300349280517617 0.00149777182047641 -0.00193103885249443"
+        rpy="0 0 0" />
+      <mass
+        value="0.000901473198307758" />
+      <inertia
+        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
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/base_imu.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0 0.772549019607843 0.207843137254902 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/base_imu.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_base_imu"
+    type="fixed">
+    <origin
+      xyz="-0.12838 0.0031592 0.1474"
+      rpy="-3.1416 0 -1.5708" />
+    <parent
+      link="base_link" />
+    <child
+      link="base_imu" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+
+</robot>
+
+
+

stretch_description/batch/nina/urdf/stretch_d435i.xacro

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="stretch_d435i">
+
+  <xacro:arg name="use_nominal_extrinsics" default="true"/>
+  <xacro:arg name="add_plug" default="false"/>
+  <xacro:include filename="$(find realsense2_description)/urdf/_d435i.urdf.xacro"/>
+
+  <!-- xyz = "
+       depth (- recessed into head / + protruding from front)
+       up and down (- down / + up)
+       sideways (- right / + left)
+       " -->
+  <xacro:sensor_d435i parent="link_head_tilt" use_nominal_extrinsics="$(arg use_nominal_extrinsics)">
+    <origin
+      xyz="0.03 -0.0122 0.0182"
+      rpy="0.0 0.0 0.0" />
+  </xacro:sensor_d435i>
+
+  
+</robot>
+
+  
+

stretch_description/batch/nina/urdf/stretch_description.xacro

@@ -0,0 +1,18 @@
+<?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>
+
+  
+

stretch_description/batch/nina/urdf/stretch_dry_erase_marker.xacro

@@ -0,0 +1,133 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="stretch_dry_erase_marker">
+
+  <xacro:property name="scale_finger_length" value="0.9" />
+
+    <link
+    name="link_dry_erase_holder">
+    <inertial>
+      <origin
+        xyz="-0.00033908527678797 -4.72017042119077E-09 0.0177808576938317"
+        rpy="0 0 0" />
+      <mass
+        value="0.0347079059161226" />
+      <inertia
+        ixx="7.67214948728222E-06"
+        ixy="-8.06855261037992E-08"
+        ixz="1.26219371489433E-07"
+        iyy="7.63827740251564E-06"
+        iyz="9.35838075307735E-08"
+        izz="1.65327256645883E-07" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_dry_erase_holder.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_dry_erase_holder.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_dry_erase_holder"
+    type="fixed">
+    <origin
+      xyz="0 0 -0.038"
+      rpy="-3.1416 -2.5445E-16 4.71238898038469" />
+    <parent
+      link="link_wrist_yaw" />
+    <child
+      link="link_dry_erase_holder" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+  <link
+    name="link_dry_erase_marker">
+    <inertial>
+      <origin
+        xyz="0.108173292344959 -2.76023830181327E-08 -0.000137755476015355"
+        rpy="0 0 0" />
+      <mass
+        value="0.00715580304305244" />
+      <inertia
+        ixx="3.95174478409412E-08"
+        ixy="2.65904489437298E-17"
+        ixz="1.33089367561534E-13"
+        iyy="3.95174462639471E-08"
+        iyz="-7.8953462362072E-12"
+        izz="1.57789054873579E-15" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_dry_erase_marker.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_dry_erase_marker.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_dry_erase_marker"
+    type="fixed">
+    <origin
+      xyz="-0.081029 0 0.025418"
+      rpy="-4.8183E-17 -2.5445E-16 -2.1228E-28" />
+    <parent
+      link="link_dry_erase_holder" />
+    <child
+      link="link_dry_erase_marker" />
+    <axis
+      xyz="1 0 0" />
+  </joint>
+  
+  <link
+      name="link_dry_erase_tip">
+  </link>
+  
+  <joint
+    name="joint_dry_erase_tip"
+    type="fixed">
+    <origin
+      xyz="0.005 0 0"
+      rpy="0 0 1.5707963267948966" />
+    <parent
+      link="link_dry_erase_marker" />
+    <child
+      link="link_dry_erase_tip" />
+  </joint>
+  
+  
+</robot>
+
+  
+

stretch_description/batch/nina/urdf/stretch_gripper.xacro

@@ -0,0 +1,302 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="stretch_gripper">
+
+  <xacro:property name="scale_finger_length" value="0.9" />
+
+  <link
+    name="link_gripper">
+    <inertial>
+      <origin
+        xyz="-0.0170229132730066 0.0131410320934285 -0.0371614759484659"
+        rpy="0 0 0" />
+      <mass
+        value="0.101902711393094" />
+      <inertia
+        ixx="0.001"
+        ixy="0"
+        ixz="0"
+        iyy="0.001"
+        iyz="0"
+        izz="0.001" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_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_gripper.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- rpy="3.1416 0 1.5708" -->
+  <!-- rpy="0.0 0 1.5708" -->
+  <joint
+    name="joint_gripper"
+    type="fixed">
+    <origin
+      xyz="0 0 0"
+      rpy="3.14159 0 -1.5708" />
+    <parent
+      link="link_wrist_yaw" />
+    <child
+      link="link_gripper" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+  
+  <link
+    name="link_gripper_finger_left">
+    <inertial>
+      <origin
+        xyz="0.0945047483510102 0.0124301080924361 -4.44089209850063E-16"
+        rpy="0 0 0" />
+      <mass
+        value="0.0476207785199474" />
+      <inertia
+        ixx="0.001"
+        ixy="0"
+        ixz="0"
+        iyy="0.001"
+        iyz="0"
+        izz="0.001" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_gripper_finger_left.STL" scale="${scale_finger_length} 1.0 1.0"/>
+      </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_gripper_finger_left.STL" scale="${scale_finger_length} 1.0 1.0"/>
+      </geometry>
+    </collision>
+  </link>
+  
+  <joint
+    name="joint_gripper_finger_left"
+    type="revolute">
+    <origin
+      xyz="-0.047231 -0.010151 -0.04679"
+      rpy="2.1762E-15 0.5236 3.1416" />
+    <parent
+      link="link_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="-3.16381381648689E-08 -2.91408530639359E-09 0.00812579670381812"
+        rpy="0 0 0" />
+      <mass
+        value="0.00382159917455729" />
+      <inertia
+        ixx="0.001"
+        ixy="0"
+        ixz="0"
+        iyy="0.001"
+        iyz="0"
+        izz="0.001" />
+    </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="${scale_finger_length * 0.19011} 0.014912 0"
+      rpy="-1.5708 -4.774E-15 -2.5545" />
+    <parent
+      link="link_gripper_finger_left" />
+    <child
+      link="link_gripper_fingertip_left" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+  
+  <link
+    name="link_gripper_finger_right">
+    <inertial>
+      <origin
+        xyz="-0.0945047483510099 -0.0124301080924345 0"
+        rpy="0 0 0" />
+      <mass
+        value="0.0476207785199481" />
+      <inertia
+        ixx="0.001"
+        ixy="0"
+        ixz="0"
+        iyy="0.001"
+        iyz="0"
+        izz="0.001" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_gripper_finger_right.STL" scale="${scale_finger_length} 1.0 1.0"/>
+      </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_gripper_finger_right.STL" scale="${scale_finger_length} 1.0 1.0"/>
+      </geometry>
+    </collision>
+  </link>
+  
+  <joint
+    name="joint_gripper_finger_right"
+    type="revolute">
+    <origin
+      xyz="-0.047231 0.010049 -0.04679"
+      rpy="3.1416 -0.5236 1.2943E-15" />
+    <parent
+      link="link_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.59386303963494E-08 6.70949018566347E-09 0.00812579516130402"
+        rpy="0 0 0" />
+      <mass
+        value="0.00382160037319545" />
+      <inertia
+        ixx="0.001"
+        ixy="0"
+        ixz="0"
+        iyy="0.001"
+        iyz="0"
+        izz="0.001" />
+    </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="${scale_finger_length * -0.19011} -0.014912 0"
+      rpy="-1.5708 -2.0539E-15 0.58705" />
+    <parent
+      link="link_gripper_finger_right" />
+    <child
+      link="link_gripper_fingertip_right" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+
+  
+  <link
+      name="link_grasp_center">
+  </link>
+  
+  <joint
+    name="joint_grasp_center"
+    type="fixed">
+    <origin
+      xyz="-0.205478 0 -0.138154"
+      rpy="0 0 3.141579" />
+    <parent
+      link="link_gripper" />
+    <child
+      link="link_grasp_center" />
+  </joint>
+  
+</robot>

stretch_description/batch/nina/urdf/stretch_gripper_with_puller.xacro

@@ -0,0 +1,359 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="stretch_gripper_with_puller">
+
+  <xacro:property name="scale_finger_length" value="0.9" />
+
+  <link
+    name="link_gripper">
+    <inertial>
+      <origin
+        xyz="-0.0170229132730066 0.0131410320934285 -0.0371614759484659"
+        rpy="0 0 0" />
+      <mass
+        value="0.101902711393094" />
+      <inertia
+        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
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_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_gripper.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- rpy="3.1416 0 1.5708" -->
+  <!-- rpy="0.0 0 1.5708" -->
+  <joint
+    name="joint_gripper"
+    type="fixed">
+    <origin
+      xyz="0 0 0"
+      rpy="3.14159 0 -1.5708" />
+    <parent
+      link="link_wrist_yaw" />
+    <child
+      link="link_gripper" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+
+    <link
+    name="link_puller">
+    <inertial>
+      <origin
+        xyz="-7.28171145880641E-08 -0.00602531629961259 -7.69536139810789E-08"
+        rpy="0 0 0" />
+      <mass
+        value="0.00292784355893436" />
+      <inertia
+        ixx="1.09317111513898E-07"
+        ixy="-9.31929125413026E-13"
+        ixz="-3.36065188387015E-16"
+        iyy="1.10835075609816E-15"
+        iyz="1.43669784789342E-12"
+        izz="1.09317110675672E-07" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/link_puller.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_puller.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_puller"
+    type="fixed">
+    <origin
+      xyz="0 0.07265 -0.0335"
+      rpy="4.2817E-15 1.1582E-14 -3.0531E-15" />
+    <parent
+      link="link_gripper" />
+    <child
+      link="link_puller" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+  
+  <link
+    name="link_gripper_finger_left">
+    <inertial>
+      <origin
+        xyz="0.0945047483510102 0.0124301080924361 -4.44089209850063E-16"
+        rpy="0 0 0" />
+      <mass
+        value="0.0476207785199474" />
+      <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_left.STL" scale="${scale_finger_length} 1.0 1.0"/>
+      </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_gripper_finger_left.STL" scale="${scale_finger_length} 1.0 1.0"/>
+      </geometry>
+    </collision>
+  </link>
+  
+  <joint
+    name="joint_gripper_finger_left"
+    type="revolute">
+    <origin
+      xyz="-0.047231 -0.010151 -0.04679"
+      rpy="2.1762E-15 0.5236 3.1416" />
+    <parent
+      link="link_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="-3.16381381648689E-08 -2.91408530639359E-09 0.00812579670381812"
+        rpy="0 0 0" />
+      <mass
+        value="0.00382159917455729" />
+      <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="${scale_finger_length * 0.19011} 0.014912 0"
+      rpy="-1.5708 -4.774E-15 -2.5545" />
+    <parent
+      link="link_gripper_finger_left" />
+    <child
+      link="link_gripper_fingertip_left" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+  
+  <link
+    name="link_gripper_finger_right">
+    <inertial>
+      <origin
+        xyz="-0.0945047483510099 -0.0124301080924345 0"
+        rpy="0 0 0" />
+      <mass
+        value="0.0476207785199481" />
+      <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" scale="${scale_finger_length} 1.0 1.0"/>
+      </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_gripper_finger_right.STL" scale="${scale_finger_length} 1.0 1.0"/>
+      </geometry>
+    </collision>
+  </link>
+  
+  <joint
+    name="joint_gripper_finger_right"
+    type="revolute">
+    <origin
+      xyz="-0.047231 0.010049 -0.04679"
+      rpy="3.1416 -0.5236 1.2943E-15" />
+    <parent
+      link="link_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.59386303963494E-08 6.70949018566347E-09 0.00812579516130402"
+        rpy="0 0 0" />
+      <mass
+        value="0.00382160037319545" />
+      <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="${scale_finger_length * -0.19011} -0.014912 0"
+      rpy="-1.5708 -2.0539E-15 0.58705" />
+    <parent
+      link="link_gripper_finger_right" />
+    <child
+      link="link_gripper_fingertip_right" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+
+  
+  <link
+      name="link_grasp_center">
+  </link>
+  
+  <joint
+    name="joint_grasp_center"
+    type="fixed">
+    <origin
+      xyz="-0.205478 0 -0.138154"
+      rpy="0 0 3.141579" />
+    <parent
+      link="link_gripper" />
+    <child
+      link="link_grasp_center" />
+  </joint>
+  
+</robot>
+
+  
+

stretch_description/batch/nina/urdf/stretch_laser_range_finder.xacro

@@ -0,0 +1,59 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="stretch_laser_range_finder">
+  
+  <link
+    name="laser">
+    <inertial>
+      <origin
+        xyz="0 0 -0.000755956127492408"
+        rpy="0 0 0" />
+      <mass
+        value="0.0749979022894495" />
+      <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/laser.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/laser.STL" />
+      </geometry>
+    </collision>
+  </link>
+  
+  <joint
+    name="joint_laser"
+    type="fixed">
+    <origin
+      xyz="0.004 0 0.1664"
+      rpy="0 0 3.1416" />
+    <parent
+      link="base_link" />
+    <child
+      link="laser" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+
+</robot>

stretch_description/batch/nina/urdf/stretch_main.xacro

@@ -0,0 +1,841 @@
+<?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.001"
+        ixy="0"
+        ixz="0"
+        iyy="0.001"
+        iyz="0"
+        izz="0.001" />
+    </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="continuous">
+    <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.001"
+        ixy="0"
+        ixz="0"
+        iyy="0.001"
+        iyz="0"
+        izz="0.001" />
+    </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="continuous">
+    <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="caster_link">
+    <collision>
+      <geometry>
+        <sphere radius="0.032"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <surface>
+        <friction>
+          <ode>
+            <mu>0</mu>
+            <mu2>0</mu2>
+            <slip1>1.0</slip1>
+            <slip2>1.0</slip2>
+          </ode>
+        </friction>
+      </surface>
+    </collision>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/omni_wheel_m.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
+      </material>
+    </visual>
+    <inertial>
+      <mass value="0.01" />
+      <origin xyz="0 0 0" />
+      <inertia
+        ixx="0.001"
+        ixy="0"
+        ixz="0"
+        iyy="0.001"
+        iyz="0"
+        izz="0.001" />
+    </inertial>
+  </link>
+
+  <joint name="caster_joint" type="fixed">
+    <parent link="base_link"/>
+    <child link="caster_link"/>
+    <origin xyz="-0.245 0.0 0.032" rpy="${-M_PI/2} 0 0"/>
+    <axis xyz="0 0 1" />
+  </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.0709854511954588"
+        ixy="-0.00433428742758457"
+        ixz="-0.000186110788697573"
+        iyy="0.000437922053342648"
+        iyz="-0.00288788257713431"
+        izz="0.071104808501661" />
+    </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.001"
+        ixy="0"
+        ixz="0"
+        iyy="0.001"
+        iyz="0"
+        izz="0.001" />
+    </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.001"
+        ixy="0"
+        ixz="0"
+        iyy="0.001"
+        iyz="0"
+        izz="0.001" />
+    </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="0.001"
+        ixy="0"
+        ixz="0"
+        iyy="0.001"
+        iyz="0"
+        izz="0.001" />
+    </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.001"
+        ixy="0"
+        ixz="0"
+        iyy="0.001"
+        iyz="0"
+        izz="0.001" />
+    </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="0.001"
+        ixy="0"
+        ixz="0"
+        iyy="0.001"
+        iyz="0"
+        izz="0.001" />
+    </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.5708 4.2595E-14 2.6415E-15"/>
+    <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="0.001"
+        ixy="0"
+        ixz="0"
+        iyy="0.001"
+        iyz="0"
+        izz="0.001" />
+    </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 3.1416" />
+    <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="0.001"
+        ixy="0"
+        ixz="0"
+        iyy="0.001"
+        iyz="0"
+        izz="0.001" />
+    </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="0.001"
+        ixy="0"
+        ixz="0"
+        iyy="0.001"
+        iyz="0"
+        izz="0.001" />
+    </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>

stretch_description/batch/nina/urdf/stretch_respeaker.xacro

@@ -0,0 +1,62 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="stretch_respeaker">
+
+  <link
+    name="respeaker_base">
+    <inertial>
+      <origin
+        xyz="-0.003809 -0.0023075 -0.012854"
+        rpy="0 0 0" />
+      <mass
+        value="0.015643" />
+      <inertia
+        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
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/respeaker_base.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/respeaker_base.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint
+    name="joint_respeaker"
+    type="fixed">
+    <origin
+      xyz="0 1.37236408874452 0.00303065898329655"
+      rpy="-1.5707963267949 -0.698131700797725 4.93295812652799E-16" />
+    <parent
+      link="link_mast" />
+    <child
+      link="respeaker_base" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+  
+</robot>
+
+  
+

stretch_description/launch/display.launch

@@ -0,0 +1,20 @@
+<launch>
+
+    <param name="robot_description" command="xacro $(find stretch_description)/urdf/stretch_description.xacro" />
+
+    <node name="joint_state_publisher_gui" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" >
+        <param name="rate" value="15.0"/>
+        <rosparam>
+            zeros:
+                joint_lift: 0.2
+                joint_wrist_yaw: 3.4
+        </rosparam>
+    </node>
+
+    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" >
+        <param name="publish_frequency" value="15.0"/>
+    </node>
+
+    <node name="rviz" pkg="rviz" type="rviz" output="screen" args="-d $(find stretch_description)/rviz/stretch.rviz" />
+
+</launch>

stretch_description/rviz/stretch.rviz

@@ -0,0 +1,330 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded: ~
+      Splitter Ratio: 0.5
+    Tree Height: 719
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        camera_accel_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_accel_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_bottom_screw_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_color_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_color_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_depth_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_depth_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_gyro_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_gyro_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_infra1_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_infra1_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_infra2_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_infra2_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        caster_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        laser:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_arm_l0:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_arm_l1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_arm_l2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_arm_l3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_arm_l4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_aruco_inner_wrist:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_aruco_left_base:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_aruco_right_base:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_aruco_shoulder:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_aruco_top_wrist:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_grasp_center:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        link_gripper:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_gripper_finger_left:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_gripper_finger_right:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_gripper_fingertip_left:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_gripper_fingertip_right:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_head:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_head_pan:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_head_tilt:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_left_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_lift:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_mast:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_right_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_wrist_yaw:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        respeaker_base:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      Robot Description: robot_description
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: base_link
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
+      Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 3.0607848167419434
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Field of View: 0.7853981852531433
+      Focal Point:
+        X: 0.17513221502304077
+        Y: 0.0775941014289856
+        Z: 0.4477757513523102
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.38979804515838623
+      Target Frame: <Fixed Frame>
+      Yaw: 0.4481166899204254
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1016
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000001b50000035afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000363000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000057d0000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1848
+  X: 72
+  Y: 27

stretch_description/urdf/stretch_base_imu.xacro

@@ -0,0 +1,61 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="stretch_base_imu">
+
+<link
+    name="base_imu">
+    <inertial>
+      <origin
+        xyz="0.00300349280517617 0.00149777182047641 -0.00193103885249443"
+        rpy="0 0 0" />
+      <mass
+        value="0.000901473198307758" />
+      <inertia
+        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
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/base_imu.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0 0.772549019607843 0.207843137254902 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://stretch_description/meshes/base_imu.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_base_imu"
+    type="fixed">
+    <origin
+      xyz="-0.12838 0.0031592 0.1474"
+      rpy="-3.1416 0 -1.5708" />
+    <parent
+      link="base_link" />
+    <child
+      link="base_imu" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+
+</robot>
+
+
+

stretch_description/urdf/stretch_description.xacro

@@ -9,6 +9,7 @@
   <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_main.xacro

@@ -99,8 +99,8 @@
     <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}"/>
+      xyz="0 0 1" />
+    <dynamics damping="${joint_damping}" friction="${joint_friction}" spring_reference="${joint_spring_reference}" spring_stiffness="${joint_spring_stiffness}"/>
   </joint>
 
   <link
@@ -155,8 +155,8 @@
     <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}"/>
+      xyz="0 0 1" />
+    <dynamics damping="${joint_damping}" friction="${joint_friction}" spring_reference="${joint_spring_reference}" spring_stiffness="${joint_spring_stiffness}"/>
   </joint>
 
   <link name="caster_link">

stretch_funmap/src/stretch_funmap/segment_max_height_image.py

@@ -15,7 +15,7 @@ from stretch_funmap.numba_height_image import numba_create_segment_image_uint8
 import hello_helpers.fit_plane as fp
 
 
-def find_object_to_grasp(height_image, display_on=False): 
+def find_object_to_grasp(height_image, display_on=False):
     h_image = height_image.image
     m_per_unit = height_image.m_per_height_unit
     m_per_pix = height_image.m_per_pix
@@ -251,9 +251,11 @@ def find_closest_flat_surface(height_image, robot_xy_pix, display_on=False):
     segments_image, segment_info, height_to_segment_id = segment(image, m_per_unit, zero_height, segmentation_scale, verbose=False)
     if segment_info is None:
         return None, None
-        
+
     floor_id, floor_mask = find_floor(segment_info, segments_image, verbose=False)
-    
+    if floor_mask is None:
+        return None, None
+
     remove_floor = True
     if remove_floor:
         segments_image[floor_mask > 0] = 0
@@ -1030,15 +1032,9 @@ def find_floor(segment_info, segments_image, verbose=False):
         height_m = segment_info[i]['height_m']
         bin_value = segment_info[i]['bin_value']
 
-        # Old values changed on June 3, 2021 due to no floor segment
-        # being within the bounds, which resulted in an error.
-        
-        #min_floor_m = -0.02
-        #max_floor_m = 0.1
-
-        # New values set on June 3, 2021.
-        min_floor_m = -0.05
-        max_floor_m = 0.05
+        # New values set on June 14, 2022.
+        min_floor_m = -0.1
+        max_floor_m = 0.1
 
         if verbose: 
             print('segment = {0}, histogram bin_value = {1}, height_m = {2}, min_floor_m = {3}, max_floor_m = {4}'.format(i, bin_value, height_m, min_floor_m, max_floor_m))
@@ -1061,8 +1057,7 @@ def find_floor(segment_info, segments_image, verbose=False):
             print('floor_id =', floor_id)
             print('max_bin_value =', max_bin_value)
     else:
-        if verbose: 
-            print('segment_max_height_image.py : no floor segment found...')
+        print('segment_max_height_image.py : no floor segment found...')
     return floor_id, floor_mask
 
 

stretch_gazebo/README.md

@@ -53,6 +53,16 @@ This will launch an Rviz instance that visualizes the sensors and an empty world
 
 ![](../images/gazebo.png)
 
+#### Running Demo with Keyboard Teleop node
+*keyboard_teleop_gazebo* : node that provides a keyboard interface to control the robot's joints within the gazebo simulation.
+
+```bash
+# Terminal 1:
+roslaunch stretch_gazebo gazebo.launch rviz:=true
+# Terminal 2:
+rosrun stretch_gazebo keyboard_teleop_gazebo
+```
+
 ## Running Gazebo with MoveIt! and Stretch
 
 ```bash

stretch_gazebo/nodes/keyboard.py

@@ -0,0 +1,44 @@
+#!/usr/bin/env python3
+
+import sys, tty, termios
+
+# This allows Ctrl-C and related keyboard commands to still function.
+# It detects escape codes in order to recognize arrow keys. It also
+# has buffer flushing to avoid repeated commands.  This is
+# blocking code.
+
+def getch():
+  stdin_fd = 0
+  # "Return a list containing the tty attributes for file descriptor
+  # fd, as follows: [iflag, oflag, cflag, lflag, ispeed, ospeed, cc]"
+  # from https://docs.python.org/2/library/termios.html
+  original_tty_attributes = termios.tcgetattr(stdin_fd)
+  new_tty_attributes = termios.tcgetattr(stdin_fd)
+  # Change the lflag (local modes) to turn off canonical mode
+  new_tty_attributes[3] &= ~termios.ICANON
+  # Set VMIN = 0 and VTIME > 0 for a timed read, as explained in:
+  # http://unixwiz.net/techtips/termios-vmin-vtime.html
+  new_tty_attributes[6][termios.VMIN] = b'\x00'
+  new_tty_attributes[6][termios.VTIME] = b'\x01'
+  try:
+    termios.tcsetattr(stdin_fd, termios.TCSAFLUSH, new_tty_attributes)
+    ch1 = sys.stdin.read(1)
+    if ch1 == '\x1b':
+      # special key pressed
+      ch2 = sys.stdin.read(1)
+      ch3 = sys.stdin.read(1)
+      ch = ch1 + ch2 + ch3
+    else:
+      # not a special key
+      ch = ch1
+  finally:
+    termios.tcsetattr(stdin_fd, termios.TCSAFLUSH, original_tty_attributes)
+  return ch
+
+
+if __name__ == '__main__':
+
+    while True:
+        c = getch()
+        print('c')
+

stretch_gazebo/nodes/keyboard_teleop_gazebo

@@ -0,0 +1,288 @@
+#!/usr/bin/env python
+from __future__ import print_function
+
+import math
+import keyboard as kb
+
+import rospy
+from sensor_msgs.msg import JointState
+from control_msgs.msg import FollowJointTrajectoryGoal, FollowJointTrajectoryAction
+from trajectory_msgs.msg import JointTrajectoryPoint
+from geometry_msgs.msg import Twist
+import actionlib
+
+
+# NOTE: FUNMAP services and capabilities won't work with Stretch Gazebo simulation
+
+class GetKeyboardCommands:
+    def __init__(self):
+        self.step_size = 'medium'
+        self.rad_per_deg = math.pi / 180.0
+        self.small_deg = 3.0
+        self.small_rad = self.rad_per_deg * self.small_deg
+        self.small_translate = 0.005  # 0.02
+        self.medium_deg = 6.0
+        self.medium_rad = self.rad_per_deg * self.medium_deg
+        self.medium_translate = 0.04
+        self.big_deg = 12.0
+        self.big_rad = self.rad_per_deg * self.big_deg
+        self.big_translate = 0.06
+
+    def get_deltas(self):
+        if self.step_size == 'small':
+            deltas = {'rad': self.small_rad, 'translate': self.small_translate}
+        if self.step_size == 'medium':
+            deltas = {'rad': self.medium_rad, 'translate': self.medium_translate}
+        if self.step_size == 'big':
+            deltas = {'rad': self.big_rad, 'translate': self.big_translate}
+        return deltas
+
+    def print_commands(self, joint_state, command):
+        if command is None:
+            return
+
+        joints = joint_state.name
+
+        def in_joints(i):
+            return len(list(set(i) & set(joints))) > 0
+
+        print('---------- KEYBOARD TELEOP MENU -----------')
+        print('                                           ')
+        if in_joints(['joint_head_tilt']):
+            print('              i HEAD UP                    ')
+        if in_joints(['joint_head_pan']):
+            print(' j HEAD LEFT            l HEAD RIGHT       ')
+        if in_joints(['joint_head_tilt']):
+            print('              , HEAD DOWN                  ')
+        print('                                           ')
+        print('                                           ')
+        print(' 7 BASE ROTATE LEFT     9 BASE ROTATE RIGHT')
+        print(' home                   page-up            ')
+        print('                                           ')
+        print('                                           ')
+        if in_joints(['joint_lift']):
+            print('              8 LIFT UP                    ')
+            print('              up-arrow                     ')
+        print(' 4 BASE FORWARD         6 BASE BACK        ')
+        print(' left-arrow             right-arrow        ')
+        if in_joints(['joint_lift']):
+            print('              2 LIFT DOWN                  ')
+            print('              down-arrow                   ')
+        print('                                           ')
+        print('                                           ')
+        if in_joints(['joint_arm_l0', 'joint_arm_l1', 'joint_arm_l2', 'joint_arm_l3']):
+            print('              w ARM OUT                    ')
+        if in_joints(['joint_wrist_yaw']):
+            print(' a WRIST FORWARD        d WRIST BACK       ')
+        if in_joints(['joint_arm_l0', 'joint_arm_l1', 'joint_arm_l2', 'joint_arm_l3']):
+            print('              x ARM IN                     ')
+        if in_joints(['joint_wrist_pitch', 'joint_wrist_roll']):
+            print('                                           ')
+            print('                                           ')
+        if in_joints(['joint_wrist_pitch']):
+            print(' c PITCH FORWARD        v PITCH BACK       ')
+        if in_joints(['joint_wrist_roll']):
+            print(' o ROLL FORWARD         p ROLL BACK        ')
+        print('                                           ')
+        print('                                           ')
+        if in_joints(['joint_gripper_finger_left', 'joint_gripper_finger_right', 'gripper_aperture']):
+            print('              5 GRIPPER CLOSE              ')
+            print('              0 GRIPPER OPEN               ')
+        print('                                           ')
+        print('  step size:  b BIG, m MEDIUM, s SMALL     ')
+        print('                                           ')
+        print('              q QUIT                       ')
+        print('                                           ')
+        print('-------------------------------------------')
+
+    def get_command(self, node):
+        command = None
+
+        c = kb.getch()
+
+        ####################################################
+        ## BASIC KEYBOARD TELEOPERATION COMMANDS
+        ####################################################
+
+        # 8 or up arrow
+        if c == '8' or c == '\x1b[A':
+            command = {'joint': 'joint_lift', 'delta': self.get_deltas()['translate']}
+        # 2 or down arrow
+        if c == '2' or c == '\x1b[B':
+            command = {'joint': 'joint_lift', 'delta': -self.get_deltas()['translate']}
+
+        # 4 or left arrow
+        if c == '4' or c == '\x1b[D':
+            command = {'joint': 'translate_mobile_base', 'inc': self.get_deltas()['translate']}
+        # 6 or right arrow
+        if c == '6' or c == '\x1b[C':
+            command = {'joint': 'translate_mobile_base', 'inc': -self.get_deltas()['translate']}
+        # 1 or end key
+        if c == '7' or c == '\x1b[H':
+            command = {'joint': 'rotate_mobile_base', 'inc': self.get_deltas()['rad']}
+        # 3 or pg down 5~
+        if c == '9' or c == '\x1b[5':
+            command = {'joint': 'rotate_mobile_base', 'inc': -self.get_deltas()['rad']}
+
+        if c == 'w' or c == 'W':
+            command = {'joint': 'wrist_extension', 'delta': self.get_deltas()['translate']}
+        if c == 'x' or c == 'X':
+            command = {'joint': 'wrist_extension', 'delta': -self.get_deltas()['translate']}
+        if c == 'd' or c == 'D':
+            command = {'joint': 'joint_wrist_yaw', 'delta': -self.get_deltas()['rad']}
+        if c == 'a' or c == 'A':
+            command = {'joint': 'joint_wrist_yaw', 'delta': self.get_deltas()['rad']}
+        if c == 'v' or c == 'V':
+            command = {'joint': 'joint_wrist_pitch', 'delta': -self.get_deltas()['rad']}
+        if c == 'c' or c == 'C':
+            command = {'joint': 'joint_wrist_pitch', 'delta': self.get_deltas()['rad']}
+        if c == 'p' or c == 'P':
+            command = {'joint': 'joint_wrist_roll', 'delta': -self.get_deltas()['rad']}
+        if c == 'o' or c == 'O':
+            command = {'joint': 'joint_wrist_roll', 'delta': self.get_deltas()['rad']}
+        if c == '5' or c == '\x1b[E' or c == 'g' or c == 'G':
+            # grasp
+            command = {'joint': 'joint_gripper_finger_left', 'delta': -self.get_deltas()['rad']}
+        if c == '0' or c == '\x1b[2' or c == 'r' or c == 'R':
+            # release
+            command = {'joint': 'joint_gripper_finger_left', 'delta': self.get_deltas()['rad']}
+        if c == 'i' or c == 'I':
+            command = {'joint': 'joint_head_tilt', 'delta': (2.0 * self.get_deltas()['rad'])}
+        if c == ',' or c == '<':
+            command = {'joint': 'joint_head_tilt', 'delta': -(2.0 * self.get_deltas()['rad'])}
+        if c == 'j' or c == 'J':
+            command = {'joint': 'joint_head_pan', 'delta': (2.0 * self.get_deltas()['rad'])}
+        if c == 'l' or c == 'L':
+            command = {'joint': 'joint_head_pan', 'delta': -(2.0 * self.get_deltas()['rad'])}
+        if c == 'b' or c == 'B':
+            rospy.loginfo('process_keyboard.py: changing to BIG step size')
+            self.step_size = 'big'
+        if c == 'm' or c == 'M':
+            rospy.loginfo('process_keyboard.py: changing to MEDIUM step size')
+            self.step_size = 'medium'
+        if c == 's' or c == 'S':
+            rospy.loginfo('process_keyboard.py: changing to SMALL step size')
+            self.step_size = 'small'
+        if c == 'q' or c == 'Q':
+            rospy.loginfo('keyboard_teleop exiting...')
+            rospy.signal_shutdown('Received quit character (q), so exiting')
+
+        ####################################################
+
+        return command
+
+
+class KeyboardTeleopNode:
+
+    def __init__(self):
+
+        self.keys = GetKeyboardCommands()
+        self.rate = 10.0
+        self.joint_state = None
+        self.twist = Twist()
+
+    def joint_states_callback(self, joint_state):
+        self.joint_state = joint_state
+
+    def send_command(self, command):
+        joint_state = self.joint_state
+        self.trajectory_client_selector(command)
+        if (joint_state is not None) and (command is not None):
+            if 'translate_mobile_base' == command['joint'] or 'rotate_mobile_base' == command['joint']:
+                self.cmd_vel_pub.publish(self.twist)
+                return 0
+            point = JointTrajectoryPoint()
+            point.time_from_start = rospy.Duration(0.2)
+            trajectory_goal = FollowJointTrajectoryGoal()
+            trajectory_goal.goal_time_tolerance = rospy.Time(1.0)
+
+            joint_name = command['joint']
+
+            if joint_name in ['joint_lift', 'joint_wrist_yaw', 'joint_head_pan', 'joint_head_tilt']:
+                trajectory_goal.trajectory.joint_names = [joint_name]
+                joint_index = joint_state.name.index(joint_name)
+                joint_value = joint_state.position[joint_index]
+                delta = command['delta']
+                new_value = joint_value + delta
+                point.positions = [new_value]
+            elif joint_name in ["joint_gripper_finger_left", "wrist_extension"]:
+                if joint_name == "joint_gripper_finger_left":
+                    trajectory_goal.trajectory.joint_names = ['joint_gripper_finger_left', 'joint_gripper_finger_right']
+                else:
+                    trajectory_goal.trajectory.joint_names = ['joint_arm_l0','joint_arm_l1', 'joint_arm_l2', 'joint_arm_l3']
+                positions = []
+                for j_name in trajectory_goal.trajectory.joint_names:
+                    joint_index = joint_state.name.index(j_name)
+                    joint_value = joint_state.position[joint_index]
+                    delta = command['delta']
+                    new_value = joint_value + delta/len(trajectory_goal.trajectory.joint_names)
+                    positions.append(new_value)
+                point.positions = positions
+
+            trajectory_goal.trajectory.points = [point]
+            trajectory_goal.trajectory.header.stamp = rospy.Time.now()
+            self.trajectory_client.send_goal(trajectory_goal)
+            self.trajectory_client.wait_for_result()
+
+    def trajectory_client_selector(self, command):
+        self.trajectory_client = None
+        self.twist.linear.x = 0
+        self.twist.angular.z = 0
+        try:
+            joint = command['joint']
+            if joint == 'joint_lift' or joint == 'joint_wrist_yaw' or joint == 'wrist_extension':
+                self.trajectory_client = self.trajectory_arm_client
+            if joint == 'joint_head_pan' or joint == 'joint_head_tilt':
+                self.trajectory_client = self.trajectory_head_client
+            if joint == 'joint_gripper_finger_right' or joint == 'joint_gripper_finger_left':
+                self.trajectory_client = self.trajectory_gripper_client
+            if joint == 'translate_mobile_base' or joint == 'rotate_mobile_base':
+                if joint == 'translate_mobile_base':
+                    if 'inc' in command:
+                        self.twist.linear.x = command['inc']
+                    else:
+                        self.twist.linear.x = command['delta']
+                else:
+                    if 'inc' in command:
+                        self.twist.angular.z = command['inc']
+                    else:
+                        self.twist.angular.z = command['delta']
+        except TypeError:
+            0
+
+    def main(self):
+        rospy.init_node('keyboard_teleop_gazebo')
+
+        self.trajectory_gripper_client = actionlib.SimpleActionClient(
+            '/stretch_gripper_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
+        server_reached = self.trajectory_gripper_client.wait_for_server(timeout=rospy.Duration(60.0))
+
+        self.trajectory_head_client = actionlib.SimpleActionClient('/stretch_head_controller/follow_joint_trajectory',
+                                                                   FollowJointTrajectoryAction)
+        server_reached = self.trajectory_head_client.wait_for_server(timeout=rospy.Duration(60.0))
+
+        self.trajectory_arm_client = actionlib.SimpleActionClient('/stretch_arm_controller/follow_joint_trajectory',
+                                                                  FollowJointTrajectoryAction)
+        server_reached = self.trajectory_arm_client.wait_for_server(timeout=rospy.Duration(60.0))
+
+        self.cmd_vel_pub = rospy.Publisher('/stretch_diff_drive_controller/cmd_vel', Twist, queue_size=10)
+
+        rospy.Subscriber('/joint_states', JointState, self.joint_states_callback)
+
+        rate = rospy.Rate(self.rate)
+
+        command = 1  # set equal to not None, so menu is printed out on first loop
+        while not rospy.is_shutdown():
+            if self.joint_state is not None:
+                self.keys.print_commands(self.joint_state, command)
+                command = self.keys.get_command(self)
+                self.send_command(command)
+            rate.sleep()
+
+
+if __name__ == '__main__':
+    try:
+        node = KeyboardTeleopNode()
+        node.main()
+    except KeyboardInterrupt:
+        rospy.loginfo('interrupt received, so shutting down')