Resolve conflict

hello_helpers/README.md

@@ -57,6 +57,33 @@ temp = hm.HelloNode.quick_create('temp')
 temp.move_to_pose({'joint_lift': 0.4})
 ```
 
+#### Attributes
+
+##### `dryrun`
+
+This attribute allows you to control whether the robot actually moves when calling `move_to_pose()`, `home_the_robot()`, `stow_the_robot()`, or other motion methods in this class. When `dryrun` is set to True, these motion methods return immediately. This attribute is helpful when you want to run just the perception/planning part of your node without actually moving the robot. For example, you could replace the following verbose snippet:
+
+```python
+# roslaunch the stretch launch file beforehand
+import hello_helpers.hello_misc as hm
+temp = hm.HelloNode.quick_create('temp')
+actually_move = False
+[...]
+if actually_move:
+    temp.move_to_pose({'translate_mobile_base': 1.0})
+```
+
+to be more consise:
+
+```python
+# roslaunch the stretch launch file beforehand
+import hello_helpers.hello_misc as hm
+temp = hm.HelloNode.quick_create('temp')
+[...]
+temp.dryrun = True
+temp.move_to_pose({'translate_mobile_base': 1.0})
+```
+
 #### Methods
 
 ##### `move_to_pose(pose, return_before_done=False, custom_contact_thresholds=False, custom_full_goal=False)`
@@ -81,6 +108,18 @@ If you set `custom_full_goal` to True, the dictionary format is string/tuple key
 self.move_to_pose({'joint_arm': (0.5, 0.01, 0.01, 40)}, custom_full_goal=True)
 ```
 
