From 052ed008ffce31ce363b1a91439f0a11d47d5ce3 Mon Sep 17 00:00:00 2001 From: hello-jackson Date: Mon, 26 Jun 2023 17:46:38 -0700 Subject: [PATCH] wip --- stretch_core/nodes/command_groups.py | 84 +++++++++++++------ stretch_core/nodes/joint_trajectory_server.py | 12 ++- 2 files changed, 69 insertions(+), 27 deletions(-) diff --git a/stretch_core/nodes/command_groups.py b/stretch_core/nodes/command_groups.py index a8502dd..9e0d387 100644 --- a/stretch_core/nodes/command_groups.py +++ b/stretch_core/nodes/command_groups.py @@ -16,6 +16,7 @@ class HeadPanCommandGroup(SimpleCommandGroup): calibrated_looked_left_offset_rad = node.controller_parameters['pan_looked_left_offset'] self.looked_left_offset_rad = calibrated_looked_left_offset_rad self.looked_left = False + def update_joint_range(self, joint_range, node=None): if joint_range is not None: @@ -32,8 +33,13 @@ class HeadPanCommandGroup(SimpleCommandGroup): def init_execution(self, robot, robot_status, **kwargs): if self.active: _, pan_error = self.update_execution(robot_status) - robot.head.move_by('head_pan', pan_error, v_r=self.goal['velocity'], a_r=self.goal['acceleration']) - self.looked_left = pan_error > 0.0 + + if not self.goal_reached(): + robot.head.move_by('head_pan', pan_error, v_r=self.goal['velocity'], a_r=self.goal['acceleration']) + self.looked_left = pan_error > 0.0 + return True + return False + return True def update_execution(self, robot_status, **kwargs): self.error = None @@ -65,6 +71,7 @@ class HeadTiltCommandGroup(SimpleCommandGroup): backlash_transition_angle_rad = node.controller_parameters['tilt_angle_backlash_transition'] self.backlash_transition_angle_rad = backlash_transition_angle_rad self.looking_up = False + def update_joint_range(self, joint_range, node=None): if joint_range is not None: @@ -81,8 +88,13 @@ class HeadTiltCommandGroup(SimpleCommandGroup): def init_execution(self, robot, robot_status, **kwargs): if self.active: _, tilt_error = self.update_execution(robot_status) - robot.head.move_by('head_tilt', tilt_error, v_r=self.goal['velocity'], a_r=self.goal['acceleration']) - self.looking_up = self.goal['position'] > (self.backlash_transition_angle_rad + self.calibrated_offset_rad) + + if not self.goal_reached(): + robot.head.move_by('head_tilt', tilt_error, v_r=self.goal['velocity'], a_r=self.goal['acceleration']) + self.looking_up = self.goal['position'] > (self.backlash_transition_angle_rad + self.calibrated_offset_rad) + return True + return False + return True def update_execution(self, robot_status, **kwargs): self.error = None @@ -104,6 +116,7 @@ class HeadTiltCommandGroup(SimpleCommandGroup): class WristYawCommandGroup(SimpleCommandGroup): def __init__(self, range_rad=None, node=None): SimpleCommandGroup.__init__(self, 'joint_wrist_yaw', range_rad, node=node) + def update_joint_range(self, joint_range, node=None): if joint_range is not None: @@ -119,10 +132,16 @@ class WristYawCommandGroup(SimpleCommandGroup): def init_execution(self, robot, robot_status, **kwargs): if self.active: - robot.end_of_arm.move_by('wrist_yaw', - self.update_execution(robot_status)[1], - v_r=self.goal['velocity'], - a_r=self.goal['acceleration']) + _, wrist_yaw_error_r = self.update_execution(robot_status) + + if not self.goal_reached(): + robot.end_of_arm.move_by('wrist_yaw', + wrist_yaw_error, + v_r=self.goal['velocity'], + a_r=self.goal['acceleration']) + return True + return False + return True def update_execution(self, robot_status, **kwargs): self.error = None @@ -143,6 +162,7 @@ class GripperCommandGroup(SimpleCommandGroup): SimpleCommandGroup.__init__(self, 'joint_gripper_finger_left', range_robotis, acceptable_joint_error=1.0, node=node) self.gripper_joint_names = ['joint_gripper_finger_left', 'joint_gripper_finger_right', 'gripper_aperture'] self.update_joint_range(range_robotis) + def update_joint_range(self, joint_range, node=None): if joint_range is not None: @@ -216,10 +236,15 @@ class GripperCommandGroup(SimpleCommandGroup): gripper_robotis_error = self.gripper_conversion.aperture_to_robotis(gripper_error) elif (self.name == 'joint_gripper_finger_left') or (self.name == 'joint_gripper_finger_right'): gripper_robotis_error = self.gripper_conversion.finger_to_robotis(gripper_error) - robot.end_of_arm.move_by('stretch_gripper', - gripper_robotis_error, - v_r=self.goal['velocity'], - a_r=self.goal['acceleration']) + + if not self.goal_reached(): + robot.end_of_arm.move_by('stretch_gripper', + gripper_robotis_error, + v_r=self.goal['velocity'], + a_r=self.goal['acceleration']) + return True + return False + return True def update_execution(self, robot_status, **kwargs): self.error = None @@ -356,13 +381,18 @@ class ArmCommandGroup(SimpleCommandGroup): def init_execution(self, robot, robot_status, **kwargs): if self.active: _, extension_error_m = self.update_execution(robot_status, force_single=True) - robot.arm.move_by(extension_error_m, - v_m=self.goal['velocity'], - a_m=self.goal['acceleration'], - contact_thresh_pos=self.goal['contact_threshold'], - contact_thresh_neg=-self.goal['contact_threshold'] \ - if self.goal['contact_threshold'] is not None else None) self.retracted = extension_error_m < 0.0 + + if not self.goal_reached(): + robot.arm.move_by(extension_error_m, + v_m=self.goal['velocity'], + a_m=self.goal['acceleration'], + contact_thresh_pos=self.goal['contact_threshold'], + contact_thresh_neg=-self.goal['contact_threshold'] \ + if self.goal['contact_threshold'] is not None else None) + return True + return False + return True def update_execution(self, robot_status, **kwargs): success_callback = kwargs['success_callback'] if 'success_callback' in kwargs.keys() else None @@ -408,12 +438,17 @@ class LiftCommandGroup(SimpleCommandGroup): def init_execution(self, robot, robot_status, **kwargs): if self.active: - robot.lift.move_by(self.update_execution(robot_status)[1], - v_m=self.goal['velocity'], - a_m=self.goal['acceleration'], - contact_thresh_pos=self.goal['contact_threshold'], - contact_thresh_neg=-self.goal['contact_threshold'] \ - if self.goal['contact_threshold'] is not None else None) + _, lift_error_m = self.update_execution(robot_status) + if not self.goal_reached(): + robot.lift.move_by(lift_error_m, + v_m=self.goal['velocity'], + a_m=self.goal['acceleration'], + contact_thresh_pos=self.goal['contact_threshold'], + contact_thresh_neg=-self.goal['contact_threshold'] \ + if self.goal['contact_threshold'] is not None else None) + return True + return False + return True def update_execution(self, robot_status, **kwargs): success_callback = kwargs['success_callback'] if 'success_callback' in kwargs.keys() else None @@ -586,6 +621,7 @@ class MobileBaseCommandGroup(SimpleCommandGroup): v_m=self.goal['velocity'], a_m=self.goal['acceleration'], contact_thresh=self.goal['contact_threshold']) + return True def update_execution(self, robot_status, **kwargs): success_callback = kwargs['success_callback'] if 'success_callback' in kwargs.keys() else None diff --git a/stretch_core/nodes/joint_trajectory_server.py b/stretch_core/nodes/joint_trajectory_server.py index 5a9a99e..d94c951 100644 --- a/stretch_core/nodes/joint_trajectory_server.py +++ b/stretch_core/nodes/joint_trajectory_server.py @@ -106,9 +106,15 @@ class JointTrajectoryAction: return robot_status = self.node.robot.get_status() # uses lock held by robot - for c in self.command_groups: - c.init_execution(self.node.robot, robot_status) - self.node.robot.push_command() + + successful_movements = [c.init_execution(self.node.robot, robot_status) for c in self.command_groups] + unsuccessful_movements_count = len(successful_movements) - sum(successful_movements) + + rospy.loginfo(("unsuccessful movements count: {0}\number of valid points: {1}").format(unsuccessful_movements_count, num_valid_points)) + + ## TODO find a count of commanded movements that is indepent of the amount of points in the trajectory goal + if unsuccessful_movements_count < num_valid_points: + self.node.robot.push_command() goals_reached = [c.goal_reached() for c in self.command_groups] update_rate = rospy.Rate(15.0)