diff --git a/hello_helpers/src/hello_helpers/simple_command_group.py b/hello_helpers/src/hello_helpers/simple_command_group.py new file mode 100644 index 0000000..9bf6a03 --- /dev/null +++ b/hello_helpers/src/hello_helpers/simple_command_group.py @@ -0,0 +1,175 @@ +import hello_helpers.hello_misc as hm + + +class SimpleCommandGroup: + def __init__(self, joint_name, joint_range, acceptable_joint_error=0.015): + """Simple command group to extend + + Attributes + ---------- + name: str + joint name + range: tuple(float) + acceptable joint bounds + active: bool + whether joint is active + index: int + index of joint's goal in point + goal: dict + components of the goal + error: float + the error between actual and desired + acceptable_joint_error: float + how close to zero the error must reach + """ + self.name = joint_name + self.range = joint_range + self.active = False + self.index = None + self.goal = {"position": None} + self.error = None + self.acceptable_joint_error = acceptable_joint_error + + def get_num_valid_commands(self): + """Returns number of active joints in the group + + Returns + ------- + int + the number of active joints within this group + """ + if self.active: + return 1 + + return 0 + + def update(self, commanded_joint_names, invalid_joints_callback, **kwargs): + """Activates joints in the group + + Checks commanded joints to activate the command + group and validates joints used correctly. + + Parameters + ---------- + commanded_joint_names: list(str) + list of commanded joints in the trajectory + invalid_joints_callback: func + error callback for misuse of joints in trajectory + + Returns + ------- + bool + False if commanded joints invalid, else True + """ + self.active = False + self.index = None + if self.name in commanded_joint_names: + self.index = commanded_joint_names.index(self.name) + self.active = True + + return True + + def set_goal(self, point, invalid_goal_callback, fail_out_of_range_goal, **kwargs): + """Sets goal for the joint group + + Sets and validates the goal point for the joints + in this command group. + + Parameters + ---------- + point: trajectory_msgs.JointTrajectoryPoint + the target point for all joints + invalid_goal_callback: func + error callback for invalid goal + fail_out_of_range_goal: bool + whether to bound out-of-range goals to range or fail + + Returns + ------- + bool + False if commanded goal invalid, else True + """ + self.goal = {"position": None, "velocity": None, "acceleration": None, "contact_threshold": None} + if self.active: + goal_pos = point.positions[self.index] if len(point.positions) > self.index else None + if goal_pos is None: + err_str = ("Received goal point with positions array length={0}. " + "This joint ({1})'s index is {2}. Length of array must cover all joints listed " + "in commanded_joint_names.").format(len(point.positions), self.name, self.index) + invalid_goal_callback(err_str) + return False + + self.goal['position'] = hm.bound_ros_command(self.range, goal_pos, fail_out_of_range_goal) + self.goal['velocity'] = point.velocities[self.index] if len(point.velocities) > self.index else None + self.goal['acceleration'] = point.accelerations[self.index] if len(point.accelerations) > self.index else None + self.goal['contact_threshold'] = point.effort[self.index] if len(point.effort) > self.index else None + if self.goal['position'] is None: + err_str = ("Received {0} goal point that is out of bounds. " + "Range = {1}, but goal point = {2}.").format(self.name, self.range, goal_pos) + invalid_goal_callback(err_str) + return False + + return True + + def init_execution(self, robot, robot_status, **kwargs): + """Starts execution of the point + + Uses Stretch's Python API to begin moving to the + target point. + + Parameters + ---------- + robot: stretch_body.robot.Robot + top-level interface to Python API + robot_status: dict + robot's current status + """ + raise NotImplementedError + + def update_execution(self, robot_status, **kwargs): + """Monitors progress of joint group + + Checks against robot's status to track progress + towards the target point. + + This method must set self.error. + + Parameters + ---------- + robot_status: dict + robot's current status + + Returns + ------- + float/None + error value if group active, else None + """ + raise NotImplementedError + + def goal_reached(self): + """Returns whether reached target point + + Returns + ------- + bool + if active, whether reached target point, else True + """ + if self.active: + return (abs(self.error) < self.acceptable_joint_error) + + return True + + def joint_state(self, robot_status, **kwargs): + """Returns state of the joint group + + Parameters + ---------- + robot_status: dict + whole robot's current status + + Returns + ------- + (float, float, float) + Current position, velocity, and effort + """ + raise NotImplementedError diff --git a/stretch_core/nodes/command_groups.py b/stretch_core/nodes/command_groups.py index d53774a..d395619 100644 --- a/stretch_core/nodes/command_groups.py +++ b/stretch_core/nodes/command_groups.py @@ -3,233 +3,70 @@ from __future__ import print_function import numpy as np import hello_helpers.hello_misc as hm +from hello_helpers.simple_command_group import SimpleCommandGroup from hello_helpers.gripper_conversion import GripperConversion -class SimpleCommandGroup: - def __init__(self, joint_name, joint_range, acceptable_joint_error=0.015): - """Simple command group to extend - - Attributes - ---------- - name: str - joint name - range: tuple(float) - acceptable joint bounds - active: bool - whether joint is active - index: int - index of joint's goal in point - goal: dict - components of the goal - error: float - the error between actual and desired - acceptable_joint_error: float - how close to zero the error must reach - """ - self.name = joint_name - self.range = joint_range - self.active = False - self.index = None - self.goal = {"position": None} - self.error = None - self.acceptable_joint_error = acceptable_joint_error - - def get_num_valid_commands(self): - """Returns number of active joints in the group - - Returns - ------- - int - the number of active joints within this group - """ - if self.active: - return 1 - - return 0 - - def update(self, commanded_joint_names, invalid_joints_callback, **kwargs): - """Activates joints in the group - - Checks commanded joints to activate the command - group and validates joints used correctly. - - Parameters - ---------- - commanded_joint_names: list(str) - list of commanded joints in the trajectory - invalid_joints_callback: func - error callback for misuse of joints in trajectory - - Returns - ------- - bool - False if commanded joints invalid, else True - """ - self.active = False - self.index = None - if self.name in commanded_joint_names: - self.index = commanded_joint_names.index(self.name) - self.active = True - - return True - - def set_goal(self, point, invalid_goal_callback, fail_out_of_range_goal, **kwargs): - """Sets goal for the joint group - - Sets and validates the goal point for the joints - in this command group. - - Parameters - ---------- - point: trajectory_msgs.JointTrajectoryPoint - the target point for all joints - invalid_goal_callback: func - error callback for invalid goal - fail_out_of_range_goal: bool - whether to bound out-of-range goals to range or fail - - Returns - ------- - bool - False if commanded goal invalid, else True - """ - self.goal = {"position": None, "velocity": None, "acceleration": None, "contact_threshold": None} - if self.active: - goal_pos = point.positions[self.index] if len(point.positions) > self.index else None - if goal_pos is None: - err_str = ("Received goal point with positions array length={0}. " - "This joint ({1})'s index is {2}. Length of array must cover all joints listed " - "in commanded_joint_names.").format(len(point.positions), self.name, self.index) - invalid_goal_callback(err_str) - return False - - self.goal['position'] = hm.bound_ros_command(self.range, goal_pos, fail_out_of_range_goal) - self.goal['velocity'] = point.velocities[self.index] if len(point.velocities) > self.index else None - self.goal['acceleration'] = point.accelerations[self.index] if len(point.accelerations) > self.index else None - self.goal['contact_threshold'] = point.effort[self.index] if len(point.effort) > self.index else None - if self.goal['position'] is None: - err_str = ("Received {0} goal point that is out of bounds. " - "Range = {1}, but goal point = {2}.").format(self.name, self.range, goal_pos) - invalid_goal_callback(err_str) - return False - - return True - - def init_execution(self, robot, robot_status, **kwargs): - """Starts execution of the point - - Uses Stretch's Python API to begin moving to the - target point. - - Parameters - ---------- - robot: stretch_body.robot.Robot - top-level interface to Python API - robot_status: dict - robot's current status - """ - raise NotImplementedError - - def update_execution(self, robot_status, **kwargs): - """Monitors progress of joint group - - Checks against robot's status to track progress - towards the target point. - - This method must set self.error. - - Parameters - ---------- - robot_status: dict - robot's current status - - Returns - ------- - float/None - error value if group active, else None - """ - raise NotImplementedError - - def goal_reached(self): - """Returns whether reached target point - - Returns - ------- - bool - if active, whether reached target point, else True - """ - if self.active: - return (abs(self.error) < self.acceptable_joint_error) - - return True - - 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 return None + def joint_state(self, robot_status, **kwargs): + pan_status = robot_status['head']['head_pan'] + pan_backlash_correction = self.looked_left_offset_rad if self.looked_left else 0.0 + pos = pan_status['pos'] + self.calibrated_offset_rad + pan_backlash_correction + return (pos, pan_status['vel'], pan_status['effort']) + 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 tilt_error > (self.head_tilt_backlash_transition_angle + self.head_tilt_calibrated_offset): - 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 return None + def joint_state(self, robot_status, **kwargs): + tilt_status = robot_status['head']['head_tilt'] + tilt_backlash_correction = self.looking_up_offset_rad if self.looking_up else 0.0 + pos = tilt_status['pos'] + self.calibrated_offset_rad + tilt_backlash_correction + return (pos, tilt_status['vel'], tilt_status['effort']) + class WristYawCommandGroup(SimpleCommandGroup): def __init__(self, range_rad): @@ -250,10 +87,14 @@ class WristYawCommandGroup(SimpleCommandGroup): return None + def joint_state(self, robot_status, **kwargs): + yaw_status = robot_status['end_of_arm']['wrist_yaw'] + return (yaw_status['pos'], yaw_status['vel'], yaw_status['effort']) + class GripperCommandGroup(SimpleCommandGroup): def __init__(self, range_robotis): - SimpleCommandGroup.__init__(self, None, None, acceptable_joint_error=1.0) + SimpleCommandGroup.__init__(self, 'joint_gripper_finger_left', None, acceptable_joint_error=1.0) self.gripper_joint_names = ['joint_gripper_finger_left', 'joint_gripper_finger_right', 'gripper_aperture'] self.gripper_conversion = GripperConversion() self.range_aperture_m = (self.gripper_conversion.robotis_to_aperture(range_robotis[0]), @@ -330,14 +171,23 @@ class GripperCommandGroup(SimpleCommandGroup): return None + def joint_state(self, robot_status, **kwargs): + joint_name = kwargs['joint_name'] if 'joint_name' in kwargs.keys() else self.name + gripper_status = robot_status['end_of_arm']['stretch_gripper'] + pos_aperture_m, pos_rad, effort, vel = self.gripper_conversion.status_to_all(gripper_status) + if (joint_name == 'gripper_aperture'): + return (pos_aperture_m, vel, effort) + elif (joint_name == 'joint_gripper_finger_left') or (joint_name == 'joint_gripper_finger_right'): + return (pos_rad, vel, effort) + -class TelescopingCommandGroup(SimpleCommandGroup): - def __init__(self, range_m, wrist_extension_calibrated_retracted_offset): - #SimpleCommandGroup.__init__(self, 'wrist_extension', range_m, acceptable_joint_error=0.005) +class ArmCommandGroup(SimpleCommandGroup): + def __init__(self, range_m, calibrated_retracted_offset_m=0.0): 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: @@ -417,36 +267,35 @@ 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) return None + def joint_state(self, robot_status, **kwargs): + arm_status = robot_status['arm'] + arm_backlash_correction = self.retracted_offset_m if self.retracted else 0.0 + pos = arm_status['pos'] + arm_backlash_correction + return (pos, arm_status['vel'], arm_status['motor']['effort']) + class LiftCommandGroup(SimpleCommandGroup): def __init__(self, range_m): @@ -473,6 +322,10 @@ class LiftCommandGroup(SimpleCommandGroup): return None + def joint_state(self, robot_status, **kwargs): + lift_status = robot_status['lift'] + return (lift_status['pos'], lift_status['vel'], lift_status['motor']['effort']) + class MobileBaseCommandGroup(SimpleCommandGroup): def __init__(self, virtual_range_m=(-0.5, 0.5)): @@ -689,3 +542,12 @@ class MobileBaseCommandGroup(SimpleCommandGroup): return (abs(self.error) < self.acceptable_joint_error) return True + + def joint_state(self, robot_status, **kwargs): + robot_mode = kwargs['robot_mode'] + manipulation_origin = kwargs['manipulation_origin'] + base_status = robot_status['base'] + if robot_mode == "manipulation": + pos = base_status['x'] - manipulation_origin['x'] + return (pos, base_status['x_vel'], base_status['effort'][0]) + return (0.0, 0.0, 0.0) diff --git a/stretch_core/nodes/joint_trajectory_server.py b/stretch_core/nodes/joint_trajectory_server.py index cc1f39a..5b93a00 100644 --- a/stretch_core/nodes/joint_trajectory_server.py +++ b/stretch_core/nodes/joint_trajectory_server.py @@ -10,7 +10,7 @@ from trajectory_msgs.msg import JointTrajectoryPoint from command_groups import HeadPanCommandGroup, HeadTiltCommandGroup, \ WristYawCommandGroup, GripperCommandGroup, \ - TelescopingCommandGroup, LiftCommandGroup, \ + ArmCommandGroup, LiftCommandGroup, \ MobileBaseCommandGroup @@ -42,18 +42,20 @@ class JointTrajectoryAction: r.end_of_arm.motors['stretch_gripper'].world_rad_to_pct(gripper_range_rad[1])) self.head_pan_cg = HeadPanCommandGroup(head_pan_range_rad, - self.node.head_pan_calibrated_offset_rad, - self.node.head_pan_calibrated_looked_left_offset_rad) + self.node.controller_parameters['pan_angle_offset'], + self.node.controller_parameters['pan_looked_left_offset']) self.head_tilt_cg = HeadTiltCommandGroup(head_tilt_range_rad, - self.node.head_tilt_calibrated_offset_rad, - self.node.head_tilt_calibrated_looking_up_offset_rad, - self.node.head_tilt_backlash_transition_angle_rad) + self.node.controller_parameters['tilt_angle_offset'], + self.node.controller_parameters['tilt_looking_up_offset'], + self.node.controller_parameters['tilt_angle_backlash_transition']) self.wrist_yaw_cg = WristYawCommandGroup(wrist_yaw_range_rad) self.gripper_cg = GripperCommandGroup(gripper_range_robotis) - self.telescoping_cg = TelescopingCommandGroup(tuple(r.arm.params['range_m']), - self.node.wrist_extension_calibrated_retracted_offset_m) + self.arm_cg = ArmCommandGroup(tuple(r.arm.params['range_m']), + self.node.controller_parameters['arm_retracted_offset']) self.lift_cg = LiftCommandGroup(tuple(r.lift.params['range_m'])) self.mobile_base_cg = MobileBaseCommandGroup(virtual_range_m=(-0.5, 0.5)) + 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] def execute_cb(self, goal): with self.node.robot_stop_lock: @@ -68,11 +70,9 @@ class JointTrajectoryAction: ################################################### # Decide what to do based on the commanded joints. - command_groups = [self.telescoping_cg, self.lift_cg, self.mobile_base_cg, self.head_pan_cg, - self.head_tilt_cg, self.wrist_yaw_cg, self.gripper_cg] updates = [c.update(commanded_joint_names, self.invalid_joints_callback, robot_mode=self.node.robot_mode) - for c in command_groups] + for c in self.command_groups] if not all(updates): # The joint names violated at least one of the command # group's requirements. The command group should have @@ -80,7 +80,7 @@ class JointTrajectoryAction: self.node.robot_mode_rwlock.release_read() return - num_valid_points = sum([c.get_num_valid_commands() for c in command_groups]) + num_valid_points = sum([c.get_num_valid_commands() for c in self.command_groups]) if num_valid_points <= 0: err_str = ("Received a command without any valid joint names." "Received joint names = {0}").format(commanded_joint_names) @@ -103,7 +103,7 @@ class JointTrajectoryAction: 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] + for c in self.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 @@ -112,11 +112,11 @@ 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 self.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] + goals_reached = [c.goal_reached() for c in self.command_groups] update_rate = rospy.Rate(15.0) goal_start_time = rospy.Time.now() @@ -140,9 +140,8 @@ 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) - for c in command_groups] + named_errors = [c.update_execution(robot_status, success_callback=self.success_callback) + for c in self.command_groups] # It's not clear how this could ever happen. The # groups in command_groups.py seem to return # (self.name, self.error) or None, rather than True. @@ -151,7 +150,7 @@ class JointTrajectoryAction: return self.feedback_callback(commanded_joint_names, point, named_errors) - goals_reached = [c.goal_reached() for c in command_groups] + goals_reached = [c.goal_reached() for c in self.command_groups] update_rate.sleep() rospy.logdebug("{0} joint_traj action: Achieved target point.".format(self.node.node_name)) diff --git a/stretch_core/nodes/stretch_driver b/stretch_core/nodes/stretch_driver index 44f1ddc..2f22209 100755 --- a/stretch_core/nodes/stretch_driver +++ b/stretch_core/nodes/stretch_driver @@ -27,13 +27,8 @@ from nav_msgs.msg import Odometry from sensor_msgs.msg import JointState, Imu, MagneticField from std_msgs.msg import Header -import hello_helpers.hello_misc as hm -from hello_helpers.gripper_conversion import GripperConversion from joint_trajectory_server import JointTrajectoryAction -GRIPPER_DEBUG = False -BACKLASH_DEBUG = False - class StretchBodyNode: @@ -44,23 +39,6 @@ class StretchBodyNode: self.default_goal_timeout_s = 10.0 self.default_goal_timeout_duration = rospy.Duration(self.default_goal_timeout_s) - # Initialize calibration offsets - 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 - self.wrist_extension_calibrated_retracted_offset_m = 0.0 - self.head_tilt_backlash_transition_angle_rad = -0.4 - - self.gripper_conversion = GripperConversion() - self.mobile_base_manipulation_origin = None #{'x':None, 'y':None, 'theta':None} self.robot_stop_lock = threading.Lock() @@ -85,47 +63,33 @@ 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 + # set new mobile base velocities if available if self.robot_mode == 'navigation': time_since_last_twist = rospy.get_time() - self.last_twist_time if time_since_last_twist < self.timeout: self.robot.base.set_velocity(self.linear_velocity_mps, self.angular_velocity_radps) self.robot.push_command() else: - # Too much information in general, although it could be blocked, since it's just INFO. + # Watchdog timer stops motion if no communication within timeout self.robot.base.set_velocity(0.0, 0.0) self.robot.push_command() - # get copy of the current robot status (uses lock held by the robot) - robot_status = self.robot.get_status() - - - # In the future, consider using time stamps from the robot's + # TODO: In the future, consider using time stamps from the robot's # motor control boards and other boards. These would need to # be synchronized with the rospy clock. #robot_time = robot_status['timestamp_pc'] - #rospy.loginfo('robot_time =', robot_time) #current_time = rospy.Time.from_sec(robot_time) - current_time = rospy.Time.now() + robot_status = self.robot.get_status() # obtain odometry - # assign relevant base status to variables base_status = robot_status['base'] x = base_status['x'] - x_raw = x y = base_status['y'] theta = base_status['theta'] + q = tf_conversions.transformations.quaternion_from_euler(0, 0, theta) x_vel = base_status['x_vel'] - x_vel_raw = x_vel - y_vel = base_status['y_vel'] x_effort = base_status['effort'][0] - x_effort_raw = x_effort theta_vel = base_status['theta_vel'] pose_time_s = base_status['pose_time_s'] @@ -134,71 +98,7 @@ class StretchBodyNode: x_vel = 0.0 x_effort = 0.0 - # 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) - pos_out = arm_status['pos'] + arm_backlash_correction - vel_out = arm_status['vel'] - eff_out = arm_status['motor']['effort'] - - lift_status = robot_status['lift'] - pos_up = lift_status['pos'] - vel_up = lift_status['vel'] - eff_up = lift_status['motor']['effort'] - - if self.use_robotis_end_of_arm: - # assign relevant wrist status to variables - wrist_status = robot_status['end_of_arm']['wrist_yaw'] - wrist_rad = wrist_status['pos'] - wrist_vel = wrist_status['vel'] - wrist_effort = wrist_status['effort'] - - # assign relevant gripper status to variables - gripper_status = robot_status['end_of_arm']['stretch_gripper'] - if GRIPPER_DEBUG: - print('-----------------------') - print('gripper_status[\'pos\'] =', gripper_status['pos']) - print('gripper_status[\'pos_pct\'] =', gripper_status['pos_pct']) - gripper_aperture_m, gripper_finger_rad, gripper_finger_effort, gripper_finger_vel = self.gripper_conversion.status_to_all(gripper_status) - if GRIPPER_DEBUG: - print('gripper_aperture_m =', gripper_aperture_m) - print('gripper_finger_rad =', gripper_finger_rad) - print('-----------------------') - - 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) - 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) - 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'] - - q = tf_conversions.transformations.quaternion_from_euler(0, 0, theta) - - if self.broadcast_odom_tf: + if self.broadcast_odom_tf: # publish odometry via TF t = TransformStamped() t.header.stamp = current_time @@ -213,7 +113,7 @@ class StretchBodyNode: t.transform.rotation.w = q[3] self.tf_broadcaster.sendTransform(t) - # publish odometry via the odom topic + # publish odometry odom = Odometry() odom.header.stamp = current_time odom.header.frame_id = self.odom_frame_id @@ -228,80 +128,36 @@ class StretchBodyNode: odom.twist.twist.angular.z = theta_vel self.odom_pub.publish(odom) - # publish joint state for the arm + # publish joint state joint_state = JointState() joint_state.header.stamp = current_time - # joint_arm_l3 is the most proximal and joint_arm_l0 is the - # most distal joint of the telescoping arm model. The joints - # are connected in series such that moving the most proximal - # joint moves all the other joints in the global frame. - joint_state.name = ['wrist_extension', 'joint_lift', 'joint_arm_l3', 'joint_arm_l2', 'joint_arm_l1', 'joint_arm_l0'] - - # set positions of the telescoping joints - positions = [pos_out/4.0 for i in xrange(4)] - # set lift position - positions.insert(0, pos_up) - # set wrist_extension position - positions.insert(0, pos_out) - - # set velocities of the telescoping joints - velocities = [vel_out/4.0 for i in xrange(4)] - # set lift velocity - velocities.insert(0, vel_up) - # set wrist_extension velocity - velocities.insert(0, vel_out) - - # set efforts of the telescoping joints - efforts = [eff_out for i in xrange(4)] - # set lift effort - efforts.insert(0, eff_up) - # set wrist_extension effort - efforts.insert(0, eff_out) - - if self.use_robotis_head: - head_joint_names = ['joint_head_pan', 'joint_head_tilt'] - joint_state.name.extend(head_joint_names) - - positions.append(head_pan_rad) - velocities.append(head_pan_vel) - efforts.append(head_pan_effort) - - positions.append(head_tilt_rad) - velocities.append(head_tilt_vel) - efforts.append(head_tilt_effort) - - if self.use_robotis_end_of_arm: - end_of_arm_joint_names = ['joint_wrist_yaw', 'joint_gripper_finger_left', 'joint_gripper_finger_right'] - joint_state.name.extend(end_of_arm_joint_names) - - positions.append(wrist_rad) - velocities.append(wrist_vel) - efforts.append(wrist_effort) - - positions.append(gripper_finger_rad) - velocities.append(gripper_finger_vel) - efforts.append(gripper_finger_effort) - - positions.append(gripper_finger_rad) - velocities.append(gripper_finger_vel) - efforts.append(gripper_finger_effort) - - # set virtual joint for mobile base translation - joint_state.name.append('joint_mobile_base_translation') - if self.robot_mode == 'manipulation': - manipulation_translation = x_raw - self.mobile_base_manipulation_origin['x'] - positions.append(manipulation_translation) - velocities.append(x_vel_raw) - efforts.append(x_effort_raw) - else: - positions.append(0.0) - velocities.append(0.0) - efforts.append(0.0) - - # set joint_state - joint_state.position = positions - joint_state.velocity = velocities - joint_state.effort = efforts + for cg in self.joint_trajectory_action.command_groups: + pos, vel, effort = cg.joint_state(robot_status, robot_mode=self.robot_mode, + manipulation_origin=self.mobile_base_manipulation_origin) + joint_state.name.append(cg.name) + joint_state.position.append(pos) + joint_state.velocity.append(vel) + joint_state.effort.append(effort) + + # add telescoping joints to joint state + arm_cg = self.joint_trajectory_action.arm_cg + joint_state.name.extend(arm_cg.telescoping_joints) + pos, vel, effort = arm_cg.joint_state(robot_status) + for _ in range(len(arm_cg.telescoping_joints)): + joint_state.position.append(pos / len(arm_cg.telescoping_joints)) + joint_state.velocity.append(vel / len(arm_cg.telescoping_joints)) + joint_state.effort.append(effort) + + # add gripper joints to joint state + gripper_cg = self.joint_trajectory_action.gripper_cg + missing_gripper_joint_names = list(set(gripper_cg.gripper_joint_names) - set(joint_state.name)) + for j in missing_gripper_joint_names: + pos, vel, effort = gripper_cg.joint_state(robot_status, joint_name=j) + joint_state.name.append(j) + joint_state.position.append(pos) + joint_state.velocity.append(vel) + joint_state.effort.append(effort) + self.joint_state_pub.publish(joint_state) ################################################## @@ -323,7 +179,6 @@ class StretchBodyNode: i.angular_velocity.x = gx i.angular_velocity.y = gy i.angular_velocity.z = gz - i.linear_acceleration.x = ax i.linear_acceleration.y = ay i.linear_acceleration.z = az @@ -508,47 +363,40 @@ class StretchBodyNode: if self.broadcast_odom_tf: self.tf_broadcaster = tf2_ros.TransformBroadcaster() - large_ang = 45.0 * np.pi/180.0 - + large_ang = np.radians(45.0) filename = rospy.get_param('~controller_calibration_file') rospy.loginfo('Loading controller calibration parameters for the head from YAML file named {0}'.format(filename)) with open(filename, 'r') as fid: - controller_parameters = yaml.safe_load(fid) - - rospy.loginfo('controller parameters loaded = {0}'.format(controller_parameters)) - - self.head_tilt_calibrated_offset_rad = controller_parameters['tilt_angle_offset'] - ang = self.head_tilt_calibrated_offset_rad - if (abs(ang) > large_ang): - rospy.loginfo('WARNING: self.head_tilt_calibrated_offset_rad HAS AN UNUSUALLY LARGE MAGNITUDE') - rospy.loginfo('self.head_tilt_calibrated_offset_rad in degrees = {0}'.format(np.degrees(self.head_tilt_calibrated_offset_rad))) - - self.head_pan_calibrated_offset_rad = controller_parameters['pan_angle_offset'] - ang = self.head_pan_calibrated_offset_rad - if (abs(ang) > large_ang): - rospy.loginfo('WARNING: self.head_pan_calibrated_offset_rad HAS AN UNUSUALLY LARGE MAGNITUDE') - rospy.loginfo('self.head_pan_calibrated_offset_rad in degrees = {0}'.format(np.degrees(self.head_pan_calibrated_offset_rad))) - - self.head_pan_calibrated_looked_left_offset_rad = controller_parameters['pan_looked_left_offset'] - ang = self.head_pan_calibrated_looked_left_offset_rad - if (abs(ang) > large_ang): - rospy.loginfo('WARNING: self.head_pan_calibrated_looked_left_offset_rad HAS AN UNUSUALLY LARGE MAGNITUDE') - rospy.loginfo('self.head_pan_calibrated_looked_left_offset_rad in degrees = {0}'.format(np.degrees(self.head_pan_calibrated_looked_left_offset_rad))) - - self.head_tilt_backlash_transition_angle_rad = controller_parameters['tilt_angle_backlash_transition'] - rospy.loginfo('self.head_tilt_backlash_transition_angle_rad in degrees = {0}'.format(np.degrees(self.head_tilt_backlash_transition_angle_rad))) - - self.head_tilt_calibrated_looking_up_offset_rad = controller_parameters['tilt_looking_up_offset'] - ang = self.head_tilt_calibrated_looking_up_offset_rad - if (abs(ang) > large_ang): - rospy.loginfo('WARNING: self.head_tilt_calibrated_looking_up_offset_rad HAS AN UNUSUALLY LARGE MAGNITUDE') - rospy.loginfo('self.head_tilt_calibrated_looking_up_offset_rad in degrees = {0}'.format(np.degrees(self.head_tilt_calibrated_looking_up_offset_rad))) - - self.wrist_extension_calibrated_retracted_offset_m = controller_parameters['arm_retracted_offset'] - m = self.wrist_extension_calibrated_retracted_offset_m - if (abs(m) > 0.05): - rospy.loginfo('WARNING: self.wrist_extension_calibrated_retracted_offset_m HAS AN UNUSUALLY LARGE MAGNITUDE') - rospy.loginfo('self.wrist_extension_calibrated_retracted_offset_m in meters = {0}'.format(self.wrist_extension_calibrated_retracted_offset_m)) + self.controller_parameters = yaml.safe_load(fid) + rospy.loginfo('controller parameters loaded = {0}'.format(self.controller_parameters)) + + head_tilt_calibrated_offset_rad = self.controller_parameters['tilt_angle_offset'] + if (abs(head_tilt_calibrated_offset_rad) > large_ang): + rospy.logwarn('WARNING: head_tilt_calibrated_offset_rad HAS AN UNUSUALLY LARGE MAGNITUDE') + rospy.loginfo('head_tilt_calibrated_offset_rad in degrees = {0}'.format(np.degrees(head_tilt_calibrated_offset_rad))) + + head_pan_calibrated_offset_rad = self.controller_parameters['pan_angle_offset'] + if (abs(head_pan_calibrated_offset_rad) > large_ang): + rospy.logwarn('WARNING: head_pan_calibrated_offset_rad HAS AN UNUSUALLY LARGE MAGNITUDE') + rospy.loginfo('head_pan_calibrated_offset_rad in degrees = {0}'.format(np.degrees(head_pan_calibrated_offset_rad))) + + head_pan_calibrated_looked_left_offset_rad = self.controller_parameters['pan_looked_left_offset'] + if (abs(head_pan_calibrated_looked_left_offset_rad) > large_ang): + rospy.logwarn('WARNING: head_pan_calibrated_looked_left_offset_rad HAS AN UNUSUALLY LARGE MAGNITUDE') + rospy.loginfo('head_pan_calibrated_looked_left_offset_rad in degrees = {0}'.format(np.degrees(head_pan_calibrated_looked_left_offset_rad))) + + head_tilt_backlash_transition_angle_rad = self.controller_parameters['tilt_angle_backlash_transition'] + rospy.loginfo('head_tilt_backlash_transition_angle_rad in degrees = {0}'.format(np.degrees(head_tilt_backlash_transition_angle_rad))) + + head_tilt_calibrated_looking_up_offset_rad = self.controller_parameters['tilt_looking_up_offset'] + if (abs(head_tilt_calibrated_looking_up_offset_rad) > large_ang): + rospy.logwarn('WARNING: head_tilt_calibrated_looking_up_offset_rad HAS AN UNUSUALLY LARGE MAGNITUDE') + rospy.loginfo('head_tilt_calibrated_looking_up_offset_rad in degrees = {0}'.format(np.degrees(head_tilt_calibrated_looking_up_offset_rad))) + + arm_calibrated_retracted_offset_m = self.controller_parameters['arm_retracted_offset'] + if (abs(arm_calibrated_retracted_offset_m) > 0.05): + rospy.logwarn('WARNING: arm_calibrated_retracted_offset_m HAS AN UNUSUALLY LARGE MAGNITUDE') + rospy.loginfo('arm_calibrated_retracted_offset_m in meters = {0}'.format(arm_calibrated_retracted_offset_m)) self.linear_velocity_mps = 0.0 # m/s ROS SI standard for cmd_vel (REP 103) self.angular_velocity_radps = 0.0 # rad/s ROS SI standard for cmd_vel (REP 103)