+##### `home_the_robot()`
+
+This is a convenience method to interact with the driver's [`/home_the_robot` service](../stretch_core/README.md#home_the_robot-std_srvstrigger).
+
+##### `stow_the_robot()`
+
+This is a convenience method to interact with the driver's [`/stow_the_robot` service](../stretch_core/README.md#stow_the_robot-std_srvstrigger).
+
+##### `stop_the_robot()`
+
+This is a convenience method to interact with the driver's [`/stop_the_robot` service](../stretch_core/README.md#stop_the_robot-std_srvstrigger).
+
 ##### `get_tf(from_frame, to_frame)`
 
 Use this method to get the transform ([geometry_msgs/TransformStamped](https://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/TransformStamped.html)) between two frames. This method is blocking. For example, this method can do forward kinematics from the base_link to the link between the gripper fingers, link_grasp_center, using:

hello_helpers/src/hello_helpers/hello_misc.py

@@ -20,6 +20,7 @@ from trajectory_msgs.msg import JointTrajectoryPoint
 import tf2_ros
 from sensor_msgs.msg import PointCloud2
 from std_srvs.srv import Trigger, TriggerRequest
+from std_msgs.msg import String
 
 
 #######################
@@ -69,6 +70,8 @@ class HelloNode:
     def __init__(self):
         self.joint_state = None
         self.point_cloud = None
+        self.tool = None
+        self.dryrun = False
 
     @classmethod
     def quick_create(cls, name, wait_for_first_pointcloud=False):
@@ -81,8 +84,14 @@ class HelloNode:
 
     def point_cloud_callback(self, point_cloud):
         self.point_cloud = point_cloud
-    
+
+    def tool_callback(self, tool_string):
+        self.tool = tool_string.data
+
     def move_to_pose(self, pose, return_before_done=False, custom_contact_thresholds=False, custom_full_goal=False):
+        if self.dryrun:
+            return
+
         point = JointTrajectoryPoint()
         point.time_from_start = rospy.Duration(0.0)
         trajectory_goal = FollowJointTrajectoryGoal()
@@ -164,6 +173,34 @@ class HelloNode:
                 continue
             rate.sleep()
 
+    def home_the_robot(self):
+        if self.dryrun:
+            return
+
+        trigger_request = TriggerRequest()
+        trigger_result = self.home_the_robot_service(trigger_request)
+        rospy.logdebug(f"{self.node_name}'s HelloNode.home_the_robot: got message {trigger_result.message}")
+        return trigger_result.success
+
+    def stow_the_robot(self):
+        if self.dryrun:
+            return
+
+        trigger_request = TriggerRequest()
+        trigger_result = self.stow_the_robot_service(trigger_request)
+        rospy.logdebug(f"{self.node_name}'s HelloNode.stow_the_robot: got message {trigger_result.message}")
+        return trigger_result.success
+
+    def stop_the_robot(self):
+        trigger_request = TriggerRequest()
+        trigger_result = self.stop_the_robot_service(trigger_request)
+        rospy.logdebug(f"{self.node_name}'s HelloNode.stop_the_robot: got message {trigger_result.message}")
+        return trigger_result.success
+
+    def get_tool(self):
+        assert(self.tool is not None)
+        return self.tool
+
     def main(self, node_name, node_topic_namespace, wait_for_first_pointcloud=True):
         rospy.init_node(node_name)
         self.node_name = rospy.get_name()
@@ -177,14 +214,20 @@ class HelloNode:
         
         self.tf2_buffer = tf2_ros.Buffer()
         self.tf2_listener = tf2_ros.TransformListener(self.tf2_buffer)
-        
+
+        self.tool_subscriber = rospy.Subscriber('/tool', String, self.tool_callback)
+
         self.point_cloud_subscriber = rospy.Subscriber('/camera/depth/color/points', PointCloud2, self.point_cloud_callback)
         self.point_cloud_pub = rospy.Publisher('/' + node_topic_namespace + '/point_cloud2', PointCloud2, queue_size=1)
 
+        rospy.wait_for_service('/home_the_robot')
+        rospy.wait_for_service('/stow_the_robot')
         rospy.wait_for_service('/stop_the_robot')
-        rospy.loginfo('Node ' + self.node_name + ' connected to /stop_the_robot service.')
+        rospy.loginfo('Node ' + self.node_name + ' connected to robot services.')
+        self.home_the_robot_service = rospy.ServiceProxy('/home_the_robot', Trigger)
+        self.stow_the_robot_service = rospy.ServiceProxy('/stow_the_robot', Trigger)
         self.stop_the_robot_service = rospy.ServiceProxy('/stop_the_robot', Trigger)
-        
+
         if wait_for_first_pointcloud:
             # Do not start until a point cloud has been received
             point_cloud_msg = self.point_cloud

stretch_core/README.md

@@ -26,26 +26,73 @@ If set to true, stretch_driver will publish an odom to base_link TF.
 
 #### Published Topics
 
+##### /mode ([std_msgs/String](https://docs.ros.org/en/noetic/api/std_msgs/html/msg/String.html))
+
+This topic publishes which mode the driver is in at 15hz. stretch_driver has a few modes that change how the robot is controlled. The modes are:
+
+ - "position": The default and simplest mode. In this mode, you can control every joint on the robot using position commands. For example, the telescoping arm (whose range is from zero fully retracted to ~52 centimeters fully extended) would move to 25cm out when you send it a [0.25m position cmd](../hello_helpers/README.md#movetoposepose-returnbeforedonefalse-customcontactthresholdsfalse-customfullgoalfalse). Two kinds of position commands are available for the mobile base, translation and rotation, with joint names "translate_mobile_base" and "rotate_mobile_base", and these commands have no limits since the wheels can spin continuously.
+   - Position commands are tracked in the firmware by a trapezoidal motion profile, and specifing the optional velocity and acceleration in [JointTrajectoryPoint](https://docs.ros.org/en/noetic/api/trajectory_msgs/html/msg/JointTrajectoryPoint.html) changes the shape of the trapezoid. More details about the motion profile [in the tutorial](https://docs.hello-robot.com/0.2/stretch-tutorials/ros1/follow_joint_trajectory/).
+   - Position commands can be contact sensitive, which is helpful for manipulating objects in the world. For example, I could [open a cabinet](https://youtu.be/SXgj9be3PdM) by reaching out with the telescoping arm and detecting contact with the door. In order to specify contact thresholds for a position command, the optional effort in [JointTrajectoryPoint](https://docs.ros.org/en/noetic/api/trajectory_msgs/html/msg/JointTrajectoryPoint.html) is misused to mean a threshold for the effort the robot will apply while executing the position command.
+   - Position commands can be preemptable, so you can issue a new position command before the previous one has finished and the robot will smoothly move to execute the latest command. This feature is helpful for scripts that use visual servo-ing or allow a user to teleop the robot.
+   - The driver can be switched into "position" mode at any time using the [switch_to_position_mode service](#TODO).
+ - "navigation": In this mode, every joint behaves identically to "position" mode except for the mobile base. The mobile base responds to velocity commands at a topic instead of position commands via the "translate_mobile_base" and "rotate_mobile_base" joints in the action server. You would publish [geometry_msgs/Twist](https://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/Twist.html) messages to the [/stretch/cmd_vel topic](#TODO). Since Twist messages are generalized to robots that can move with velocity in any direction, only the `Twist.linear.x` (translational velocity) and `Twist.angular.z` (rotational velocity) fields apply for differential drive mobile bases.
+   - Velocity control of the base is a common way to move mobile robots around. For example, the [Navigation Stack](https://github.com/hello-robot/stretch_ros/blob/noetic/stretch_navigation/README.md) is a piece of software that uses this mode to move the robot to different points in a map.
+   - This mode has a few safety features to prevent Stretch from "running away" at the last commanded velocity if the node that was sending commands were to crash for whatever reason. The first is a 0.5 second timeout within the stretch_driver node, which means that if the driver doesn't receive a new Twist command within 0.5s, the base will be commanded to stop smoothly. The second is a 1 second timeout within the firmware of the wheels, which means that even if the ROS layer were to crash, the robot will still be commanded to stop abruptly within 1 second at the lowest layer. This low-level feature was added relatively recently to the firmware, so be sure to [update your firmware](https://github.com/hello-robot/stretch_firmware/blob/master/docs/tutorial_updating_firmware.md) to the latest version.
+   - The driver can be switched into "navigation" mode at any time using the [switch_to_navigation_mode service](#TODO).
+ - "trajectory": **Not available in ROS1, check out [Stretch ROS2](https://github.com/hello-robot/stretch_ros2)**. In this mode, every joint follows a trajectory that is modeled as a spline. The key benefit of this mode is control over the timing at which the robot achieves positions (a.k.a waypoints), enabling smooth and coordinated motion through a preplanned trajectory. [More details here](https://forum.hello-robot.com/t/creating-smooth-motion-using-trajectories/671).
+ - "homing": This is the mode reported by the driver when a homing sequence has been triggered (e.g. through the [home_the_robot service](#hometherobot-stdsrvstriggerhttpsdocsrosorgennoeticapistdsrvshtmlsrvtriggerhtml)). While this mode is active, no other commands will be accepted by the driver. After the robot has completed its 30 second homing sequence, it will return to the mode it was in before.
+ - "stowing": This is the mode reported by the driver when a stowing sequence has been triggered (e.g. through the [stow_the_robot service](#stowtherobot-stdsrvstriggerhttpsdocsrosorgennoeticapistdsrvshtmlsrvtriggerhtml)). While this mode is active, no other commands will be accepted by the driver. After the robot has completed its stowing sequence, it will return to the mode it was in before.
+ - "runstopped": This is the mode reported by the driver when the robot is in runstop, either through the user pressing the [glowing white button](https://docs.hello-robot.com/0.2/stretch-tutorials/getting_started/safety_guide/#runstop) in Stretch's head or through the [runstop service](#runstop-stdsrvssetboolhttpsdocsrosorgennoeticapistdsrvshtmlsrvsetboolhtml). While this mode is active, no other commands will be accepted by the driver. After the robot has been taken out of runstop, it will return to the mode it was in before, or "position" mode if the driver was launched while the robot was runstopped.
+ - "manipulation": **Deprecated**. This mode was previously available in ROS1 Melodic, but was removed when the driver was ported to ROS1 Noetic. The mode supported a virtual prismatic joint for the mobile base, allowing you to treat the mobile base as a joint that could move forwards/backwards 0.5m. It was removed because the mode had little utility for most users of the driver.
+
 ##### /battery ([sensor_msgs/BatteryState](https://docs.ros.org/en/noetic/api/sensor_msgs/html/msg/BatteryState.html))
 
 This topic publishes Stretch's battery and charge status. Charging status, the `power_supply_status` field, is estimated by looking at changes in voltage readings over time, where plugging-in causes the voltage to jump up (i.e. status becomes 'charging') and pulling the plug out is detected by a voltage dip (i.e. status becomes 'discharging'). Estimation of charging status is most reliable when the charger is in SUPPLY mode (see [docs here](https://docs.hello-robot.com/0.2/stretch-hardware-guides/docs/battery_maintenance_guide_re1/#charger) for how to change charging modes). Charging status is unknown at boot of this node. Consequently, the `current` field is positive at boot of this node, regardless of whether the robot is charging/discharging. After a charging state change, there is a ~10 second timeout where state changes won't be detected. Additionally, outlier voltage readings can slip past the filters and incorrectly indicate a charging state change (albeit rarely). Finally, voltage readings are affected by power draw (e.g. the onboard computer starts a computationally taxing program), which can lead to incorrect indications of charging state change. Stretch RE2s have a hardware switch in the charging port that can detect when a plug has been plugged in, regardless of whether the plug is providing any power. Therefore, this node combines the previous voltage-based estimate with readings from this hardware switch to make better charging state estimates on RE2s (effectively eliminating the false positive case where a computational load draws more power).
 
 Since a battery is always present on a Stretch system, we instead misuse the `present` field to indicate whether a plug is plugged in to the charging port (regardless of whether it's providing power) on RE2 robots. This field is always false on RE1s. The unmeasured fields (e.g. charge in Ah) return a NaN, or 'not a number'.
 
+##### /is_homed ([std_msgs/Bool](https://docs.ros.org/en/noetic/api/std_msgs/html/msg/Bool.html))
+
+This topic publishes whether Stretch's encoders has been homed. If the robot isn't homed, joint states will be incorrect and motion commands won't be accepted by the driver. The [home_the_robot service](#hometherobot-stdsrvstriggerhttpsdocsrosorgennoeticapistdsrvshtmlsrvtriggerhtml) can be used to home Stretch.
+
+##### /is_runstopped ([std_msgs/Bool](https://docs.ros.org/en/noetic/api/std_msgs/html/msg/Bool.html))
+
+This topic publishes whether Stretch is runstopped. When the robot is runstopped (typically by [pressing glowing white button](https://docs.hello-robot.com/0.2/stretch-tutorials/getting_started/safety_guide/#runstop) in Stretch's head), motion for all joints is stopped and new motion commands aren't accepted. This is a safety feature built into the firmware for the four primary actuators. It's also possible to runstop the robot programmatically using the [runstop service](#runstop-stdsrvssetboolhttpsdocsrosorgennoeticapistdsrvshtmlsrvsetboolhtml)
+
+##### /is_calibrated ([std_msgs/Bool](https://docs.ros.org/en/noetic/api/std_msgs/html/msg/Bool.html))
+
+**Deprecated:** This topic has been renamed to [is_homed](#ishomed-stdmsgsboolhttpsdocsrosorgennoeticapistdmsgshtmlmsgboolhtml) because we often use the terms "calibrated" or "calibration" in context of [URDF calibrations](../stretch_calibration/README.md), whereas this topic returns whether the robot's encoders are homed.
+
 #### Published Services
 
-##### /calibrate_the_robot ([std_srvs/Trigger](https://docs.ros.org/en/noetic/api/std_srvs/html/srv/Trigger.html))
+##### /home_the_robot ([std_srvs/Trigger](https://docs.ros.org/en/noetic/api/std_srvs/html/srv/Trigger.html))
 
-This service will start Stretch's homing procedure, where every joint's zero is found. Robots with relative encoders (vs absolute encoders) need a homing procedure when they power on. For Stretch, it's a 30-second procedure that must occur everytime the robot wakes up before you may send motion commands to or read correct joint positions from Stretch's prismatic and multiturn revolute joints. When this service is triggered, the [mode topic](#mode-std_msgsstring) will reflect that the robot is in "calibration" mode, and after the homing procedure is complete, will switch back to whatever mode the robot was in before this service was triggered. While stretch_driver is in "calibration" mode, no commands to the [cmd_vel topic](#TODO) or the [follow joint trajectory action service](#TODO) will be accepted.
+This service will start Stretch's homing procedure, where every joint's zero is found. Robots with relative encoders (vs absolute encoders) need a homing procedure when they power on. For Stretch, it's a 30-second procedure that must occur everytime the robot wakes up before you may send motion commands to or read correct joint positions from Stretch's prismatic and multiturn revolute joints. When this service is triggered, the [mode topic](#mode-stdmsgsstringhttpsdocsrosorgennoeticapistdmsgshtmlmsgstringhtml) will reflect that the robot is in "homing" mode, and after the homing procedure is complete, will switch back to whatever mode the robot was in before this service was triggered. While stretch_driver is in "homing" mode, no commands to the [cmd_vel topic](#TODO) or the [follow joint trajectory action service](#TODO) will be accepted.
 
 Other ways to home the robot include using the `stretch_robot_home.py` CLI tool from a terminal, or calling [`robot.home()`](https://docs.hello-robot.com/0.2/stretch-tutorials/stretch_body/tutorial_stretch_body_api/#stretch_body.robot.Robot.home) from Stretch's Python API.
 
+Runstopping the robot while this service is running will yield undefined behavior and likely leave the driver in a bad state.
+
 ##### /stow_the_robot ([std_srvs/Trigger](https://docs.ros.org/en/noetic/api/std_srvs/html/srv/Trigger.html))
 
-This service will start Stretch's stowing procedure, where the arm is stowed into the footprint of the mobile base. This service is more convenient than sending a [follow joint trajectory command](#TODO) since it knows what gripper is installed at the end of arm and stows these additional joints automatically.
+This service will start Stretch's stowing procedure, where the arm is stowed into the footprint of the mobile base. This service is more convenient than sending a [follow joint trajectory command](#TODO) since it knows what gripper is installed at the end of arm and stows these additional joints automatically. When this service is triggered, the [mode topic](#mode-stdmsgsstringhttpsdocsrosorgennoeticapistdmsgshtmlmsgstringhtml) will reflect that the robot is in "stowing" mode, and after the homing procedure is complete, will switch back to whatever mode the robot was in before this service was triggered. While stretch_driver is in "stowing" mode, no commands to the [cmd_vel topic](#TODO) or the [follow joint trajectory action service](#TODO) will be accepted.
 
 Other ways to stow the robot include using the `stretch_robot_stow.py` CLI tool from a terminal, or calling [`robot.stow()`](https://docs.hello-robot.com/0.2/stretch-tutorials/stretch_body/tutorial_stretch_body_api/#stretch_body.robot.Robot.stow) from Stretch's Python API.
 
+Runstopping the robot while this service is running will yield undefined behavior and likely leave the driver in a bad state.
+
+##### /stop_the_robot ([std_srvs/Trigger](https://docs.ros.org/en/noetic/api/std_srvs/html/srv/Trigger.html))
+
+This service immediately stops any currently active motion.
+
+##### /runstop ([std_srvs/SetBool](https://docs.ros.org/en/noetic/api/std_srvs/html/srv/SetBool.html))
+
+This service can put Stretch into runstop or take Stretch out of runstop. It's common to put the robot into/out of runstop by pressing the [glowing white button](https://docs.hello-robot.com/0.2/stretch-tutorials/getting_started/safety_guide/#runstop) in Stretch's head (at which point the robot will beep and the button will be blinking to indicate that it's runstopped), and holding the button down for two seconds to take it out of runstop (the button will return to non-blinking). This service acts as a programmatic way to achieve the same effect. When this service is triggered, the [mode topic](#mode-stdmsgsstringhttpsdocsrosorgennoeticapistdmsgshtmlmsgstringhtml) will reflect that the robot is in "runstopped" mode, and after the robot is taken out of runstop, the driver will switch back to whatever mode the robot was in before this service was triggered. While stretch_driver is in "runstopped" mode, no commands to the [cmd_vel topic](#TODO) or the [follow joint trajectory action service](#TODO) will be accepted.
+
+##### /calibrate_the_robot ([std_srvs/Trigger](https://docs.ros.org/en/noetic/api/std_srvs/html/srv/Trigger.html))
+
+**Deprecated:** This service has been renamed to [home_the_robot](#hometherobot-stdsrvstriggerhttpsdocsrosorgennoeticapistdsrvshtmlsrvtriggerhtml) because we often use the terms "calibrate" or "calibration" in context of [URDF calibrations](../stretch_calibration/README.md), whereas this service homes the robot's encoders.
+
 #### Transforms
 
 See [stretch_driver.launch's TF docs](#TODO) to learn about the full Stretch TF tree. The stretch_driver node, which is part of stretch_driver.launch, is responsible for publishing the "odom" to "base_link" transform if the [broadcast_odom_tf parameter](#broadcastodomtf) is set to true. Odometry for the robot is calculated within the underlying Stretch Body Python SDK within the [update method in the Base class](https://github.com/hello-robot/stretch_body/blob/ea987c3d4f21a65ce4e85c6c92cd5d2efb832c41/body/stretch_body/base.py#L555-L651) by looking at the encoders for the left and right wheels. Note: Odometry calculated from wheel encoders is susceptible to drift for a variety of reasons (e.g. wheel slip, misalignment, loose belt tension, time dilation). A reasonable amount of drift is ~1cm per meter translated by the mobile base. A common remedy is to use a localization library to correct for the drift by integrating other sensors into the calculation of the odometry. For example, [AMCL](http://wiki.ros.org/amcl) is a particle filter ROS package for using Lidar scans + wheel odometry to correct for drift.

stretch_core/nodes/keyboard_teleop

@@ -126,48 +126,56 @@ class GetKeyboardCommands:
                 trigger_request = TriggerRequest() 
                 trigger_result = node.trigger_drive_to_scan_service(trigger_request)
                 rospy.loginfo('trigger_result = {0}'.format(trigger_result))
+                command = 'service_occurred'
                 
         # Trigger localizing the robot to a new pose anywhere on the current map
         if ((c == '+') or (c == '=')) and self.mapping_on:
             trigger_request = TriggerRequest() 
             trigger_result = node.trigger_global_localization_service(trigger_request)
             rospy.loginfo('trigger_result = {0}'.format(trigger_result))
+            command = 'service_occurred'
 
         # Trigger localizing the robot to a new pose that is near its current pose on the map
         if ((c == '-') or (c == '_')) and self.mapping_on:
             trigger_request = TriggerRequest() 
             trigger_result = node.trigger_local_localization_service(trigger_request)
             rospy.loginfo('trigger_result = {0}'.format(trigger_result))
+            command = 'service_occurred'
 
         # Trigger driving the robot to the estimated next best place to perform a 3D scan
         if ((c == '\\') or (c == '|')) and self.mapping_on:
             trigger_request = TriggerRequest() 
             trigger_result = node.trigger_drive_to_scan_service(trigger_request)
             rospy.loginfo('trigger_result = {0}'.format(trigger_result))
+            command = 'service_occurred'
 
         # Trigger performing a 3D scan using the D435i
         if (c == ' ') and self.mapping_on:
             trigger_request = TriggerRequest() 
             trigger_result = node.trigger_head_scan_service(trigger_request)
             rospy.loginfo('trigger_result = {0}'.format(trigger_result))
+            command = 'service_occurred'
 
         # Trigger rotating the mobile base to align with the nearest 3D cliff detected visually
         if ((c == '[') or (c == '{')) and self.mapping_on:
             trigger_request = TriggerRequest() 
             trigger_result = node.trigger_align_with_nearest_cliff_service(trigger_request)
             rospy.loginfo('trigger_result = {0}'.format(trigger_result))
+            command = 'service_occurred'
 
         # DEPRECATED: Trigger extend arm until contact
         if ((c == ']') or (c == '}')) and self.mapping_on:
             trigger_request = TriggerRequest() 
             trigger_result = node.trigger_reach_until_contact_service(trigger_request)
             rospy.loginfo('trigger_result = {0}'.format(trigger_result))
+            command = 'service_occurred'
 
         # DEPRECATED: Trigger lower arm until contact
         if ((c == ':') or (c == ';')) and self.mapping_on:
             trigger_request = TriggerRequest() 
             trigger_result = node.trigger_lower_until_contact_service(trigger_request)
             rospy.loginfo('trigger_result = {0}'.format(trigger_result))
+            command = 'service_occurred'
             
         
         ####################################################
@@ -179,36 +187,42 @@ class GetKeyboardCommands:
             trigger_request = TriggerRequest() 
             trigger_result = node.trigger_write_hello_service(trigger_request)
             rospy.loginfo('trigger_result = {0}'.format(trigger_result))
+            command = 'service_occurred'
 
         # Trigger open drawer demo with downward hook motion
         if ((c == 'z') or (c == 'Z')) and self.open_drawer_on:
             trigger_request = TriggerRequest() 
             trigger_result = node.trigger_open_drawer_down_service(trigger_request)
             rospy.loginfo('trigger_result = {0}'.format(trigger_result))
+            command = 'service_occurred'
 
         # Trigger open drawer demo with upward hook motion
         if ((c == '.') or (c == '>')) and self.open_drawer_on:
             trigger_request = TriggerRequest() 
             trigger_result = node.trigger_open_drawer_up_service(trigger_request)
             rospy.loginfo('trigger_result = {0}'.format(trigger_result))
+            command = 'service_occurred'
 
         # Trigger clean surface demo
         if ((c == '/') or (c == '?')) and self.clean_surface_on:
             trigger_request = TriggerRequest() 
             trigger_result = node.trigger_clean_surface_service(trigger_request)
             rospy.loginfo('trigger_result = {0}'.format(trigger_result))
+            command = 'service_occurred'
             
         # Trigger grasp object demo    
         if ((c == '\'') or (c == '\"')) and self.grasp_object_on:
             trigger_request = TriggerRequest() 
             trigger_result = node.trigger_grasp_object_service(trigger_request)
             rospy.loginfo('trigger_result = {0}'.format(trigger_result))
+            command = 'service_occurred'
 
         # Trigger deliver object demo    
         if ((c == 'y') or (c == 'Y')) and self.deliver_object_on:
             trigger_request = TriggerRequest() 
             trigger_result = node.trigger_deliver_object_service(trigger_request)
             rospy.loginfo('trigger_result = {0}'.format(trigger_result))
+            command = 'service_occurred'
 
             
         ####################################################
@@ -311,7 +325,7 @@ class KeyboardTeleopNode(hm.HelloNode):
 
     def send_command(self, command):
         joint_state = self.joint_state
-        if (joint_state is not None) and (command is not None):
+        if (joint_state is not None) and (command is not None) and (isinstance(command, dict)):
             point = JointTrajectoryPoint()
             point.time_from_start = rospy.Duration(0.0)
             trajectory_goal = FollowJointTrajectoryGoal()

stretch_core/nodes/stretch_driver

@@ -40,9 +40,10 @@ class StretchDriverNode:
 
         self.robot_stop_lock = threading.Lock()
         self.stop_the_robot = False
-
         self.robot_mode_rwlock = RWLock()
         self.robot_mode = None
+        self.control_modes = ['position', 'navigation']
+        self.prev_runstop_state = None
 
         self.voltage_history = []
         self.charging_state_history = [BatteryState.POWER_SUPPLY_STATUS_UNKNOWN] * 10
@@ -82,6 +83,7 @@ class StretchDriverNode:
         current_time = rospy.Time.now()
         robot_status = self.robot.get_status()
 
+        ##################################################
         # obtain odometry
         base_status = robot_status['base']
         x = base_status['x']
@@ -123,6 +125,8 @@ class StretchDriverNode:
         odom.twist.twist.angular.z = theta_vel
         self.odom_pub.publish(odom)
 
+        ##################################################
+        # obstain battery state
         pimu_hardware_id = self.robot.pimu.board_info['hardware_id']
         invalid_reading = float('NaN')
         v = float(robot_status['pimu']['voltage'])
@@ -157,6 +161,7 @@ class StretchDriverNode:
         elif self.charging_state == BatteryState.POWER_SUPPLY_STATUS_DISCHARGING:
             i = -1 * float(robot_status['pimu']['current'])
 
+        # publish battery state
         battery_state = BatteryState()
         battery_state.header.stamp = current_time
         battery_state.voltage = v
@@ -174,14 +179,29 @@ class StretchDriverNode:
             battery_state.present = robot_status['pimu']['charger_connected']
         self.power_pub.publish(battery_state)
 
+        ##################################################
+        # publish homed status
         calibration_status = Bool()
         calibration_status.data = self.robot.is_calibrated()
         self.calibration_pub.publish(calibration_status)
+        self.homed_pub.publish(calibration_status)
+
+        # publish runstop event
+        runstop_event = Bool()
+        runstop_event.data = robot_status['pimu']['runstop_event']
+        self.runstop_event_pub.publish(runstop_event)
 
+        # publish mode status
         mode_msg = String()
         mode_msg.data = self.robot_mode
         self.mode_pub.publish(mode_msg)
 
+        # publish end of arm tool
+        tool_msg = String()
+        tool_msg.data = self.robot.end_of_arm.name
+        self.tool_pub.publish(tool_msg)
+
+        ##################################################
         # publish joint state
         joint_state = JointState()
         joint_state.header.stamp = current_time
@@ -263,6 +283,10 @@ class StretchDriverNode:
         ##################################################
 
         self.robot_mode_rwlock.release_read()
+        # must happen after the read release, otherwise the write lock in change_mode() will cause a deadlock
+        if (self.prev_runstop_state == None and runstop_event.data) or (self.prev_runstop_state != None and runstop_event.data != self.prev_runstop_state):
+            self.runstop_the_robot(runstop_event.data, just_change_mode=True)
+        self.prev_runstop_state = runstop_event.data
 
     ######## CHANGE MODES #########
 
@@ -295,25 +319,59 @@ class StretchDriverNode:
             self.robot.base.enable_pos_incr_mode()
         self.change_mode('position', code_to_run)
 
-    def calibrate(self):
+    def home_the_robot(self):
         self.robot_mode_rwlock.acquire_read()
+        can_home = self.robot_mode in self.control_modes
         last_robot_mode = copy.copy(self.robot_mode)
         self.robot_mode_rwlock.release_read()
+        if not can_home:
+            errmsg = f'Cannot home while in mode={last_robot_mode}.'
+            rospy.logerr(errmsg)
+            return False, errmsg
         def code_to_run():
             pass
-        self.change_mode('calibration', code_to_run)
+        self.change_mode('homing', code_to_run)
         self.robot.home()
         self.change_mode(last_robot_mode, code_to_run)
+        return True, 'Homed.'
 
     def stow_the_robot(self):
         self.robot_mode_rwlock.acquire_read()
+        can_stow = self.robot_mode in self.control_modes
         last_robot_mode = copy.copy(self.robot_mode)
         self.robot_mode_rwlock.release_read()
+        if not can_stow:
+            errmsg = f'Cannot stow while in mode={last_robot_mode}.'
+            rospy.logerr(errmsg)
+            return False, errmsg
         def code_to_run():
             pass
         self.change_mode('stowing', code_to_run)
         self.robot.stow()
         self.change_mode(last_robot_mode, code_to_run)
+        return True, 'Stowed.'
+
+    def runstop_the_robot(self, runstopped, just_change_mode=False):
+        if runstopped:
+            self.robot_mode_rwlock.acquire_read()
+            already_runstopped = self.robot_mode == 'runstopped'
+            if not already_runstopped:
+                self.prerunstop_mode = copy.copy(self.robot_mode)
+            self.robot_mode_rwlock.release_read()
+            if already_runstopped:
+                return
+            self.change_mode('runstopped', lambda: None)
+            if not just_change_mode:
+                self.robot.pimu.runstop_event_trigger()
+        else:
+            self.robot_mode_rwlock.acquire_read()
+            already_not_runstopped = self.robot_mode != 'runstopped'
+            self.robot_mode_rwlock.release_read()
+            if already_not_runstopped:
+                return
+            self.change_mode(self.prerunstop_mode, lambda: None)
+            if not just_change_mode:
+                self.robot.pimu.runstop_event_reset()
 
     ######## SERVICE CALLBACKS #######
 
@@ -338,20 +396,20 @@ class StretchDriverNode:
             message='Stopped the robot.'
             )
 
-    def calibrate_callback(self, request):
-        rospy.loginfo('Received calibrate_the_robot service call.')
-        self.calibrate()
+    def home_the_robot_callback(self, request):
+        rospy.loginfo('Received home_the_robot service call.')
+        did_succeed, msg = self.home_the_robot()
         return TriggerResponse(
-            success=True,
-            message='Calibrated.'
+            success=did_succeed,
+            message=msg
         )
 
     def stow_the_robot_callback(self, request):
         rospy.loginfo('Recevied stow_the_robot service call.')
-        self.stow_the_robot()
+        did_succeed, msg = self.stow_the_robot()
         return TriggerResponse(
-            success=True,
-            message='Stowed.'
+            success=did_succeed,
+            message=msg
         )
 
     def navigation_mode_service_callback(self, request):
@@ -359,39 +417,21 @@ class StretchDriverNode:
         return TriggerResponse(
             success=True,
             message='Now in navigation mode.'
-            )
+        )
 
     def position_mode_service_callback(self, request):
         self.turn_on_position_mode()
         return TriggerResponse(
             success=True,
             message='Now in position mode.'
-            )
+        )
 
     def runstop_service_callback(self, request):
-        if request.data:
-            with self.robot_stop_lock:
-                self.stop_the_robot = True
-
-                self.robot.base.translate_by(0.0)
-                self.robot.base.rotate_by(0.0)
-                self.robot.arm.move_by(0.0)
-                self.robot.lift.move_by(0.0)
-                self.robot.push_command()
-
-                for joint in self.robot.head.joints:
-                    self.robot.head.move_by(joint, 0.0)
-                for joint in self.robot.end_of_arm.joints:
-                    self.robot.end_of_arm.move_by(joint, 0.0)
-
-            self.robot.pimu.runstop_event_trigger()
-        else:
-            self.robot.pimu.runstop_event_reset()
-
+        self.runstop_the_robot(request.data)
         return SetBoolResponse(
             success=True,
             message='is_runstopped: {0}'.format(request.data)
-            )
+        )
 
     ########### MAIN ############
 
@@ -410,8 +450,13 @@ class StretchDriverNode:
 
         self.robot = rb.Robot()
         self.robot.startup(start_non_dxl_thread=False,start_dxl_thread=True,start_sys_mon_thread=True) #Handle the non_dxl status in local loop, not thread
+        if not self.robot.is_calibrated():
+            rospy.logwarn(f'{self.node_name} robot is not homed')
 
         mode = rospy.get_param('~mode', "position")
+        if mode not in self.control_modes:
+            rospy.logwarn(f'{self.node_name} given invalid mode={mode}, using position instead')
+            mode = 'position'
         rospy.loginfo('mode = ' + str(mode))
         if mode == "position":
             self.turn_on_position_mode()
@@ -465,17 +510,20 @@ class StretchDriverNode:
 
         self.power_pub = rospy.Publisher('battery', BatteryState, queue_size=1)
         self.calibration_pub = rospy.Publisher('is_calibrated', Bool, queue_size=1)
+        self.homed_pub = rospy.Publisher('is_homed', Bool, queue_size=1)
         self.mode_pub = rospy.Publisher('mode', String, queue_size=1)
+        self.tool_pub = rospy.Publisher('tool', String, queue_size=1)
 
         self.imu_mobile_base_pub = rospy.Publisher('imu_mobile_base', Imu, queue_size=1)
         self.magnetometer_mobile_base_pub = rospy.Publisher('magnetometer_mobile_base', MagneticField, queue_size=1)
         self.imu_wrist_pub = rospy.Publisher('imu_wrist', Imu, queue_size=1)
+        self.runstop_event_pub = rospy.Publisher('is_runstopped', Bool, queue_size=1)
 
         rospy.Subscriber("cmd_vel", Twist, self.set_mobile_base_velocity_callback)
 
         # ~ symbol gets parameter from private namespace
         self.joint_state_rate = rospy.get_param('~rate', 15.0)
-        self.timeout = rospy.get_param('~timeout', 1.0)
+        self.timeout = rospy.get_param('~timeout', 0.5)
         rospy.loginfo("{0} rate = {1} Hz".format(self.node_name, self.joint_state_rate))
         rospy.loginfo("{0} timeout = {1} s".format(self.node_name, self.timeout))
 
@@ -510,9 +558,14 @@ class StretchDriverNode:
                                                     Trigger,
                                                     self.stop_the_robot_callback)
 
+        # TODO: deprecated, will be removed
         self.calibrate_the_robot_service = rospy.Service('/calibrate_the_robot',
                                                          Trigger,
-                                                         self.calibrate_callback)
+                                                         self.home_the_robot_callback)
+
+        self.home_the_robot_service = rospy.Service('/home_the_robot',
+                                                    Trigger,
+                                                    self.home_the_robot_callback)
 
         self.stow_the_robot_service = rospy.Service('/stow_the_robot',
                                                     Trigger,

stretch_demos/launch/grasp_object.launch

@@ -1,6 +1,8 @@
 <launch>
 
   <arg name="debug_directory" value="$(env HELLO_FLEET_PATH)/debug/"/>
+  <arg name="dryrun" value="false" />
+  <arg name="rviz" value="true" />
 
   <!-- REALSENSE D435i -->
   <include file="$(find stretch_core)/launch/d435i_high_resolution.launch"></include>
@@ -9,7 +11,7 @@
     <param name="initial_mode" type="string" value="High Accuracy"/>
   </node>
   <!-- -->
-  
+
   <!-- STRETCH DRIVER -->
   <param name="/stretch_driver/broadcast_odom_tf" type="bool" value="false"/>
   <param name="/stretch_driver/fail_out_of_range_goal" type="bool" value="false"/>
@@ -25,19 +27,28 @@
   <!-- LASER RANGE FINDER -->
   <include file="$(find stretch_core)/launch/rplidar.launch" />
   <!-- -->
-  
+
   <!-- LASER SCAN MATCHER FOR ODOMETRY -->
   <include file="$(find stretch_core)/launch/stretch_scan_matcher.launch" />
   <!-- -->
 
+  <!-- ALIGNED GRIPPER LINK -->
+  <node name="aligned_gripper_link_tf_publisher" pkg="tf" type="static_transform_publisher" args="0 0 0 -0.5 0.4999968 0.5 0.5000032 /link_straight_gripper /link_straight_gripper_aligned 100" />
+  <!-- -->
+
   <!-- GRASP OBJECT -->
   <node name="grasp_object" pkg="stretch_demos" type="grasp_object" output="screen">
     <param name="debug_directory" type="string" value="$(arg debug_directory)"/>
+    <param name="dryrun" type="bool" value="$(arg dryrun)"/>
   </node>
   <!-- -->
-    
+
   <!-- KEYBOARD TELEOP -->
   <node name="keyboard_teleop" pkg="stretch_core" type="keyboard_teleop" output="screen" args='--mapping_on --grasp_object_on'/>
   <!-- -->
-    
+
+  <!-- VISUALIZE -->
+  <node name="rviz" pkg="rviz" type="rviz" output="screen" args="-d $(find stretch_demos)/rviz/grasp_object.rviz" if="$(arg rviz)" />
+  <!-- -->
+
 </launch>

stretch_demos/nodes/grasp_object

@@ -41,7 +41,7 @@ class GraspObjectNode(hm.HelloNode):
         self.lift_position = None
         self.manipulation_view = None
         self.debug_directory = None
-        
+
     def joint_states_callback(self, joint_states):
         with self.joint_states_lock: 
             self.joint_states = joint_states
@@ -50,23 +50,15 @@ class GraspObjectNode(hm.HelloNode):
         lift_position, lift_velocity, lift_effort = hm.get_lift_state(joint_states)
         self.lift_position = lift_position
         self.left_finger_position, temp1, temp2 = hm.get_left_finger_state(joint_states)
-        
+
     def lower_tool_until_contact(self):
         rospy.loginfo('lower_tool_until_contact')
         trigger_request = TriggerRequest() 
         trigger_result = self.trigger_lower_until_contact_service(trigger_request)
         rospy.loginfo('trigger_result = {0}'.format(trigger_result))
-        
-    def move_to_initial_configuration(self):
-        initial_pose = {'wrist_extension': 0.01,
-                        'joint_wrist_yaw': 0.0,
-                        'gripper_aperture': 0.125}
-
-        rospy.loginfo('Move to the initial configuration for drawer opening.')
-        self.move_to_pose(initial_pose)
 
     def look_at_surface(self, scan_time_s=None):
-        self.manipulation_view = mp.ManipulationView(self.tf2_buffer, self.debug_directory)
+        self.manipulation_view = mp.ManipulationView(self.tf2_buffer, self.debug_directory, self.get_tool())
         manip = self.manipulation_view
         head_settle_time_s = 0.02 #1.0
         manip.move_head(self.move_to_pose)
@@ -95,29 +87,23 @@ class GraspObjectNode(hm.HelloNode):
             at_goal = self.move_base.backward(forward_m, detect_obstacles=False, tolerance_distance_m=tolerance_distance_m)
         
     def trigger_grasp_object_callback(self, request):
-        actually_move = True
         max_lift_m = 1.09
         min_extension_m = 0.01
         max_extension_m = 0.5
-        
+
+        # 1. Initial configuration
         use_default_mode = False
-        if use_default_mode: 
+        if use_default_mode:
             # Set the D435i to Default mode for obstacle detection
-            trigger_request = TriggerRequest() 
+            trigger_request = TriggerRequest()
             trigger_result = self.trigger_d435i_default_mode_service(trigger_request)
             rospy.loginfo('trigger_result = {0}'.format(trigger_result))
 
-        if actually_move:
-            rospy.loginfo('Retract the tool.')
-            pose = {'wrist_extension': 0.01}
-            self.move_to_pose(pose)
+        rospy.loginfo('Stow the arm.')
+        self.stow_the_robot()
 
-            rospy.loginfo('Reorient the wrist.')
-            pose = {'joint_wrist_yaw': 0.0}
-            self.move_to_pose(pose)
-            
-        self.look_at_surface(scan_time_s = 3.0)
-        
+        # 2. Scan surface and find grasp target
+        self.look_at_surface(scan_time_s = 4.0)
         grasp_target = self.manipulation_view.get_grasp_target(self.tf2_buffer)
         if grasp_target is None:
             return TriggerResponse(
@@ -125,113 +111,106 @@ class GraspObjectNode(hm.HelloNode):
                 message='Failed to find grasp target'
             )
 
+        # 3. Move to pregrasp pose
         pregrasp_lift_m = self.manipulation_view.get_pregrasp_lift(grasp_target, self.tf2_buffer)
-
+        if self.tool == "tool_stretch_dex_wrist":
+            pregrasp_lift_m += 0.02
         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}
+        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 self.tool == "tool_stretch_dex_wrist":
+            rospy.loginfo('Rotate pitch/roll for grasping.')
+            pose = {'joint_wrist_pitch': -0.3, 'joint_wrist_roll': 0.0}
             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)))
+        rospy.loginfo('pregrasp_yaw = {0:.2f} rad'.format(pregrasp_yaw))
+        rospy.loginfo('pregrasp_yaw = {0:.2f} deg'.format(pregrasp_yaw * (180.0/np.pi)))
+        rospy.loginfo('Rotate the gripper for grasping.')
+        pose = {'joint_wrist_yaw': pregrasp_yaw}
+        self.move_to_pose(pose)
 
-        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)
+        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)
+        rospy.loginfo('pregrasp_mobile_base_m = {0:.3f} m'.format(pregrasp_mobile_base_m))
+        rospy.loginfo('pregrasp_wrist_extension_m = {0:.3f} m'.format(pregrasp_wrist_extension_m))
+        rospy.loginfo('Drive to pregrasp location.')
+        self.drive(pregrasp_mobile_base_m)
 
-        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)
+        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}
-            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}
+            rospy.loginfo('Extend tool above surface.')
+            pose = {'wrist_extension': extension_m}
             self.move_to_pose(pose)
+        else:
+            rospy.loginfo('negative wrist extension for pregrasp, so not extending or retracting.')
 
-            rospy.loginfo('Reorient the wrist.')
-            pose = {'joint_wrist_yaw': 0.0}
-            self.move_to_pose(pose)
+        # 4. Grasp the object and lift it
+        grasp_mobile_base_m, grasp_lift_m, grasp_wrist_extension_m = self.manipulation_view.get_grasp_from_pregrasp(grasp_target, self.tf2_buffer)
+        rospy.loginfo('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('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)
+        extension_m = min(extension_m, max_extension_m)
+        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)
+
+        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,
             message='Completed object grasp!'
-            )
+        )
 
-    
     def main(self):
         hm.HelloNode.main(self, 'grasp_object', 'grasp_object', wait_for_first_pointcloud=False)
 
         self.debug_directory = rospy.get_param('~debug_directory')
         rospy.loginfo('Using the following directory for debugging files: {0}'.format(self.debug_directory))
-
+        self.dryrun = rospy.get_param('~dryrun', False)
 
         self.joint_states_subscriber = rospy.Subscriber('/stretch/joint_states', JointState, self.joint_states_callback)
         

stretch_demos/rviz/grasp_object.rviz

@@ -0,0 +1,582 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded: ~
+      Splitter Ratio: 0.5
+    Tree Height: 1079
+  - 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
+    Name: Time
+    SyncMode: 0
+    SyncSource: PointCloud2
+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: 0.5
+      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_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_straight_gripper:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_wrist_pitch:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_wrist_roll:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_wrist_yaw:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_wrist_yaw_bottom:
+          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
+    - Class: rviz/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: false
+        base_link:
+          Value: false
+        camera_accel_frame:
+          Value: false
+        camera_accel_optical_frame:
+          Value: false
+        camera_aligned_depth_to_color_frame:
+          Value: false
+        camera_bottom_screw_frame:
+          Value: false
+        camera_color_frame:
+          Value: false
+        camera_color_optical_frame:
+          Value: false
+        camera_depth_frame:
+          Value: false
+        camera_depth_optical_frame:
+          Value: false
+        camera_gyro_frame:
+          Value: false
+        camera_gyro_optical_frame:
+          Value: false
+        camera_infra1_frame:
+          Value: false
+        camera_infra1_optical_frame:
+          Value: false
+        camera_infra2_frame:
+          Value: false
+        camera_infra2_optical_frame:
+          Value: false
+        camera_link:
+          Value: false
+        caster_link:
+          Value: false
+        laser:
+          Value: false
+        link_arm_l0:
+          Value: false
+        link_arm_l1:
+          Value: false
+        link_arm_l2:
+          Value: false
+        link_arm_l3:
+          Value: false
+        link_arm_l4:
+          Value: false
+        link_aruco_inner_wrist:
+          Value: false
+        link_aruco_left_base:
+          Value: false
+        link_aruco_right_base:
+          Value: false
+        link_aruco_shoulder:
+          Value: false
+        link_aruco_top_wrist:
+          Value: false
+        link_grasp_center:
+          Value: false
+        link_gripper_finger_left:
+          Value: false
+        link_gripper_finger_right:
+          Value: false
+        link_gripper_fingertip_left:
+          Value: false
+        link_gripper_fingertip_right:
+          Value: false
+        link_head:
+          Value: false
+        link_head_pan:
+          Value: false
+        link_head_tilt:
+          Value: false
+        link_left_wheel:
+          Value: false
+        link_lift:
+          Value: false
+        link_mast:
+          Value: false
+        link_right_wheel:
+          Value: false
+        link_straight_gripper:
+          Value: false
+        link_straight_gripper_aligned:
+          Value: false
+        link_wrist_pitch:
+          Value: false
+        link_wrist_roll:
+          Value: false
+        link_wrist_yaw:
+          Value: false
+        link_wrist_yaw_bottom:
+          Value: false
+        map:
+          Value: false
+        odom:
+          Value: false
+        respeaker_base:
+          Value: false
+      Marker Alpha: 1
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        map:
+          odom:
+            base_link:
+              caster_link:
+                {}
+              laser:
+                {}
+              link_aruco_left_base:
+                {}
+              link_aruco_right_base:
+                {}
+              link_left_wheel:
+                {}
+              link_mast:
+                link_head:
+                  link_head_pan:
+                    link_head_tilt:
+                      camera_bottom_screw_frame:
+                        camera_link:
+                          camera_accel_frame:
+                            camera_accel_optical_frame:
+                              {}
+                          camera_aligned_depth_to_color_frame:
+                            camera_color_optical_frame:
+                              {}
+                          camera_color_frame:
+                            {}
+                          camera_depth_frame:
+                            camera_depth_optical_frame:
+                              {}
+                          camera_gyro_frame:
+                            camera_gyro_optical_frame:
+                              {}
+                          camera_infra1_frame:
+                            camera_infra1_optical_frame:
+                              {}
+                          camera_infra2_frame:
+                            camera_infra2_optical_frame:
+                              {}
+                link_lift:
+                  link_arm_l4:
+                    link_arm_l3:
+                      link_arm_l2:
+                        link_arm_l1:
+                          link_arm_l0:
+                            link_aruco_inner_wrist:
+                              {}
+                            link_aruco_top_wrist:
+                              {}
+                            link_wrist_yaw:
+                              link_wrist_yaw_bottom:
+                                link_wrist_pitch:
+                                  link_wrist_roll:
+                                    link_straight_gripper:
+                                      link_grasp_center:
+                                        {}
+                                      link_gripper_finger_left:
+                                        link_gripper_fingertip_left:
+                                          {}
+                                      link_gripper_finger_right:
+                                        link_gripper_fingertip_right:
+                                          {}
+                                      link_straight_gripper_aligned:
+                                        {}
+                  link_aruco_shoulder:
+                    {}
+                respeaker_base:
+                  {}
+              link_right_wheel:
+                {}
+      Update Interval: 0
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: RGB8
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 1
+      Size (m): 0.009999999776482582
+      Style: Points
+      Topic: /camera/depth/color/points
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/LaserScan
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Name: LaserScan
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic: /scan_filtered
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: map
+    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: 6.687999725341797
+      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
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: false
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.7853981852531433
+      Target Frame: <Fixed Frame>
+      Yaw: 0.7853981852531433
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1376
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000004c2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000004c2000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000004c2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000004c2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009b80000003efc0100000002fb0000000800540069006d00650100000000000009b8000003bc00fffffffb0000000800540069006d006501000000000000045000000000000000000000085c000004c200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 2488
+  X: 72
+  Y: 27

stretch_description/urdf/stretch_description.xacro

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

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

stretch_funmap/nodes/funmap

@@ -239,10 +239,10 @@ class FunmapNode(hm.HelloNode):
                 av_effort_threshold = 34.0
 
                 if (effort >= single_effort_threshold):
-                    rospy.loginfo('Extension single effort exceeded single_effort_threshold: {0} >= {1}'.format(
+                    rospy.logdebug('Extension single effort exceeded single_effort_threshold: {0} >= {1}'.format(
                         effort, single_effort_threshold))
                 if (av_effort >= av_effort_threshold):
-                    rospy.loginfo('Extension average effort exceeded av_effort_threshold: {0} >= {1}'.format(
+                    rospy.logdebug('Extension average effort exceeded av_effort_threshold: {0} >= {1}'.format(
                         av_effort, av_effort_threshold))
 
                 return ((effort >= single_effort_threshold) or
@@ -257,10 +257,10 @@ class FunmapNode(hm.HelloNode):
                 av_effort_threshold = 40.0
 
                 if (effort >= single_effort_threshold):
-                    rospy.loginfo('Extension single effort exceeded single_effort_threshold: {0} >= {1}'.format(
+                    rospy.logdebug('Extension single effort exceeded single_effort_threshold: {0} >= {1}'.format(
                         effort, single_effort_threshold))
                 if (av_effort >= av_effort_threshold):
-                    rospy.loginfo('Extension average effort exceeded av_effort_threshold: {0} >= {1}'.format(
+                    rospy.logdebug('Extension average effort exceeded av_effort_threshold: {0} >= {1}'.format(
                         av_effort, av_effort_threshold))
 
                 return ((effort >= single_effort_threshold) or
@@ -274,10 +274,10 @@ class FunmapNode(hm.HelloNode):
             av_effort_threshold = 20.0
 
             if (effort <= single_effort_threshold):
-                rospy.loginfo('Lift single effort less than single_effort_threshold: {0} <= {1}'.format(
+                rospy.logdebug('Lift single effort less than single_effort_threshold: {0} <= {1}'.format(
                     effort, single_effort_threshold))
             if (av_effort <= av_effort_threshold):
-                rospy.loginfo('Lift average effort less than av_effort_threshold: {0} <= {1}'.format(
+                rospy.logdebug('Lift average effort less than av_effort_threshold: {0} <= {1}'.format(
                     av_effort, av_effort_threshold))
 
             return ((effort <= single_effort_threshold) or

stretch_funmap/src/stretch_funmap/manipulation_planning.py

@@ -198,10 +198,15 @@ def detect_cliff(image, m_per_pix, m_per_height_unit, robot_xy_pix, display_text
     
 
 class ManipulationView():
-    def __init__(self, tf2_buffer, debug_directory=None):
+    def __init__(self, tf2_buffer, debug_directory=None, tool='tool_stretch_gripper'):
         self.debug_directory = debug_directory
         print('ManipulationView __init__: self.debug_directory =', self.debug_directory)
-        
+        self.gripper_links = {
+            'tool_stretch_gripper': 'link_gripper',
+            'tool_stretch_dex_wrist': 'link_straight_gripper_aligned'
+        }
+        self.tool = tool
+
         # Define the volume of interest for planning using the current
         # view.
         
@@ -420,7 +425,7 @@ class ManipulationView():
         # The planar component of the link gripper x_axis is parallel
         # to the middle of the gripper, but points in the opposite
         # direction.
-        gripper_frame = 'link_gripper'
+        gripper_frame = self.gripper_links[self.tool]
         gripper_points_to_image_mat, ip_timestamp = h.get_points_to_image_mat(gripper_frame, tf2_buffer)
         #
         # forward_direction = np.array([1.0, 0.0, 0.0])
@@ -469,7 +474,7 @@ class ManipulationView():
         # The planar component of the link gripper x_axis is parallel
         # to the middle of the gripper, but points in the opposite
         # direction.
-        gripper_frame = 'link_gripper'
+        gripper_frame = self.gripper_links[self.tool]
         gripper_points_to_image_mat, ip_timestamp = h.get_points_to_image_mat(gripper_frame, tf2_buffer)
 
         # Obtain the gripper yaw axis location in the image by
@@ -582,7 +587,7 @@ class ManipulationView():
         # The planar component of the link gripper x_axis is parallel
         # to the middle of the gripper, but points in the opposite
         # direction.
-        gripper_frame = 'link_gripper'
+        gripper_frame = self.gripper_links[self.tool]
         gripper_points_to_image_mat, ip_timestamp = h.get_points_to_image_mat(gripper_frame, tf2_buffer)
 
         # Obtain the gripper yaw axis location in the image by

stretch_gazebo/config/dex_wrist.yaml

@@ -0,0 +1,15 @@
+stretch_dex_wrist_controller:
+  type: "position_controllers/JointTrajectoryController"
+  joints:
+    - joint_wrist_pitch
+    - joint_wrist_roll
+  allow_partial_joints_goal: true
+
+  constraints:
+    goal_time: 0.6
+    stopped_velocity_tolerance: 0.05
+    joint_gripper_finger_right: {trajectory: 0.1, goal: 0.1}
+    joint_gripper_finger_left: {trajectory: 0.1, goal: 0.1}
+    stop_trajectory_duration: 0.5
+    state_publish_rate:  25
+    action_monitor_rate: 10

stretch_gazebo/launch/gazebo.launch

@@ -6,10 +6,14 @@
   <arg name="headless" default="false"/>
   <arg name="debug" default="false"/>
   <arg name="rviz" default="false"/>
-  <arg name="model" default="$(find stretch_gazebo)/urdf/stretch_gazebo.urdf.xacro"/>
   <arg name="gpu_lidar" default="false"/>
   <arg name="visualize_lidar" default="false"/>
   <arg name="world" default="worlds/empty.world"/>
+  <arg name="dex_wrist" default="false"/>
+  <param name="stretch_gazebo/dex_wrist" type="bool" value="$(arg dex_wrist)"/>
+
+  <arg name="model" value="$(find stretch_gazebo)/urdf/stretch_gazebo_standard_gripper.urdf.xacro" unless="$(arg dex_wrist)"/>
+  <arg name="model" value="$(find stretch_gazebo)/urdf/stretch_gazebo_dex_wrist.urdf.xacro" if="$(arg dex_wrist)"/>
 
   <include file="$(find gazebo_ros)/launch/empty_world.launch">
     <arg name="world_name" value="$(arg world)" />
@@ -21,7 +25,8 @@
     <arg name="verbose" value="true"/>
   </include>
 
-  <param name="robot_description" command="$(find xacro)/xacro $(arg model) gpu_lidar:=$(arg gpu_lidar) visualize_lidar:=$(arg visualize_lidar)" />
+  <param name="robot_description" command="$(find xacro)/xacro $(arg model) gpu_lidar:=$(arg gpu_lidar) visualize_lidar:=$(arg visualize_lidar)"/>
+
 
   <!-- push robot_description to factory and spawn robot in gazebo -->
   <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model"
@@ -50,8 +55,16 @@
   <rosparam command="load"
             file="$(find stretch_gazebo)/config/gripper.yaml" />
 
+  <rosparam command="load"
+        file="$(find stretch_gazebo)/config/dex_wrist.yaml" if="$(arg dex_wrist)"/>
+
+  <node name="stretch_controller_spawner" pkg="controller_manager" type="spawner"
+        args="stretch_joint_state_controller stretch_diff_drive_controller stretch_arm_controller stretch_head_controller stretch_gripper_controller"
+        unless="$(arg dex_wrist)"/>
+
   <node name="stretch_controller_spawner" pkg="controller_manager" type="spawner"
-        args="stretch_joint_state_controller stretch_diff_drive_controller stretch_arm_controller stretch_head_controller stretch_gripper_controller"/>
+        args="stretch_joint_state_controller stretch_diff_drive_controller stretch_arm_controller stretch_head_controller stretch_gripper_controller stretch_dex_wrist_controller"
+  if="$(arg dex_wrist)"/>
 
   <node name="publish_ground_truth_odom" pkg="stretch_gazebo" type="publish_ground_truth_odom.py" output="screen"/>
 

stretch_gazebo/nodes/keyboard_teleop_gazebo

@@ -180,6 +180,7 @@ class KeyboardTeleopNode:
         self.rate = 10.0
         self.joint_state = None
         self.twist = Twist()
+        self.dex_wrist_control = False
 
     def joint_states_callback(self, joint_state):
         self.joint_state = joint_state
@@ -197,8 +198,7 @@ class KeyboardTeleopNode:
             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']:
+            if joint_name in ['joint_lift', 'joint_wrist_yaw', 'joint_wrist_roll', 'joint_wrist_pitch', '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]
@@ -221,8 +221,9 @@ class KeyboardTeleopNode:
 
             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()
+            if self.trajectory_client:
+                self.trajectory_client.send_goal(trajectory_goal)
+                self.trajectory_client.wait_for_result()
 
     def trajectory_client_selector(self, command):
         self.trajectory_client = None
@@ -236,6 +237,9 @@ class KeyboardTeleopNode:
                 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 self.dex_wrist_control:
+                if joint == 'joint_wrist_roll' or joint == 'joint_wrist_pitch':
+                    self.trajectory_client = self.trajectory_dex_wrist_client
             if joint == 'translate_mobile_base' or joint == 'rotate_mobile_base':
                 if joint == 'translate_mobile_base':
                     if 'inc' in command:
@@ -252,6 +256,10 @@ class KeyboardTeleopNode:
 
     def main(self):
         rospy.init_node('keyboard_teleop_gazebo')
+        try:
+            self.dex_wrist_control = rospy.get_param('stretch_gazebo/dex_wrist', default=False)
+        except KeyError:
+            self.dex_wrist_control = False
 
         self.trajectory_gripper_client = actionlib.SimpleActionClient(
             '/stretch_gripper_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
@@ -265,6 +273,12 @@ class KeyboardTeleopNode:
                                                                   FollowJointTrajectoryAction)
         server_reached = self.trajectory_arm_client.wait_for_server(timeout=rospy.Duration(60.0))
 
+        if self.dex_wrist_control:
+            rospy.loginfo("Dex Wrist Control Activated")
+            self.trajectory_dex_wrist_client = actionlib.SimpleActionClient('/stretch_dex_wrist_controller/follow_joint_trajectory',
+                                                                      FollowJointTrajectoryAction)
+            server_reached = self.trajectory_dex_wrist_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)

stretch_gazebo/urdf/stretch_gazebo_dex_wrist.urdf.xacro

@@ -0,0 +1,634 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="stretch">
+  <xacro:arg name="gpu_lidar" default="false" />
+  <xacro:arg name="visualize_lidar" default="false" />
+
+  <xacro:include filename="$(find stretch_description)/urdf/stretch_main.xacro" />
+  <xacro:include filename="$(find stretch_description)/urdf/stretch_aruco.xacro" />
+  <xacro:include filename="$(find stretch_description)/urdf/stretch_d435i.xacro" />
+  <xacro:include filename="$(find stretch_description)/urdf/stretch_laser_range_finder.xacro" />
+  <xacro:include filename="$(find stretch_description)/urdf/stretch_respeaker.xacro" />
+  <xacro:include filename="$(find stretch_description)/urdf/stretch_dex_wrist.xacro" />
+
+  <gazebo>
+    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
+      <robotNamespace>/</robotNamespace>
+    </plugin>
+  </gazebo>
+
+  <!-- Base and Drive -->
+  <gazebo reference="base_link">
+      <material>Gazebo/Gray</material>
+  </gazebo>
+
+  <gazebo reference="link_right_wheel">
+      <mu1 value="100.0"/>
+      <mu2 value="200.0"/>
+      <kp value="10000000.0" />
+      <kd value="100.0" />
+      <material>Gazebo/Blue</material>
+      <minDepth>0.001</minDepth>
+  </gazebo>
+
+  <gazebo reference="link_left_wheel">
+      <mu1 value="100.0"/>
+      <mu2 value="200.0"/>
+      <kp value="10000000.0" />
+      <kd value="100.0" />
+      <material>Gazebo/Blue</material>
+      <minDepth>0.001</minDepth>
+  </gazebo>
+
+  <gazebo reference="caster_link">
+    <turnGravityOff>false</turnGravityOff>
+    <minDepth>0.001</minDepth>
+    <material>Gazebo/Blue</material>
+    <mu1>0.0</mu1>
+    <mu2>0.0</mu2>
+  </gazebo>
+
+  <transmission name="right_wheel_trans" type="SimpleTransmission">
+    <type>transmission_interface/SimpleTransmission</type>
+    <actuator name="right_wheel_motor">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+    <joint name="joint_right_wheel">
+      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
+    </joint>
+		</transmission>
+
+  <transmission name="left_wheel_trans" type="SimpleTransmission">
+    <type>transmission_interface/SimpleTransmission</type>
+    <actuator name="left_wheel_motor">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+    <joint name="joint_left_wheel">
+      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
+    </joint>
+		</transmission>
+
+  <!-- Sensors -->
+
+  <!-- Realsense D435i -->
+  <gazebo reference="camera_color_frame">
+    <sensor name="color" type="camera">
+      <pose frame="">0 0 0 0 0 0</pose>
+      <camera name="__default__">
+        <horizontal_fov>1.5009831567151233</horizontal_fov>
+        <image>
+          <width>640</width>
+          <height>480</height>
+          <format>RGB_INT8</format>
+        </image>
+        <clip>
+          <near>0.1</near>
+          <far>100</far>
+        </clip>
+        <noise>
+          <type>gaussian</type>
+          <mean>0.0</mean>
+          <stddev>0.007</stddev>
+        </noise>
+      </camera>
+      <always_on>1</always_on>
+      <update_rate>30</update_rate>
+      <visualize>0</visualize>
+    </sensor>
+  </gazebo>
+  <gazebo reference="camera_infra1_frame">
+    <sensor name="ired1" type="camera">
+      <pose frame="">0 0 0 0 0 0</pose>
+      <camera name="__default__">
+        <horizontal_fov>1.5009831567151233</horizontal_fov>
+        <image>
+          <width>640</width>
+          <height>480</height>
+          <format>L_INT8</format>
+        </image>
+        <clip>
+          <near>0.1</near>
+          <far>100</far>
+        </clip>
+        <noise>
+          <type>gaussian</type>
+          <mean>0.0</mean>
+          <stddev>0.007</stddev>
+        </noise>
+      </camera>
+      <always_on>1</always_on>
+      <update_rate>30</update_rate>
+      <visualize>0</visualize>
+    </sensor>
+  </gazebo>
+  <gazebo reference="camera_infra2_frame">
+    <sensor name="ired2" type="camera">
+      <pose frame="">0 0 0 0 0 0</pose>
+      <camera name="__default__">
+        <horizontal_fov>1.5009831567151233</horizontal_fov>
+        <image>
+          <width>640</width>
+          <height>480</height>
+          <format>L_INT8</format>
+        </image>
+        <clip>
+          <near>0.1</near>
+          <far>100</far>
+        </clip>
+        <noise>
+          <type>gaussian</type>
+          <mean>0.0</mean>
+          <stddev>0.007</stddev>
+        </noise>
+      </camera>
+      <always_on>1</always_on>
+      <update_rate>30</update_rate>
+      <visualize>0</visualize>
+    </sensor>
+  </gazebo>
+  <gazebo reference="camera_depth_frame">
+    <sensor name="depth" type="depth">
+      <pose frame="">0 0 0 0 0 0</pose>
+      <camera name="__default__">
+        <horizontal_fov>1.5009831567151233</horizontal_fov>
+        <image>
+          <width>640</width>
+          <height>480</height>
+        </image>
+        <clip>
+          <near>0.1</near>
+          <far>100</far>
+        </clip>
+        <noise>
+          <type>gaussian</type>
+          <mean>0.0</mean>
+          <stddev>0.007</stddev>
+        </noise>
+      </camera>
+      <always_on>1</always_on>
+      <update_rate>30</update_rate>
+      <visualize>0</visualize>
+    </sensor>
+  </gazebo>
+  <gazebo>
+      <plugin name="camera" filename="librealsense_gazebo_plugin.so">
+        <depthUpdateRate>30</depthUpdateRate>
+        <colorUpdateRate>30</colorUpdateRate>
+        <infraredUpdateRate>30</infraredUpdateRate>
+        <depthTopicName>depth/image_raw</depthTopicName>
+        <depthCameraInfoTopicName>depth/camera_info</depthCameraInfoTopicName>
+        <colorTopicName>color/image_raw</colorTopicName>
+        <colorCameraInfoTopicName>color/camera_info</colorCameraInfoTopicName>
+        <infrared1TopicName>infrared/image_raw</infrared1TopicName>
+        <infrared1CameraInfoTopicName>infrared/camera_info</infrared1CameraInfoTopicName>
+        <infrared2TopicName>infrared2/image_raw</infrared2TopicName>
+        <infrared2CameraInfoTopicName>infrared2/camera_info</infrared2CameraInfoTopicName>
+        <colorOpticalframeName>camera_color_optical_frame</colorOpticalframeName>
+        <depthOpticalframeName>camera_depth_optical_frame</depthOpticalframeName>
+        <infrared1OpticalframeName>camera_left_ir_optical_frame</infrared1OpticalframeName>
+        <infrared2OpticalframeName>camera_right_ir_optical_frame</infrared2OpticalframeName>
+        <rangeMinDepth>0.1</rangeMinDepth>
+        <rangeMaxDepth>10</rangeMaxDepth>
+        <pointCloud>1</pointCloud>
+        <pointCloudTopicName>depth/color/points</pointCloudTopicName>
+        <pointCloudCutoff>0.15</pointCloudCutoff>
+        <pointCloudCutoffMax>10</pointCloudCutoffMax>
+      </plugin>
+  </gazebo>
+
+  <gazebo reference="camera_gyro_frame">
+    <gravity>true</gravity>
+    <sensor name="imu_sensor" type="imu">
+      <always_on>true</always_on>
+      <visualize>false</visualize>
+      <plugin name="imu_plugin" filename="libgazebo_ros_imu_sensor.so">
+        <topicName>camera/imu/data</topicName>
+        <bodyName>camera_gyro_frame</bodyName>
+        <updateRateHZ>50.0</updateRateHZ>
+        <gaussianNoise>0.001</gaussianNoise>
+        <xyzOffset>0 0 0</xyzOffset>
+        <rpyOffset>0 0 0</rpyOffset>
+        <frameName>camera_gyro_frame</frameName>
+        <initialOrientationAsReference>false</initialOrientationAsReference>
+      </plugin>
+      <pose>0 0 0 0 0 0</pose>
+    </sensor>
+  </gazebo>
+
+  <!-- Aruco -->
+  <gazebo reference="link_aruco_right_base">
+      <material>Gazebo/Black</material>
+  </gazebo>
+
+  <gazebo reference="link_aruco_left_base">
+      <material>Gazebo/Black</material>
+  </gazebo>
+
+  <gazebo reference="link_aruco_shoulder">
+      <material>Gazebo/Black</material>
+  </gazebo>
+
+  <gazebo reference="link_aruco_top_wrist">
+      <material>Gazebo/Black</material>
+  </gazebo>
+
+  <gazebo reference="link_aruco_inner_wrist">
+      <material>Gazebo/Black</material>
+  </gazebo>
+
+  <!-- Respeaker -->
+  <gazebo reference="respeaker_base">
+      <material>Gazebo/Green</material>
+  </gazebo>
+
+  <!-- Non GPU LIDAR -->
+  <xacro:unless value="$(arg gpu_lidar)">
+    <gazebo reference="laser">
+      <material>Gazebo/Black</material>
+      <sensor type="ray" name="laser_sensor">
+        <pose>0 0 0 0 0 0</pose>
+        <visualize>$(arg visualize_lidar)</visualize>
+        <update_rate>5.5</update_rate>
+        <ray>
+          <scan>
+            <horizontal>
+              <samples>2000</samples>
+              <resolution>1</resolution>
+              <min_angle>-0.9</min_angle>
+              <max_angle>5.0</max_angle>
+            </horizontal>
+          </scan>
+          <range>
+            <min>0.15</min>
+            <max>12.0</max>
+            <resolution>0.01</resolution>
+          </range>
+          <noise>
+            <type>gaussian</type>
+            <mean>0.0</mean>
+            <stddev>0.001</stddev>
+          </noise>
+        </ray>
+        <plugin name="gazebo_ros_lidar_controller" filename="libgazebo_ros_laser.so">
+          <topicName>scan</topicName>
+          <frameName>laser</frameName>
+        </plugin>
+      </sensor>
+    </gazebo>
+  </xacro:unless>
+
+  <!-- GPU LIDAR -->
+  <xacro:if value="$(arg gpu_lidar)">
+    <gazebo reference="laser">
+      <material>Gazebo/Black</material>
+      <sensor type="gpu_ray" name="laser_sensor">
+        <pose>0 0 0 0 0 0</pose>
+        <visualize>$(arg visualize_lidar)</visualize>
+        <update_rate>5.5</update_rate>
+        <ray>
+          <scan>
+            <horizontal>
+              <samples>2000</samples>
+              <resolution>1</resolution>
+              <min_angle>-0.9</min_angle>
+              <max_angle>5.0</max_angle>
+            </horizontal>
+          </scan>
+          <range>
+            <min>0.15</min>
+            <max>12.0</max>
+            <resolution>0.01</resolution>
+          </range>
+          <noise>
+            <type>gaussian</type>
+            <mean>0.0</mean>
+            <stddev>0.001</stddev>
+          </noise>
+        </ray>
+        <plugin name="gazebo_ros_lidar_controller" filename="libgazebo_ros_gpu_laser.so">
+          <topicName>scan</topicName>
+          <frameName>laser</frameName>
+        </plugin>
+      </sensor>
+    </gazebo>
+  </xacro:if>
+
+  <!-- Base IMU -->
+  <gazebo reference="base_link">
+    <gravity>true</gravity>
+    <sensor name="base_imu" type="imu">
+      <always_on>true</always_on>
+      <visualize>false</visualize>
+      <plugin name="imu_plugin" filename="libgazebo_ros_imu_sensor.so">
+        <topicName>imu/data</topicName>
+        <bodyName>base_link</bodyName>
+        <updateRateHZ>25.0</updateRateHZ>
+        <gaussianNoise>0.001</gaussianNoise>
+        <xyzOffset>0 0 0</xyzOffset>
+        <rpyOffset>0 0 0</rpyOffset>
+        <frameName>base_link</frameName>
+        <initialOrientationAsReference>false</initialOrientationAsReference>
+      </plugin>
+      <pose>0 0 0 0 0 0</pose>
+    </sensor>
+  </gazebo>
+
+  <!-- Wrist IMU -->
+  <gazebo reference="link_wrist_yaw">
+    <gravity>true</gravity>
+    <sensor name="wrist_imu" type="imu">
+      <always_on>true</always_on>
+      <visualize>false</visualize>
+      <plugin name="imu_plugin" filename="libgazebo_ros_imu_sensor.so">
+        <topicName>wrist_imu/data</topicName>
+        <bodyName>link_wrist_yaw</bodyName>
+        <updateRateHZ>25.0</updateRateHZ>
+        <gaussianNoise>0.001</gaussianNoise>
+        <xyzOffset>0 0 0</xyzOffset>
+        <rpyOffset>0 0 0</rpyOffset>
+        <frameName>link_wrist_yaw</frameName>
+        <initialOrientationAsReference>false</initialOrientationAsReference>
+      </plugin>
+      <pose>0 0 0 0 0 0</pose>
+    </sensor>
+  </gazebo>
+
+  <!-- Lift -->
+  <gazebo reference="link_lift">
+      <mu1 value="10000.0"/>
+      <mu2 value="10000.0"/>
+      <kp value="10000000.0" />
+      <kd value="100.0" />
+      <material>Gazebo/Black</material>
+      <minDepth>0.001</minDepth>
+  </gazebo>
+
+  <gazebo reference="link_mast">
+      <material>Gazebo/Gray</material>
+  </gazebo>
+
+  <transmission name="trans_lift">
+    <type>transmission_interface/SimpleTransmission</type>
+    <actuator name="motor_lift">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+    <joint name="joint_lift">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+  </transmission>
+
+  <!-- Arm -->
+  <gazebo reference="link_arm_l0">
+      <mu1 value="100.0"/>
+      <mu2 value="200.0"/>
+      <kp value="10000000.0" />
+      <kd value="100.0" />
+      <material>Gazebo/Gray</material>
+      <minDepth>0.001</minDepth>
+  </gazebo>
+
+  <gazebo reference="link_arm_l1">
+      <mu1 value="100.0"/>
+      <mu2 value="200.0"/>
+      <kp value="10000000.0" />
+      <kd value="100.0" />
+      <material>Gazebo/Gray</material>
+      <minDepth>0.001</minDepth>
+  </gazebo>
+
+  <gazebo reference="link_arm_l2">
+      <mu1 value="100.0"/>
+      <mu2 value="200.0"/>
+      <kp value="10000000.0" />
+      <kd value="100.0" />
+      <material>Gazebo/Gray</material>
+      <minDepth>0.001</minDepth>
+  </gazebo>
+
+  <gazebo reference="link_arm_l3">
+      <mu1 value="100.0"/>
+      <mu2 value="200.0"/>
+      <kp value="10000000.0" />
+      <kd value="100.0" />
+      <material>Gazebo/Gray</material>
+      <minDepth>0.001</minDepth>
+  </gazebo>
+
+  <gazebo reference="link_arm_l4">
+      <mu1 value="100.0"/>
+      <mu2 value="200.0"/>
+      <kp value="10000000.0" />
+      <kd value="100.0" />
+      <material>Gazebo/Gray</material>
+      <minDepth>0.001</minDepth>
+  </gazebo>
+
+  <transmission name="trans_arm_l0">
+    <type>transmission_interface/SimpleTransmission</type>
+    <actuator name="motor_arm_l0" >
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+    <joint name="joint_arm_l0">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+  </transmission>
+  <transmission name="trans_arm_l1">
+    <type>transmission_interface/SimpleTransmission</type>
+    <actuator name="motor_arm_l1" >
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+    <joint name="joint_arm_l1">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+  </transmission>
+  <transmission name="trans_arm_l2">
+    <type>transmission_interface/SimpleTransmission</type>
+    <actuator name="motor_arm_l2" >
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+    <joint name="joint_arm_l2">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+  </transmission>
+  <transmission name="trans_arm_l3">
+    <type>transmission_interface/SimpleTransmission</type>
+    <actuator name="motor_arm_l3" >
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+    <joint name="joint_arm_l3">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+  </transmission>
+
+  <!-- Wrist -->
+  <gazebo reference="link_wrist_yaw">
+      <mu1 value="100.0"/>
+      <mu2 value="200.0"/>
+      <kp value="10000000.0" />
+      <kd value="100.0" />
+      <material>Gazebo/Black</material>
+      <minDepth>0.001</minDepth>
+  </gazebo>
+
+  <transmission name="trans_wrist_yaw">
+    <type>transmission_interface/SimpleTransmission</type>
+    <actuator name="motor_wrist_yaw" >
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+    <joint name="joint_wrist_yaw">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+  </transmission>
+
+  <!-- Head -->
+  <gazebo reference="link_head">
+      <mu1 value="100.0"/>
+      <mu2 value="200.0"/>
+      <material>Gazebo/Gray</material>
+  </gazebo>
+
+  <gazebo reference="link_head_tilt">
+      <mu1 value="100.0"/>
+      <mu2 value="200.0"/>
+      <kp value="10000000.0" />
+      <kd value="100.0" />
+      <material>Gazebo/Gray</material>
+      <minDepth>0.001</minDepth>
+  </gazebo>
+
+  <gazebo reference="link_head_pan">
+      <mu1 value="100.0"/>
+      <mu2 value="200.0"/>
+      <kp value="10000000.0" />
+      <kd value="100.0" />
+      <material>Gazebo/Gray</material>
+      <minDepth>0.001</minDepth>
+  </gazebo>
+
+  <transmission name="trans_head_pan">
+    <type>transmission_interface/SimpleTransmission</type>
+    <actuator name="motor_head_pan">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+    <joint name="joint_head_pan">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+  </transmission>
+
+  <transmission name="trans_head_tilt">
+    <type>transmission_interface/SimpleTransmission</type>
+    <actuator name="motor_head_tilt">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+    <joint name="joint_head_tilt">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+  </transmission>
+
+  <!--Dex Wrist-->
+  <gazebo reference="link_wrist_yaw_bottom">
+      <mu1 value="100.0"/>
+      <mu2 value="200.0"/>
+      <kp value="10000000.0" />
+      <kd value="100.0" />
+      <material>Gazebo/Gray</material>
+      <minDepth>0.001</minDepth>
+  </gazebo>
+  <gazebo reference="link_wrist_pitch">
+      <mu1 value="100.0"/>
+      <mu2 value="200.0"/>
+      <kp value="10000000.0" />
+      <kd value="100.0" />
+      <material>Gazebo/Gray</material>
+      <minDepth>0.001</minDepth>
+  </gazebo> 
+  <gazebo reference="link_wrist_roll">
+      <mu1 value="100.0"/>
+      <mu2 value="200.0"/>
+      <kp value="10000000.0" />
+      <kd value="100.0" />
+      <material>Gazebo/Gray</material>
+      <minDepth>0.001</minDepth>
+  </gazebo> 
+  <gazebo reference="link_straight_gripper">
+      <mu1 value="100.0"/>
+      <mu2 value="200.0"/>
+      <kp value="10000000.0" />
+      <kd value="100.0" />
+      <material>Gazebo/Gray</material>
+      <minDepth>0.001</minDepth>
+  </gazebo> 
+  <gazebo reference="link_gripper_finger_right">
+      <mu1 value="100.0"/>
+      <mu2 value="200.0"/>
+      <kp value="10000000.0" />
+      <kd value="100.0" />
+      <material>Gazebo/Gray</material>
+      <minDepth>0.001</minDepth>
+  </gazebo> 
+  <gazebo reference="link_gripper_fingertip_right">
+      <mu1 value="100.0"/>
+      <mu2 value="200.0"/>
+      <kp value="10000000.0" />
+      <kd value="100.0" />
+      <material>Gazebo/Black</material>
+      <minDepth>0.001</minDepth>
+  </gazebo> 
+  <gazebo reference="link_gripper_finger_left">
+      <mu1 value="100.0"/>
+      <mu2 value="200.0"/>
+      <kp value="10000000.0" />
+      <kd value="100.0" />
+      <material>Gazebo/Gray</material>
+      <minDepth>0.001</minDepth>
+  </gazebo> 
+  <gazebo reference="link_gripper_fingertip_left">
+      <mu1 value="100.0"/>
+      <mu2 value="200.0"/>
+      <kp value="10000000.0" />
+      <kd value="100.0" />
+      <material>Gazebo/Black</material>
+      <minDepth>0.001</minDepth>
+  </gazebo>   
+
+  <transmission name="trans_wrist_pitch">
+    <type>transmission_interface/SimpleTransmission</type>
+    <actuator name="motor_wrist_pitch">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+    <joint name="joint_wrist_pitch">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+  </transmission>
+
+  <transmission name="trans_wrist_roll">
+    <type>transmission_interface/SimpleTransmission</type>
+    <actuator name="motor_wrist_roll">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+    <joint name="joint_wrist_roll">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+  </transmission>
+
+  <!-- Gripper -->
+
+  <transmission name="trans_gripper_finger_left">
+    <type>transmission_interface/SimpleTransmission</type>
+    <actuator name="motor_gripper_finger_left">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+    <joint name="joint_gripper_finger_left">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+  </transmission>
+  <transmission name="trans_gripper_finger_right">
+    <type>transmission_interface/SimpleTransmission</type>
+    <actuator name="motor_gripper_finger_right">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+    <joint name="joint_gripper_finger_right">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+  </transmission>
+
+</robot>

stretch_gazebo/urdf/stretch_gazebo.urdf.xacro → stretch_gazebo/urdf/stretch_gazebo_standard_gripper.urdf.xacro

@@ -569,7 +569,7 @@
       <material>Gazebo/Black</material>
       <minDepth>0.001</minDepth>
   </gazebo>
-  
+
   <gazebo reference="link_gripper">
       <mu1 value="100.0"/>
       <mu2 value="200.0"/>
@@ -598,4 +598,4 @@
     </joint>
 </transmission>
 
-</robot>
+</robot>