From 6d314056e87ac7e9a63061c32a54f953f76b6958 Mon Sep 17 00:00:00 2001 From: hello-binit <64861565+hello-binit@users.noreply.github.com> Date: Wed, 3 Mar 2021 04:27:01 -0500 Subject: [PATCH] Generate Head Pan joint state info within command group --- stretch_core/nodes/command_groups.py | 46 +++++++++++++------ stretch_core/nodes/joint_trajectory_server.py | 16 +++---- stretch_core/nodes/stretch_driver | 14 +----- 3 files changed, 40 insertions(+), 36 deletions(-) diff --git a/stretch_core/nodes/command_groups.py b/stretch_core/nodes/command_groups.py index 01893ce..cd33db3 100644 --- a/stretch_core/nodes/command_groups.py +++ b/stretch_core/nodes/command_groups.py @@ -151,6 +151,21 @@ class SimpleCommandGroup: """ raise NotImplementedError + def joint_state(self, robot_status, **kwargs): + """Returns joint state of the robot + + Parameters + ---------- + robot_status: dict + robot's current status + + Returns + ------- + (float, float, float) + Current position, velocity, and effort + """ + raise NotImplementedError + def goal_reached(self): """Returns whether reached target point @@ -166,39 +181,40 @@ class SimpleCommandGroup: class HeadPanCommandGroup(SimpleCommandGroup): - def __init__(self, range_rad, head_pan_calibrated_offset, head_pan_calibrated_looked_left_offset, robot=None): + def __init__(self, range_rad, head_pan_calibrated_offset_rad, head_pan_calibrated_looked_left_offset_rad, robot=None): if range_rad is None and robot is not None: range_ticks = robot.head.motors['head_pan'].params['range_t'] range_rad = (robot.head.motors['head_pan'].ticks_to_world_rad(range_ticks[1]), robot.head.motors['head_pan'].ticks_to_world_rad(range_ticks[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 = head_pan_calibrated_offset_rad + self.calibrated_looked_left_offset_rad = head_pan_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 = True if pan_error > 0.0 else False 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.calibrated_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.calibrated_looked_left_offset_rad if self.looked_left else 0.0 + pos = pan_status['pos'] + self.calibrated_offset_rad + pan_backlash_correction + vel = pan_status['vel'] + effort = pan_status['effort'] + return (pos, vel, effort) + class HeadTiltCommandGroup(SimpleCommandGroup): def __init__(self, range_rad, head_tilt_calibrated_offset, diff --git a/stretch_core/nodes/joint_trajectory_server.py b/stretch_core/nodes/joint_trajectory_server.py index 0c2756a..95b6825 100644 --- a/stretch_core/nodes/joint_trajectory_server.py +++ b/stretch_core/nodes/joint_trajectory_server.py @@ -72,11 +72,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 @@ -84,7 +82,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) @@ -107,7 +105,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 @@ -117,10 +115,10 @@ class JointTrajectoryAction: 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] 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() @@ -146,13 +144,13 @@ class JointTrajectoryAction: 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] + for c in self.command_groups] if any(ret == True for ret in named_errors): self.node.robot_mode_rwlock.release_read() 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 8d2d552..e261258 100755 --- a/stretch_core/nodes/stretch_driver +++ b/stretch_core/nodes/stretch_driver @@ -171,18 +171,8 @@ class StretchBodyNode: 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'] + if self.use_robotis_head: + head_pan_rad, head_pan_vel, head_pan_effort = self.joint_trajectory_action.command_groups[0].joint_state(robot_status) # assign relevant head tilt status to variables head_tilt_status = robot_status['head']['head_tilt']