First attempt at integrating traj interface with command groups

stretch_core/stretch_core/command_groups.py

@@ -171,6 +171,21 @@ class HeadPanCommandGroup(SimpleCommandGroup):
         self.head_pan_calibrated_offset = head_pan_calibrated_offset
         self.head_pan_calibrated_looked_left_offset = head_pan_calibrated_looked_left_offset
 
+    def set_trajectory_goals(self, points, robot):
+        if self.active:
+            for waypoint in points:
+                t = waypoint.time_from_start.to_sec()
+                x = None
+                if len(waypoint.positions) > self.index:
+                    x = waypoint.positions[self.index]
+                v = None
+                if len(waypoint.velocities) > self.index:
+                    v = waypoint.velocities[self.index]
+                a = None
+                if len(waypoint.accelerations) > self.index:
+                    a = waypoint.accelerations[self.index]
+                robot.head.get_joint('head_pan').trajectory.add_waypoint(t_s=t, x_r=x, v_r=v, a_r=a)
+
     def init_execution(self, robot, robot_status, **kwargs):
         if self.active:
             _, pan_error = self.update_execution(robot_status, backlash_state=kwargs['backlash_state'])
@@ -205,6 +220,21 @@ class HeadTiltCommandGroup(SimpleCommandGroup):
         self.head_tilt_calibrated_looking_up_offset = head_tilt_calibrated_looking_up_offset
         self.head_tilt_backlash_transition_angle = head_tilt_backlash_transition_angle
 
+    def set_trajectory_goals(self, points, robot):
+        if self.active:
+            for waypoint in points:
+                t = waypoint.time_from_start.to_sec()
+                x = None
+                if len(waypoint.positions) > self.index:
+                    x = waypoint.positions[self.index]
+                v = None
+                if len(waypoint.velocities) > self.index:
+                    v = waypoint.velocities[self.index]
+                a = None
+                if len(waypoint.accelerations) > self.index:
+                    a = waypoint.accelerations[self.index]
+                robot.head.get_joint('head_tilt').trajectory.add_waypoint(t_s=t, x_r=x, v_r=v, a_r=a)
+
     def init_execution(self, robot, robot_status, **kwargs):
         if self.active:
             _, tilt_error = self.update_execution(robot_status, backlash_state=kwargs['backlash_state'])
@@ -234,6 +264,21 @@ class WristYawCommandGroup(SimpleCommandGroup):
     def __init__(self, range_rad):
         SimpleCommandGroup.__init__(self, 'joint_wrist_yaw', range_rad)
 
