Joint traj server invalidates out-of-range goals

hello_helpers/src/hello_helpers/hello_misc.py

@@ -214,7 +214,7 @@ 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(self, bounds, ros_pos, clip_ros_tolerance=1e-3):
+def bound_ros_command(bounds, ros_pos, 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.
     """

stretch_core/nodes/command_groups.py

@@ -94,14 +94,21 @@ class SimpleCommandGroup:
         """
         self.goal = {"position": None, "velocity": None, "acceleration": None, "contact_threshold": None}
         if self.active:
-            self.goal['position'] = point.positions[self.index] if len(point.positions) > self.index else None
+            goal_pos = point.positions[self.index] if len(point.positions) > self.index else None
+            if goal_pos is None:
+                err_str = ("Received goal point with positions array length={0}. "
+                           "This joint ({1})'s index is {2}. Length of array must cover all joints listed "
+                           "in commanded_joint_names.").format(len(point.positions), self.name, self.index)
+                invalid_goal_callback(err_str)
+                return False
+
+            self.goal['position'] = hm.bound_ros_command(self.range, goal_pos)
             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
             if self.goal['position'] is None:
-                err_str = ("Received goal point with positions array length={0}. "
-                           "This joint ({1})'s index is {2}. Length of array must cover all joints listed "
-                           "in commanded_joint_names.").format(len(point.positions), self.name, self.index)
+                err_str = ("Received {0} goal point that is out of bounds. "
+                            "Range = {1}, but goal point = {2}.").format(self.name, self.range, goal_pos)
                 invalid_goal_callback(err_str)
                 return False
 
@@ -157,8 +164,8 @@ class SimpleCommandGroup:
 
 
 class HeadPanCommandGroup(SimpleCommandGroup):
-    def __init__(self, head_pan_calibrated_offset, head_pan_calibrated_looked_left_offset):
-        SimpleCommandGroup.__init__(self, 'joint_head_pan', (0, 0), acceptable_joint_error=0.15)
+    def __init__(self, range_rad, head_pan_calibrated_offset, head_pan_calibrated_looked_left_offset):
+        SimpleCommandGroup.__init__(self, 'joint_head_pan', range_rad, acceptable_joint_error=0.15)
         self.head_pan_calibrated_offset = head_pan_calibrated_offset
         self.head_pan_calibrated_looked_left_offset = head_pan_calibrated_looked_left_offset
 
@@ -188,10 +195,10 @@ class HeadPanCommandGroup(SimpleCommandGroup):
 
 
 class HeadTiltCommandGroup(SimpleCommandGroup):
-    def __init__(self, head_tilt_calibrated_offset,
+    def __init__(self, range_rad, head_tilt_calibrated_offset,
                  head_tilt_calibrated_looking_up_offset,
                  head_tilt_backlash_transition_angle):
-        SimpleCommandGroup.__init__(self, 'joint_head_tilt', (0, 0), acceptable_joint_error=0.52)
+        SimpleCommandGroup.__init__(self, 'joint_head_tilt', range_rad, acceptable_joint_error=0.52)
         self.head_tilt_calibrated_offset = head_tilt_calibrated_offset
         self.head_tilt_calibrated_looking_up_offset = head_tilt_calibrated_looking_up_offset
         self.head_tilt_backlash_transition_angle = head_tilt_backlash_transition_angle
@@ -222,8 +229,8 @@ class HeadTiltCommandGroup(SimpleCommandGroup):
 
 
 class WristYawCommandGroup(SimpleCommandGroup):
-    def __init__(self):
-        SimpleCommandGroup.__init__(self, 'joint_wrist_yaw', (0, 0))
+    def __init__(self, range_rad):
+        SimpleCommandGroup.__init__(self, 'joint_wrist_yaw', range_rad)
 
     def init_execution(self, robot, robot_status, **kwargs):
         if self.active:
@@ -242,11 +249,14 @@ class WristYawCommandGroup(SimpleCommandGroup):
 
 
 class GripperCommandGroup(SimpleCommandGroup):
-    def __init__(self):
-        SimpleCommandGroup.__init__(self, None, (0, 0))
+    def __init__(self, range_robotis):
+        SimpleCommandGroup.__init__(self, None, None, acceptable_joint_error=1.0)
         self.gripper_joint_names = ['joint_gripper_finger_left', 'joint_gripper_finger_right', 'gripper_aperture']
         self.gripper_conversion = GripperConversion()
-        self.gripper_joint_goal_valid = False
+        self.range_aperture_m = (self.gripper_conversion.robotis_to_aperture(range_robotis[0]),
+                                 self.gripper_conversion.robotis_to_aperture(range_robotis[1]))
+        self.range_finger_rad = (self.gripper_conversion.robotis_to_finger(range_robotis[0]),
+                                 self.gripper_conversion.robotis_to_finger(range_robotis[1]))
 
     def update(self, commanded_joint_names, invalid_joints_callback, **kwargs):
         self.active = False
@@ -267,6 +277,30 @@ class GripperCommandGroup(SimpleCommandGroup):
 
         return True
 
+    def set_goal(self, point, invalid_goal_callback, **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
+            if goal_pos is None:
+                err_str = ("Received goal point with positions array length={0}. "
+                           "This joint ({1})'s index is {2}. Length of array must cover all joints listed "
+                           "in commanded_joint_names.").format(len(point.positions), self.name, self.index)
+                invalid_goal_callback(err_str)
+                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['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
+            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, joint_range, goal_pos)
+                invalid_goal_callback(err_str)
+                return False
+
+        return True
+
     def init_execution(self, robot, robot_status, **kwargs):
         if self.active:
             _, gripper_error = self.update_execution(robot_status)
@@ -295,8 +329,8 @@ class GripperCommandGroup(SimpleCommandGroup):
 
 
 class TelescopingCommandGroup(SimpleCommandGroup):
-    def __init__(self, wrist_extension_calibrated_retracted_offset):
-        SimpleCommandGroup.__init__(self, 'wrist_extension', (0, 0), acceptable_joint_error=0.005)
+    def __init__(self, range_m, wrist_extension_calibrated_retracted_offset):
+        SimpleCommandGroup.__init__(self, 'wrist_extension', range_m, acceptable_joint_error=0.005)
         self.wrist_extension_calibrated_retracted_offset = wrist_extension_calibrated_retracted_offset
         self.telescoping_joints = ['joint_arm_l3', 'joint_arm_l2', 'joint_arm_l1', 'joint_arm_l0']
         self.is_telescoping = False
@@ -343,8 +377,8 @@ class TelescopingCommandGroup(SimpleCommandGroup):
         self.goal = {"position": None, "velocity": None, "acceleration": None, "contact_threshold": None}
         if self.active:
             if self.is_telescoping:
-                self.goal['position'] = sum([point.positions[i] for i in self.index]) \
-                                        if len(point.positions) > max(self.index) else None
+                goal_pos = sum([point.positions[i] for i in self.index]) \
+                           if len(point.positions) > max(self.index) else None
                 self.goal['velocity'] = point.velocities[self.index[0]] \
                                         if len(point.velocities) > self.index[0] else None
                 self.goal['acceleration'] = point.accelerations[self.index[0]] \
@@ -352,8 +386,8 @@ class TelescopingCommandGroup(SimpleCommandGroup):
                 self.goal['contact_threshold'] = point.effort[self.index[0]] \
                                                  if len(point.effort) > self.index[0] else None
             else:
-                self.goal['position'] = point.positions[self.index] \
-                                        if len(point.positions) > self.index else None
+                goal_pos = point.positions[self.index] \
+                           if len(point.positions) > self.index else None
                 self.goal['velocity'] = point.velocities[self.index] \
                                         if len(point.velocities) > self.index else None
                 self.goal['acceleration'] = point.accelerations[self.index] \
@@ -361,13 +395,20 @@ class TelescopingCommandGroup(SimpleCommandGroup):
                 self.goal['contact_threshold'] = point.effort[self.index] \
                                                  if len(point.effort) > self.index else None
 
-            if self.goal['position'] is None:
+            if goal_pos is None:
                 err_str = ("Received goal point with positions array length={0}. "
                            "This joint ({1})'s index is {2}. Length of array must cover all joints listed "
                            "in commanded_joint_names.").format(len(point.positions), self.name, self.index)
                 invalid_goal_callback(err_str)
                 return False
 
+            self.goal['position'] = hm.bound_ros_command(self.range, goal_pos)
+            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)
+                invalid_goal_callback(err_str)
+                return False
+
         return True
 
     def init_execution(self, robot, robot_status, **kwargs):
@@ -400,8 +441,8 @@ class TelescopingCommandGroup(SimpleCommandGroup):
 
 
 class LiftCommandGroup(SimpleCommandGroup):
-    def __init__(self, max_arm_height):
-        SimpleCommandGroup.__init__(self, 'joint_lift', (0.0, max_arm_height))
+    def __init__(self, range_m):
+        SimpleCommandGroup.__init__(self, 'joint_lift', range_m)
 
     def init_execution(self, robot, robot_status, **kwargs):
         if self.active:
@@ -422,8 +463,8 @@ class LiftCommandGroup(SimpleCommandGroup):
 
 
 class MobileBaseCommandGroup(SimpleCommandGroup):
-    def __init__(self):
-        SimpleCommandGroup.__init__(self, 'joint_mobile_base_translation', (-0.5, 0.5),
+    def __init__(self, virtual_range_m=(-0.5, 0.5)):
+        SimpleCommandGroup.__init__(self, 'joint_mobile_base_translation', virtual_range_m,
                                     acceptable_joint_error=0.005)
         self.incrementing_joint_names = ['translate_mobile_base', 'rotate_mobile_base']
         self.active_translate_mobile_base = False
@@ -539,7 +580,7 @@ class MobileBaseCommandGroup(SimpleCommandGroup):
                 self.goal['contact_threshold'] = point.effort[self.index] if len(point.effort) > self.index else None
                 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, self.goal['position'])
+                               "Range = {1}, but goal point = {2}.").format(self.name, self.range, goal_pos)
                     invalid_goal_callback(err_str)
                     return False
 

stretch_core/nodes/joint_trajectory_server.py

@@ -25,18 +25,36 @@ class JointTrajectoryAction:
         self.feedback = FollowJointTrajectoryFeedback()
         self.result = FollowJointTrajectoryResult()
 
-        self.telescoping_cg = TelescopingCommandGroup(self.node.wrist_extension_calibrated_retracted_offset_m)
-        if self.node.use_lift:
-            self.lift_cg = LiftCommandGroup(self.node.max_arm_height)
-        self.mobile_base_cg = MobileBaseCommandGroup()
-        self.head_pan_cg = HeadPanCommandGroup(self.node.head_pan_calibrated_offset_rad,
+        r = self.node.robot
+        head_pan_range_ticks = r.head.motors['head_pan'].params['range_t']
+        head_pan_range_rad = (r.head.motors['head_pan'].ticks_to_world_rad(head_pan_range_ticks[1]),
+                              r.head.motors['head_pan'].ticks_to_world_rad(head_pan_range_ticks[0]))
+        head_tilt_range_ticks = r.head.motors['head_tilt'].params['range_t']
+        head_tilt_range_rad = (r.head.motors['head_tilt'].ticks_to_world_rad(head_tilt_range_ticks[1]),
+                               r.head.motors['head_tilt'].ticks_to_world_rad(head_tilt_range_ticks[0]))
+        wrist_yaw_range_ticks = r.end_of_arm.motors['wrist_yaw'].params['range_t']
+        wrist_yaw_range_rad = (r.end_of_arm.motors['wrist_yaw'].ticks_to_world_rad(wrist_yaw_range_ticks[1]),
+                               r.end_of_arm.motors['wrist_yaw'].ticks_to_world_rad(wrist_yaw_range_ticks[0]))
+        gripper_range_ticks = r.end_of_arm.motors['stretch_gripper'].params['range_t']
+        gripper_range_rad = (r.end_of_arm.motors['stretch_gripper'].ticks_to_world_rad(gripper_range_ticks[0]),
+                             r.end_of_arm.motors['stretch_gripper'].ticks_to_world_rad(gripper_range_ticks[1]))
+        gripper_range_robotis = (r.end_of_arm.motors['stretch_gripper'].world_rad_to_pct(gripper_range_rad[0]),
+                                 r.end_of_arm.motors['stretch_gripper'].world_rad_to_pct(gripper_range_rad[1]))
+
+        self.head_pan_cg = HeadPanCommandGroup(head_pan_range_rad,
+                                               self.node.head_pan_calibrated_offset_rad,
                                                self.node.head_pan_calibrated_looked_left_offset_rad)
-        self.head_tilt_cg = HeadTiltCommandGroup(self.node.head_tilt_calibrated_offset_rad,
+        self.head_tilt_cg = HeadTiltCommandGroup(head_tilt_range_rad,
+                                                 self.node.head_tilt_calibrated_offset_rad,
                                                  self.node.head_tilt_calibrated_looking_up_offset_rad,
                                                  self.node.head_tilt_backlash_transition_angle_rad)
-        self.wrist_yaw_cg = WristYawCommandGroup()
-        self.gripper_cg = GripperCommandGroup()
-        self.gripper_cg.acceptable_joint_error = 1.0
+        self.wrist_yaw_cg = WristYawCommandGroup(wrist_yaw_range_rad)
+        self.gripper_cg = GripperCommandGroup(gripper_range_robotis)
+        self.telescoping_cg = TelescopingCommandGroup(tuple(r.arm.params['range_m']),
+                                                      self.node.wrist_extension_calibrated_retracted_offset_m)
+        if self.node.use_lift:
+            self.lift_cg = LiftCommandGroup(tuple(r.lift.params['range_m']))
+        self.mobile_base_cg = MobileBaseCommandGroup(virtual_range_m=(-0.5, 0.5))
 
     def execute_cb(self, goal):
         with self.node.robot_stop_lock: