From f3f3096fcd0ba4857447e6ea2e6166e6b6529697 Mon Sep 17 00:00:00 2001 From: hello-binit <64861565+hello-binit@users.noreply.github.com> Date: Thu, 21 Jan 2021 04:11:48 -0500 Subject: [PATCH] First attempt at integrating traj interface with command groups --- stretch_core/stretch_core/command_groups.py | 114 ++++++++++++++++++ .../stretch_core/joint_trajectory_server.py | 102 ++++++++-------- stretch_core/stretch_core/stretch_driver.py | 10 ++ 3 files changed, 177 insertions(+), 49 deletions(-) diff --git a/stretch_core/stretch_core/command_groups.py b/stretch_core/stretch_core/command_groups.py index cdb28f9..70cf621 100644 --- a/stretch_core/stretch_core/command_groups.py +++ b/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} diff --git a/stretch_core/stretch_core/joint_trajectory_server.py b/stretch_core/stretch_core/joint_trajectory_server.py index 008d209..b097336 100644 --- a/stretch_core/stretch_core/joint_trajectory_server.py +++ b/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() diff --git a/stretch_core/stretch_core/stretch_driver.py b/stretch_core/stretch_core/stretch_driver.py index 4dbe983..a85af59 100755 --- a/stretch_core/stretch_core/stretch_driver.py +++ b/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',