+    def set_trajectory_goals(self, points, robot):
+        if self.active:
+            for waypoint in points:
+                t = waypoint.time_from_start.to_sec()
+                x = None
+                if len(waypoint.positions) > self.index:
+                    x = waypoint.positions[self.index]
+                v = None
+                if len(waypoint.velocities) > self.index:
+                    v = waypoint.velocities[self.index]
+                a = None
+                if len(waypoint.accelerations) > self.index:
+                    a = waypoint.accelerations[self.index]
+                robot.end_of_arm.motors['wrist_yaw'].trajectory.add_waypoint(t_s=t, x_r=x, v_r=v, a_r=a)
+
     def init_execution(self, robot, robot_status, **kwargs):
         if self.active:
             robot.end_of_arm.move_by('wrist_yaw',
@@ -376,6 +421,21 @@ class TelescopingCommandGroup(SimpleCommandGroup):
 
         return True
 
+    def set_trajectory_goals(self, points, robot):
+        if self.active:
+            for waypoint in points:
+                t = waypoint.time_from_start.to_sec()
+                x = None
+                if len(waypoint.positions) > self.index:
+                    x = waypoint.positions[self.index]
+                v = None
+                if len(waypoint.velocities) > self.index:
+                    v = waypoint.velocities[self.index]
+                a = None
+                if len(waypoint.accelerations) > self.index:
+                    a = waypoint.accelerations[self.index]
+                robot.arm.trajectory.add_waypoint(t_s=t, x_m=x, v_m=v, a_m=a)
+
     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:
@@ -451,6 +511,21 @@ class LiftCommandGroup(SimpleCommandGroup):
     def __init__(self, range_m):
         SimpleCommandGroup.__init__(self, 'joint_lift', range_m)
 
+    def set_trajectory_goals(self, points, robot):
+        if self.active:
+            for waypoint in points:
+                t = waypoint.time_from_start.to_sec()
+                x = None
+                if len(waypoint.positions) > self.index:
+                    x = waypoint.positions[self.index]
+                v = None
+                if len(waypoint.velocities) > self.index:
+                    v = waypoint.velocities[self.index]
+                a = None
+                if len(waypoint.accelerations) > self.index:
+                    a = waypoint.accelerations[self.index]
+                robot.lift.trajectory.add_waypoint(t_s=t, x_m=x, v_m=v, a_m=a)
+
     def init_execution(self, robot, robot_status, **kwargs):
         if self.active:
             robot.lift.move_by(self.update_execution(robot_status)[1],
@@ -530,6 +605,17 @@ class MobileBaseCommandGroup(SimpleCommandGroup):
                 if 'rotate_mobile_base' in active_incrementing_joint_names:
                     self.active_rotate_mobile_base = True
                     self.index_rotate_mobile_base = commanded_joint_names.index('rotate_mobile_base')
+            elif robot_mode == 'experimental_manipulation':
+                self.active = True
+                if 'translate_mobile_base' in active_incrementing_joint_names:
+                    self.active_translate_mobile_base = True
+                    self.index_translate_mobile_base = commanded_joint_names.index('translate_mobile_base')
+                if 'rotate_mobile_base' in active_incrementing_joint_names:
+                    self.active_rotate_mobile_base = True
+                    self.index_rotate_mobile_base = commanded_joint_names.index('rotate_mobile_base')
+                if self.active_translate_mobile_base and self.active_rotate_mobile_base:
+                    err_str = ("Experimental manipulation mode cannot receive both mobile base translate and rotate joints")
+                    invalid_joints_callback(err_str)
             else:
                 err_str = ("Must be in position mode to receive a command for the {0} joint(s). "
                            "Current mode = {1}.").format(active_positioning_joint_names, robot_mode)
@@ -538,6 +624,34 @@ class MobileBaseCommandGroup(SimpleCommandGroup):
 
         return True
 
+    def set_trajectory_goals(self, points, robot):
+        if self.active_translate_mobile_base:
+            for waypoint in points:
+                t = waypoint.time_from_start.to_sec()
+                x = None
+                if len(waypoint.positions) > self.index_translate_mobile_base:
+                    x = waypoint.positions[self.index_translate_mobile_base]
+                v = None
+                if len(waypoint.velocities) > self.index_translate_mobile_base:
+                    v = waypoint.velocities[self.index_translate_mobile_base]
+                a = None
+                if len(waypoint.accelerations) > self.index_translate_mobile_base:
+                    a = waypoint.accelerations[self.index_translate_mobile_base]
+                robot.base.trajectory.add_translate_waypoint(t_s=t, x_m=x, v_m=v, a_m=a)
+        elif self.active_rotate_mobile_base:
+            for waypoint in points:
+                t = waypoint.time_from_start.to_sec()
+                x = None
+                if len(waypoint.positions) > self.index_rotate_mobile_base:
+                    x = waypoint.positions[self.index_rotate_mobile_base]
+                v = None
+                if len(waypoint.velocities) > self.index_rotate_mobile_base:
+                    v = waypoint.velocities[self.index_rotate_mobile_base]
+                a = None
+                if len(waypoint.accelerations) > self.index_rotate_mobile_base:
+                    a = waypoint.accelerations[self.index_rotate_mobile_base]
+                robot.base.trajectory.add_rotate_waypoint(t_s=t, x_r=x, v_r=v, a_r=a)
+
     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}

stretch_core/stretch_core/joint_trajectory_server.py

@@ -96,63 +96,67 @@ class JointTrajectoryAction:
             self.node.robot_mode_rwlock.release_read()
             return self.result
 
-        ###################################################
-        # Try to reach each of the goals in sequence until
-        # an error is detected or success is achieved.
-        for pointi, point in enumerate(goal.trajectory.points):
-            self.node.get_logger().debug(("{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, 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):
-                # At least one of the goals violated the requirements
-                # of a command group. Any violations should have been
-                # reported as errors by the command groups.
-                self.node.robot_mode_rwlock.release_read()
-                return self.result
-
-            robot_status = self.node.robot.get_status() # uses lock held by robot
-            [c.init_execution(self.node.robot, robot_status, backlash_state=self.node.backlash_state)
-             for c in command_groups]
-            self.node.robot.push_command()
-
-            goals_reached = [c.goal_reached() for c in command_groups]
-            goal_start_time = self.node.get_clock().now()
-
-            while not all(goals_reached):
-                if (self.node.get_clock().now() - goal_start_time) > self.node.default_goal_timeout_duration:
-                    err_str = ("Time to execute the current goal point = <{0}> exceeded the "
-                               "default_goal_timeout = {1}").format(point, self.node.default_goal_timeout_s)
-                    self.goal_tolerance_violated_callback(err_str)
+        if self.node.robot_mode == "manipulation":
+            _ = [c.set_trajectory_goals(goal.trajectory.points, self.node.robot) for c in command_groups[:-1]]
+            self.node.robot.start_trajectory()
+        else:
+            ###################################################
+            # Try to reach each of the goals in sequence until
+            # an error is detected or success is achieved.
+            for pointi, point in enumerate(goal.trajectory.points):
+                self.node.get_logger().debug(("{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, 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):
+                    # At least one of the goals violated the requirements
+                    # of a command group. Any violations should have been
+                    # reported as errors by the command groups.
                     self.node.robot_mode_rwlock.release_read()
                     return self.result
 
-                # Check if a premption request has been received.
-                with self.node.robot_stop_lock:
-                    if self.node.stop_the_robot or self.goal_handle.is_cancel_requested:
-                        self.server.set_preempted()
-                        self.node.get_logger().debug(("{0} joint_traj action: PREEMPTION REQUESTED, but not stopping "
-                                                      "current motions to allow smooth interpolation between "
-                                                      "old and new commands.").format(self.node.node_name))
-                        self.node.stop_the_robot = False
+                robot_status = self.node.robot.get_status() # uses lock held by robot
+                [c.init_execution(self.node.robot, robot_status, backlash_state=self.node.backlash_state)
+                for c in command_groups]
+                self.node.robot.push_command()
+
+                goals_reached = [c.goal_reached() for c in command_groups]
+                goal_start_time = self.node.get_clock().now()
+
+                while not all(goals_reached):
+                    if (self.node.get_clock().now() - goal_start_time) > self.node.default_goal_timeout_duration:
+                        err_str = ("Time to execute the current goal point = <{0}> exceeded the "
+                                "default_goal_timeout = {1}").format(point, self.node.default_goal_timeout_s)
+                        self.goal_tolerance_violated_callback(err_str)
                         self.node.robot_mode_rwlock.release_read()
                         return self.result
 
-                robot_status = self.node.robot.get_status()
-                named_errors = [c.update_execution(robot_status, success_callback=self.success_callback,
-                                                   backlash_state=self.node.backlash_state)
-                                for c in command_groups]
-                if any(ret == True for ret in named_errors):
-                    self.node.robot_mode_rwlock.release_read()
-                    return self.result
+                    # Check if a premption request has been received.
+                    with self.node.robot_stop_lock:
+                        if self.node.stop_the_robot or self.goal_handle.is_cancel_requested:
+                            self.server.set_preempted()
+                            self.node.get_logger().debug(("{0} joint_traj action: PREEMPTION REQUESTED, but not stopping "
+                                                        "current motions to allow smooth interpolation between "
+                                                        "old and new commands.").format(self.node.node_name))
+                            self.node.stop_the_robot = False
+                            self.node.robot_mode_rwlock.release_read()
+                            return self.result
+
+                    robot_status = self.node.robot.get_status()
+                    named_errors = [c.update_execution(robot_status, success_callback=self.success_callback,
+                                                    backlash_state=self.node.backlash_state)
+                                    for c in command_groups]
+                    if any(ret == True for ret in named_errors):
+                        self.node.robot_mode_rwlock.release_read()
+                        return self.result
 
-                # self.feedback_callback(commanded_joint_names, point, named_errors)
-                goals_reached = [c.goal_reached() for c in command_groups]
-                time.sleep(0.1)
+                    # self.feedback_callback(commanded_joint_names, point, named_errors)
+                    goals_reached = [c.goal_reached() for c in command_groups]
+                    time.sleep(0.1)
 
-            self.node.get_logger().debug("{0} joint_traj action: Achieved target point.".format(self.node.node_name))
+                self.node.get_logger().debug("{0} joint_traj action: Achieved target point.".format(self.node.node_name))
 
         self.success_callback("Achieved all target points.")
         self.node.robot_mode_rwlock.release_read()

stretch_core/stretch_core/stretch_driver.py

@@ -398,6 +398,14 @@ class StretchBodyNode(Node):
             self.mobile_base_manipulation_origin = {'x':x, 'y':y, 'theta':theta}
         self.change_mode('manipulation', code_to_run)
 
+    def turn_on_experimental_manipulation_mode(self):
+        """Utilizes the new trajectory following interface
+        built into stretch_body.
+        """
+        def code_to_run():
+            pass
+        self.change_mode('experimental_manipulation', code_to_run)
+
     def turn_on_position_mode(self):
         # Position mode enables mobile base translation and rotation
         # using position control with sequential incremental rotations
@@ -598,6 +606,8 @@ class StretchBodyNode(Node):
             self.turn_on_navigation_mode()
         elif mode == "manipulation":
             self.turn_on_manipulation_mode()
+        elif mode == "experimental_manipulation":
+            self.turn_on_experimental_manipulation_mode()
 
         self.switch_to_manipulation_mode_service = self.create_service(Trigger,
                                                                        '/switch_to_manipulation_mode',