Make failing on out-of-range goals an optional behavior

+ Users may define out-of-range goals intentionally if they want to move any joint to its limit without knowing the joint's calibrated limit for a specific robot

hello_helpers/src/hello_helpers/hello_misc.py

@@ -226,14 +226,20 @@ def get_p1_to_p2_matrix(p1_frame_id, p2_frame_id, tf2_buffer, lookup_time=None,
         print('         exception =', e)
         return None, None
 
-def bound_ros_command(bounds, ros_pos, clip_ros_tolerance=1e-3):
+def bound_ros_command(bounds, ros_pos, fail_out_of_range_goal, clip_ros_tolerance=1e-3):
     """Clip the command with clip_ros_tolerance, instead of
     invalidating it, if it is close enough to the valid ranges.
     """
     if ros_pos < bounds[0]:
-        return bounds[0] if (bounds[0] - ros_pos) < clip_ros_tolerance else None
+        if fail_out_of_range_goal:
+            return bounds[0] if (bounds[0] - ros_pos) < clip_ros_tolerance else None
+        else:
+            return bounds[0]
 
     if ros_pos > bounds[1]:
-        return bounds[1] if (ros_pos - bounds[1]) < clip_ros_tolerance else None
+        if fail_out_of_range_goal:
+            return bounds[1] if (ros_pos - bounds[1]) < clip_ros_tolerance else None
+        else:
+            return bounds[1]
 
     return ros_pos

stretch_core/launch/keyboard_teleop.launch

@@ -2,6 +2,7 @@
 
   <!-- STRETCH DRIVER -->
   <param name="/stretch_driver/broadcast_odom_tf" type="bool" value="true"/>
+  <param name="/stretch_driver/fail_out_of_range_goal" type="bool" value="false"/>
   <include file="$(find stretch_core)/launch/stretch_driver.launch" pass_all_args="true"/>
   <!-- -->
     

stretch_core/nodes/command_groups.py

@@ -74,7 +74,7 @@ class SimpleCommandGroup:
 
         return True
 
-    def set_goal(self, point, invalid_goal_callback, **kwargs):
+    def set_goal(self, point, invalid_goal_callback, fail_out_of_range_goal, **kwargs):
         """Sets goal for the joint group
 
         Sets and validates the goal point for the joints
@@ -86,6 +86,8 @@ class SimpleCommandGroup:
             the target point for all joints
         invalid_goal_callback: func
             error callback for invalid goal
+        fail_out_of_range_goal: bool
+            whether to bound out-of-range goals to range or fail
 
         Returns
         -------
@@ -102,7 +104,7 @@ class SimpleCommandGroup:
                 invalid_goal_callback(err_str)
                 return False
 
-            self.goal['position'] = hm.bound_ros_command(self.range, goal_pos)
+            self.goal['position'] = hm.bound_ros_command(self.range, goal_pos, fail_out_of_range_goal)
             self.goal['velocity'] = point.velocities[self.index] if len(point.velocities) > self.index else None
             self.goal['acceleration'] = point.accelerations[self.index] if len(point.accelerations) > self.index else None
             self.goal['contact_threshold'] = point.effort[self.index] if len(point.effort) > self.index else None
@@ -277,7 +279,7 @@ class GripperCommandGroup(SimpleCommandGroup):
 
         return True
 
-    def set_goal(self, point, invalid_goal_callback, **kwargs):
+    def set_goal(self, point, invalid_goal_callback, fail_out_of_range_goal, **kwargs):
         self.goal = {"position": None, "velocity": None, "acceleration": None, "contact_threshold": None}
         if self.active:
             goal_pos = point.positions[self.index] if len(point.positions) > self.index else None
@@ -289,7 +291,7 @@ class GripperCommandGroup(SimpleCommandGroup):
                 return False
 
             joint_range = self.range_aperture_m if (self.name == 'gripper_aperture') else self.range_finger_rad
-            self.goal['position'] = hm.bound_ros_command(joint_range, goal_pos)
+            self.goal['position'] = hm.bound_ros_command(joint_range, goal_pos, fail_out_of_range_goal)
             self.goal['velocity'] = point.velocities[self.index] if len(point.velocities) > self.index else None
             self.goal['acceleration'] = point.accelerations[self.index] if len(point.accelerations) > self.index else None
             self.goal['contact_threshold'] = point.effort[self.index] if len(point.effort) > self.index else None
@@ -374,7 +376,7 @@ class TelescopingCommandGroup(SimpleCommandGroup):
 
         return True
 
-    def set_goal(self, point, invalid_goal_callback, **kwargs):
+    def set_goal(self, point, invalid_goal_callback, fail_out_of_range_goal, **kwargs):
         self.goal = {"position": None, "velocity": None, "acceleration": None, "contact_threshold": None}
         if self.active:
             if self.is_telescoping:
@@ -403,7 +405,7 @@ class TelescopingCommandGroup(SimpleCommandGroup):
                 invalid_goal_callback(err_str)
                 return False
 
-            self.goal['position'] = hm.bound_ros_command(self.range, goal_pos)
+            self.goal['position'] = hm.bound_ros_command(self.range, goal_pos, fail_out_of_range_goal)
             if self.goal['position'] is None:
                 err_str = ("Received {0} goal point that is out of bounds. "
                             "Range = {1}, but goal point = {2}.").format(self.name, self.range, goal_pos)
@@ -536,7 +538,7 @@ class MobileBaseCommandGroup(SimpleCommandGroup):
 
         return True
 
-    def set_goal(self, point, invalid_goal_callback, **kwargs):
+    def set_goal(self, point, invalid_goal_callback, fail_out_of_range_goal, **kwargs):
         self.goal = {"position": None, "velocity": None, "acceleration": None, "contact_threshold": None}
         self.goal_translate_mobile_base = {"position": None, "velocity": None, "acceleration": None, "contact_threshold": None}
         self.goal_rotate_mobile_base = {"position": None, "velocity": None, "acceleration": None, "contact_threshold": None}
@@ -583,7 +585,7 @@ class MobileBaseCommandGroup(SimpleCommandGroup):
                     invalid_goal_callback(err_str)
                     return False
 
-                self.goal['position'] = self.ros_to_mechaduino(goal_pos, kwargs['manipulation_origin'])
+                self.goal['position'] = self.ros_to_mechaduino(goal_pos, kwargs['manipulation_origin'], fail_out_of_range_goal)
                 self.goal['velocity'] = point.velocities[self.index] if len(point.velocities) > self.index else None
                 self.goal['acceleration'] = point.accelerations[self.index] if len(point.accelerations) > self.index else None
                 self.goal['contact_threshold'] = point.effort[self.index] if len(point.effort) > self.index else None
@@ -595,8 +597,8 @@ class MobileBaseCommandGroup(SimpleCommandGroup):
 
         return True
 
-    def ros_to_mechaduino(self, ros_ros, manipulation_origin):
-        ros_pos = hm.bound_ros_command(self.range, ros_ros)
+    def ros_to_mechaduino(self, ros_ros, manipulation_origin, fail_out_of_range_goal):
+        ros_pos = hm.bound_ros_command(self.range, ros_ros, fail_out_of_range_goal)
         return (manipulation_origin['x'] + ros_pos) if ros_pos is not None else None
 
     def init_execution(self, robot, robot_status, **kwargs):

stretch_core/nodes/joint_trajectory_server.py

@@ -101,7 +101,7 @@ class JointTrajectoryAction:
             rospy.logdebug(("{0} joint_traj action: "
                             "target point #{1} = <{2}>").format(self.node.node_name, pointi, point))
 
-            valid_goals = [c.set_goal(point, self.invalid_goal_callback,
+            valid_goals = [c.set_goal(point, self.invalid_goal_callback, self.node.fail_out_of_range_goal,
                                       manipulation_origin=self.node.mobile_base_manipulation_origin)
                            for c in command_groups]
             if not all(valid_goals):

stretch_core/nodes/stretch_driver

@@ -580,6 +580,7 @@ class StretchBodyNode:
         self.last_twist_time = rospy.get_time()
 
         # start action server for joint trajectories
+        self.fail_out_of_range_goal = rospy.get_param('~fail_out_of_range_goal', True)
         self.joint_trajectory_action = JointTrajectoryAction(self)
         self.joint_trajectory_action.server.start()