diff --git a/stretch_core/nodes/command_groups.py b/stretch_core/nodes/command_groups.py index c9f5574..07b058b 100644 --- a/stretch_core/nodes/command_groups.py +++ b/stretch_core/nodes/command_groups.py @@ -7,30 +7,23 @@ from hello_helpers.gripper_conversion import GripperConversion class HeadPanCommandGroup(SimpleCommandGroup): - def __init__(self, range_rad, head_pan_calibrated_offset, head_pan_calibrated_looked_left_offset): + def __init__(self, range_rad, calibrated_offset_rad=0.0, calibrated_looked_left_offset_rad=0.0): 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 + self.calibrated_offset_rad = calibrated_offset_rad + self.looked_left_offset_rad = calibrated_looked_left_offset_rad + self.looked_left = False def init_execution(self, robot, robot_status, **kwargs): if self.active: - _, pan_error = self.update_execution(robot_status, backlash_state=kwargs['backlash_state']) + _, 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']) - if pan_error > 0.0: - kwargs['backlash_state']['head_pan_looked_left'] = True - else: - kwargs['backlash_state']['head_pan_looked_left'] = False + self.looked_left = pan_error > 0.0 def update_execution(self, robot_status, **kwargs): self.error = None - backlash_state = kwargs['backlash_state'] if self.active: - if backlash_state['head_pan_looked_left']: - pan_backlash_correction = self.head_pan_calibrated_looked_left_offset - else: - pan_backlash_correction = 0.0 - pan_current = robot_status['head']['head_pan']['pos'] + \ - self.head_pan_calibrated_offset + pan_backlash_correction + pan_backlash_correction = self.looked_left_offset_rad if self.looked_left else 0.0 + pan_current = robot_status['head']['head_pan']['pos'] + self.calibrated_offset_rad + pan_backlash_correction self.error = self.goal['position'] - pan_current return self.name, self.error @@ -38,33 +31,24 @@ class HeadPanCommandGroup(SimpleCommandGroup): class HeadTiltCommandGroup(SimpleCommandGroup): - def __init__(self, range_rad, head_tilt_calibrated_offset, - head_tilt_calibrated_looking_up_offset, - head_tilt_backlash_transition_angle): + def __init__(self, range_rad, calibrated_offset_rad=0.0, calibrated_looking_up_offset_rad=0.0, backlash_transition_angle_rad=-0.4): 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 + self.calibrated_offset_rad = calibrated_offset_rad + self.looking_up_offset_rad = calibrated_looking_up_offset_rad + self.backlash_transition_angle_rad = backlash_transition_angle_rad + self.looking_up = False def init_execution(self, robot, robot_status, **kwargs): if self.active: - _, tilt_error = self.update_execution(robot_status, backlash_state=kwargs['backlash_state']) + _, 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']) - if self.goal['position'] > (self.head_tilt_backlash_transition_angle + self.head_tilt_calibrated_offset): - kwargs['backlash_state']['head_tilt_looking_up'] = True - else: - kwargs['backlash_state']['head_tilt_looking_up'] = False + self.looking_up = self.goal['position'] > (self.backlash_transition_angle_rad + self.calibrated_offset_rad) def update_execution(self, robot_status, **kwargs): self.error = None - backlash_state = kwargs['backlash_state'] if self.active: - if backlash_state['head_tilt_looking_up']: - tilt_backlash_correction = self.head_tilt_calibrated_looking_up_offset - else: - tilt_backlash_correction = 0.0 - tilt_current = robot_status['head']['head_tilt']['pos'] + \ - self.head_tilt_calibrated_offset + tilt_backlash_correction + tilt_backlash_correction = self.looking_up_offset_rad if self.looking_up else 0.0 + tilt_current = robot_status['head']['head_tilt']['pos'] + self.calibrated_offset_rad + tilt_backlash_correction self.error = self.goal['position'] - tilt_current return self.name, self.error @@ -172,11 +156,13 @@ class GripperCommandGroup(SimpleCommandGroup): class TelescopingCommandGroup(SimpleCommandGroup): - def __init__(self, range_m, wrist_extension_calibrated_retracted_offset): + def __init__(self, range_m, calibrated_retracted_offset_m=0.0): + #SimpleCommandGroup.__init__(self, 'wrist_extension', range_m, acceptable_joint_error=0.005) SimpleCommandGroup.__init__(self, 'wrist_extension', range_m, acceptable_joint_error=0.008) - 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 + self.retracted_offset_m = calibrated_retracted_offset_m + self.retracted = False def get_num_valid_commands(self): if self.active and self.is_telescoping: @@ -256,30 +242,23 @@ class TelescopingCommandGroup(SimpleCommandGroup): def init_execution(self, robot, robot_status, **kwargs): if self.active: - _, extension_error_m = self.update_execution(robot_status, backlash_state=kwargs['backlash_state']) + _, extension_error_m = self.update_execution(robot_status) robot.arm.move_by(extension_error_m, v_m=self.goal['velocity'], a_m=self.goal['acceleration'], contact_thresh_pos_N=self.goal['contact_threshold'], contact_thresh_neg_N=-self.goal['contact_threshold'] \ if self.goal['contact_threshold'] is not None else None) - if extension_error_m < 0.0: - kwargs['backlash_state']['wrist_extension_retracted'] = True - else: - kwargs['backlash_state']['wrist_extension_retracted'] = False + self.retracted = extension_error_m < 0.0 def update_execution(self, robot_status, **kwargs): - backlash_state = kwargs['backlash_state'] success_callback = kwargs['success_callback'] if 'success_callback' in kwargs.keys() else None self.error = None if self.active: if success_callback and robot_status['arm']['motor']['in_guarded_event']: success_callback("{0} contact detected.".format(self.name)) return True - if backlash_state['wrist_extension_retracted']: - arm_backlash_correction = self.wrist_extension_calibrated_retracted_offset - else: - arm_backlash_correction = 0.0 + arm_backlash_correction = self.retracted_offset_m if self.retracted else 0.0 extension_current = robot_status['arm']['pos'] + arm_backlash_correction self.error = self.goal['position'] - extension_current return (self.telescoping_joints, self.error) if self.is_telescoping else (self.name, self.error) diff --git a/stretch_core/nodes/joint_trajectory_server.py b/stretch_core/nodes/joint_trajectory_server.py index 52d5d05..4ec6032 100644 --- a/stretch_core/nodes/joint_trajectory_server.py +++ b/stretch_core/nodes/joint_trajectory_server.py @@ -115,8 +115,8 @@ class JointTrajectoryAction: return 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] + for c in command_groups: + c.init_execution(self.node.robot, robot_status) self.node.robot.push_command() goals_reached = [c.goal_reached() for c in command_groups] @@ -143,8 +143,7 @@ class JointTrajectoryAction: return 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) + named_errors = [c.update_execution(robot_status, success_callback=self.success_callback) for c in command_groups] # It's not clear how this could ever happen. The # groups in command_groups.py seem to return diff --git a/stretch_core/nodes/stretch_driver b/stretch_core/nodes/stretch_driver index 082d622..c05ac5a 100755 --- a/stretch_core/nodes/stretch_driver +++ b/stretch_core/nodes/stretch_driver @@ -32,7 +32,6 @@ from joint_trajectory_server import JointTrajectoryAction from stretch_diagnostics import StretchDiagnostics GRIPPER_DEBUG = False -BACKLASH_DEBUG = False class StretchBodyNode: @@ -48,11 +47,6 @@ class StretchBodyNode: self.head_tilt_calibrated_offset_rad = 0.0 self.head_pan_calibrated_offset_rad = 0.0 - # Initialize backlash state - self.backlash_state = {'head_tilt_looking_up' : False, - 'head_pan_looked_left' : False, - 'wrist_extension_retracted' : False} - # Initialize backlash offsets self.head_pan_calibrated_looked_left_offset_rad = 0.0 self.head_tilt_calibrated_looking_up_offset_rad = 0.0 @@ -83,10 +77,6 @@ class StretchBodyNode: def command_mobile_base_velocity_and_publish_state(self): self.robot_mode_rwlock.acquire_read() - if BACKLASH_DEBUG: - print('***') - print('self.backlash_state =', self.backlash_state) - # set new mobile base velocities, if appropriate # check on thread safety for this with callback that sets velocity command values if self.robot_mode == 'navigation': @@ -129,13 +119,7 @@ class StretchBodyNode: # assign relevant arm status to variables arm_status = robot_status['arm'] - if self.backlash_state['wrist_extension_retracted']: - arm_backlash_correction = self.wrist_extension_calibrated_retracted_offset_m - else: - arm_backlash_correction = 0.0 - - if BACKLASH_DEBUG: - print('arm_backlash_correction =', arm_backlash_correction) + arm_backlash_correction = 0.0 pos_out = arm_status['pos'] + arm_backlash_correction vel_out = arm_status['vel'] eff_out = arm_status['motor']['effort'] @@ -167,24 +151,14 @@ class StretchBodyNode: if self.use_robotis_head: # assign relevant head pan status to variables head_pan_status = robot_status['head']['head_pan'] - if self.backlash_state['head_pan_looked_left']: - pan_backlash_correction = self.head_pan_calibrated_looked_left_offset_rad - else: - pan_backlash_correction = 0.0 - if BACKLASH_DEBUG: - print('pan_backlash_correction =', pan_backlash_correction) + pan_backlash_correction = 0.0 head_pan_rad = head_pan_status['pos'] + self.head_pan_calibrated_offset_rad + pan_backlash_correction head_pan_vel = head_pan_status['vel'] head_pan_effort = head_pan_status['effort'] # assign relevant head tilt status to variables head_tilt_status = robot_status['head']['head_tilt'] - if self.backlash_state['head_tilt_looking_up']: - tilt_backlash_correction = self.head_tilt_calibrated_looking_up_offset_rad - else: - tilt_backlash_correction = 0.0 - if BACKLASH_DEBUG: - print('tilt_backlash_correction =', tilt_backlash_correction) + tilt_backlash_correction = 0.0 head_tilt_rad = head_tilt_status['pos'] + self.head_tilt_calibrated_offset_rad + tilt_backlash_correction head_tilt_vel = head_tilt_status['vel'] head_tilt_effort = head_tilt_status['effort']