Merge pull request #124 from hello-robot/bugfix/eoa_cg_fix

Add Wrist_roll and wrist_pitch to command groups

stretch_core/nodes/command_groups.py

@@ -136,6 +136,77 @@ class WristYawCommandGroup(SimpleCommandGroup):
         yaw_status = robot_status['end_of_arm']['wrist_yaw']
         return (yaw_status['pos'], yaw_status['vel'], yaw_status['effort'])
 
+class WristPitchCommandGroup(SimpleCommandGroup):
+    def __init__(self, range_rad=None, node=None):
+        SimpleCommandGroup.__init__(self, 'joint_wrist_pitch', range_rad, node=node)
+
+    def update_joint_range(self, joint_range, node=None):
+        if joint_range is not None:
+            self.range = joint_range
+            return
+
+        if node is None:
+            return # cannot calculate range without Stretch Body handle
+        range_ticks = node.robot.end_of_arm.motors['wrist_pitch'].params['range_t']
+        range_rad = (node.robot.end_of_arm.motors['wrist_pitch'].ticks_to_world_rad(range_ticks[1]),
+                     node.robot.end_of_arm.motors['wrist_pitch'].ticks_to_world_rad(range_ticks[0]))
+        self.range = range_rad
+
+    def init_execution(self, robot, robot_status, **kwargs):
+        if self.active:
+            robot.end_of_arm.move_by('wrist_pitch',
+                                     self.update_execution(robot_status)[1],
+                                     v_r=self.goal['velocity'],
+                                     a_r=self.goal['acceleration'])
+
+    def update_execution(self, robot_status, **kwargs):
+        self.error = None
+        if self.active:
+            self.error = self.goal['position'] - robot_status['end_of_arm']['wrist_pitch']['pos']
+            return self.name, self.error
+
+        return None
+
+    def joint_state(self, robot_status, **kwargs):
+        pitch_status = robot_status['end_of_arm']['wrist_pitch']
+        return (pitch_status['pos'], pitch_status['vel'], pitch_status['effort'])
+    
+
+class WristRollCommandGroup(SimpleCommandGroup):
+    def __init__(self, range_rad=None, node=None):
+        SimpleCommandGroup.__init__(self, 'joint_wrist_roll', range_rad, node=node)
+
+    def update_joint_range(self, joint_range, node=None):
+        if joint_range is not None:
+            self.range = joint_range
+            return
+
+        if node is None:
+            return # cannot calculate range without Stretch Body handle
+        range_ticks = node.robot.end_of_arm.motors['wrist_roll'].params['range_t']
+        range_rad = (node.robot.end_of_arm.motors['wrist_roll'].ticks_to_world_rad(range_ticks[0]),
+                     node.robot.end_of_arm.motors['wrist_roll'].ticks_to_world_rad(range_ticks[1]))
+        self.range = range_rad
+
+    def init_execution(self, robot, robot_status, **kwargs):
+        if self.active:
+            robot.end_of_arm.move_by('wrist_roll',
+                                     self.update_execution(robot_status)[1],
+                                     v_r=self.goal['velocity'],
+                                     a_r=self.goal['acceleration'])
+
+    def update_execution(self, robot_status, **kwargs):
+        self.error = None
+        if self.active:
+            self.error = self.goal['position'] - robot_status['end_of_arm']['wrist_roll']['pos']
+            return self.name, self.error
+
+        return None
+
+    def joint_state(self, robot_status, **kwargs):
+        roll_status = robot_status['end_of_arm']['wrist_roll']
+        return (roll_status['pos'], roll_status['vel'], roll_status['effort'])
+
 
 class GripperCommandGroup(SimpleCommandGroup):
     def __init__(self, range_robotis=None, node=None):

stretch_core/nodes/joint_trajectory_server.py

@@ -12,7 +12,7 @@ from trajectory_msgs.msg import JointTrajectoryPoint
 from command_groups import HeadPanCommandGroup, HeadTiltCommandGroup, \
                            WristYawCommandGroup, GripperCommandGroup, \
                            ArmCommandGroup, LiftCommandGroup, \
-                           MobileBaseCommandGroup
+                           MobileBaseCommandGroup, WristPitchCommandGroup, WristRollCommandGroup
 
 
 class JointTrajectoryAction:
@@ -34,20 +34,19 @@ class JointTrajectoryAction:
             if 'wrist_yaw' in self.node.robot.end_of_arm.joints else None
         self.gripper_cg = GripperCommandGroup(node=self.node) \
             if 'stretch_gripper' in self.node.robot.end_of_arm.joints else None
+
+        self.wrist_pitch_cg = WristPitchCommandGroup(node=self.node) \
+            if 'wrist_pitch' in self.node.robot.end_of_arm.joints else None
+        self.wrist_roll_cg = WristRollCommandGroup(node=self.node) \
+            if 'wrist_roll' in self.node.robot.end_of_arm.joints else None
+        
         self.arm_cg = ArmCommandGroup(node=self.node)
         self.lift_cg = LiftCommandGroup(node=self.node)
         self.mobile_base_cg = MobileBaseCommandGroup(node=self.node)
         self.command_groups = [self.arm_cg, self.lift_cg, self.mobile_base_cg, self.head_pan_cg,
-                               self.head_tilt_cg, self.wrist_yaw_cg, self.gripper_cg]
+                               self.head_tilt_cg, self.wrist_yaw_cg, self.gripper_cg, self.wrist_pitch_cg, self.wrist_roll_cg]
         self.command_groups = [cg for cg in self.command_groups if cg is not None]
 
-        for joint in self.node.robot.end_of_arm.joints:
-            module_name = self.node.robot.end_of_arm.params['devices'][joint].get('ros_py_module_name')
-            class_name = self.node.robot.end_of_arm.params['devices'][joint].get('ros_py_class_name')
-            if module_name and class_name:
-                endofarm_cg = getattr(importlib.import_module(module_name), class_name)(node=self.node)
-                self.command_groups.append(endofarm_cg)
-
 
     def execute_cb(self, goal):
         with self.node.robot_stop_lock: