diff --git a/stretch_core/nodes/command_groups.py b/stretch_core/nodes/command_groups.py index 0fc6835..93e2678 100644 --- a/stretch_core/nodes/command_groups.py +++ b/stretch_core/nodes/command_groups.py @@ -12,7 +12,7 @@ class SimpleCommandGroup: self.acceptable_joint_error = acceptable_joint_error self.joint_name = joint_name - def update(self, joint_names, invalid_joints_error_func, robot_mode): + def update(self, joint_names, invalid_joints_error_func, **kwargs): self.use_joint = False if self.joint_name in joint_names: self.joint_index = joint_names.index(self.joint_name) @@ -24,7 +24,7 @@ class SimpleCommandGroup: return 1 return 0 - def set_goal(self, point, invalid_goal_error_func, other): + def set_goal(self, point, invalid_goal_error_func, **kwargs): self.joint_goal = False self.goal_joint_ros = None self.goal_joint_hello = None @@ -41,11 +41,10 @@ class SimpleCommandGroup: def ros_to_mechaduino(self, joint_ros): return joint_ros - - def init_execution(self, robot_status): + def init_execution(self, **kwargs): pass - def update_execution(self, robot_status, backlash_state): + def update_execution(self, robot_status, **kwargs): # This method needs to be implemented. It also needs to set self.joint_error. return None @@ -62,7 +61,8 @@ 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 update_execution(self, robot_status, backlash_state): + def update_execution(self, robot_status, **kwargs): + backlash_state = kwargs['backlash_state'] if self.joint_goal: if backlash_state['head_pan_looked_left']: pan_backlash_correction = self.head_pan_calibrated_looked_left_offset @@ -82,7 +82,8 @@ class HeadTiltCommandGroup(SimpleCommandGroup): self.head_tilt_calibrated_offset = head_tilt_calibrated_offset self.head_tilt_calibrated_looking_up_offset = head_tilt_calibrated_looking_up_offset - def update_execution(self, robot_status, backlash_state): + def update_execution(self, robot_status, **kwargs): + backlash_state = kwargs['backlash_state'] if self.joint_goal: if backlash_state['head_tilt_looking_up']: tilt_backlash_correction = self.head_tilt_calibrated_looking_up_offset @@ -100,7 +101,7 @@ class WristYawCommandGroup(SimpleCommandGroup): def __init__(self): SimpleCommandGroup.__init__(self, 'joint_wrist_yaw', acceptable_joint_error=0.015, clip_ros_tolerance=0.001) - def update_execution(self, robot_status, backlash_state): + def update_execution(self, robot_status, **kwargs): if self.joint_goal: self.current_joint_hello = robot_status['end_of_arm']['wrist_yaw']['pos'] self.joint_error = self.goal_joint_hello - self.current_joint_hello @@ -117,7 +118,7 @@ class GripperCommandGroup: self.gripper_joint_names = ['joint_gripper_finger_left', 'joint_gripper_finger_right', 'gripper_aperture'] self.gripper_conversion = GripperConversion() - def update(self, joint_names, invalid_joints_error_func, robot_mode): + def update(self, joint_names, invalid_joints_error_func, **kwargs): self.use_gripper_joint = False self.gripper_joints_to_command_names = [j for j in self.gripper_joint_names if j in joint_names] self.gripper_joints_to_command_indices = [joint_names.index(j) for j in self.gripper_joints_to_command_names] @@ -143,7 +144,7 @@ class GripperCommandGroup: else: return 0 - def set_goal(self, point, invalid_goal_error_func, other): + def set_goal(self, point, invalid_goal_error_func, **kwargs): self.gripper_joint_goal = False self.goal_gripper_joint = None goal = None @@ -170,10 +171,10 @@ class GripperCommandGroup: def ros_to_mechaduino(self, joint_ros): return joint_ros - def init_execution(self, robot_status): + def init_execution(self, **kwargs): pass - def update_execution(self, robot_status, backlash_state): + def update_execution(self, robot_status, **kwargs): pass def goal_reached(self): @@ -191,7 +192,7 @@ class TelescopingCommandGroup: self.clip_ros_tolerance = 0.001 self.acceptable_joint_error_m = 0.005 - def update(self, joint_names, invalid_joints_error_func, robot_mode): + def update(self, joint_names, invalid_joints_error_func, **kwargs): self.use_telescoping_joints = False self.use_wrist_extension = False if 'wrist_extension' in joint_names: @@ -216,7 +217,7 @@ class TelescopingCommandGroup: return len(self.telescoping_joints) return 0 - def set_goal(self, point, invalid_goal_error_func, other): + def set_goal(self, point, invalid_goal_error_func, **kwargs): self.extension_goal = False self.goal_extension_mecha = None self.goal_extension_ros = None @@ -239,10 +240,11 @@ class TelescopingCommandGroup: def ros_to_mechaduino(self, wrist_extension_ros): return wrist_extension_ros - def init_execution(self, robot_status): + def init_execution(self, **kwargs): pass - def update_execution(self, robot_status, backlash_state): + def update_execution(self, robot_status, **kwargs): + backlash_state = kwargs['backlash_state'] if self.extension_goal: if backlash_state['wrist_extension_retracted']: arm_backlash_correction = self.wrist_extension_calibrated_retracted_offset @@ -268,7 +270,7 @@ class LiftCommandGroup: self.max_arm_height = max_arm_height self.lift_ros_range = [0.0, self.max_arm_height] - def update(self, joint_names, invalid_joints_error_func, robot_mode): + def update(self, joint_names, invalid_joints_error_func, **kwargs): self.use_lift = False if 'joint_lift' in joint_names: self.lift_index = joint_names.index('joint_lift') @@ -280,7 +282,7 @@ class LiftCommandGroup: return 1 return 0 - def set_goal(self, point, invalid_goal_error_func, other): + def set_goal(self, point, invalid_goal_error_func, **kwargs): self.lift_goal = False self.goal_lift_mecha = None self.goal_lift_ros = None @@ -298,10 +300,10 @@ class LiftCommandGroup: def ros_to_mechaduino(self, lift_ros): return lift_ros - def init_execution(self, robot_status): + def init_execution(self, **kwargs): pass - def update_execution(self, robot_status, backlash_state): + def update_execution(self, robot_status, **kwargs): if self.lift_goal: self.current_lift_mecha = robot_status['lift']['pos'] self.lift_error_m = self.goal_lift_mecha - self.current_lift_mecha @@ -324,7 +326,8 @@ class MobileBaseCommandGroup: self.acceptable_mobile_base_error_rad = (np.pi/180.0) * 6.0 self.excellent_mobile_base_error_rad = (np.pi/180.0) * 0.6 - def update(self, joint_names, invalid_joints_error_func, robot_mode): + def update(self, joint_names, invalid_joints_error_func, **kwargs): + robot_mode = kwargs['robot_mode'] self.use_mobile_base_virtual_joint = False self.use_mobile_base_inc = False self.use_mobile_base_inc_rot = ('rotate_mobile_base' in joint_names) @@ -368,7 +371,6 @@ class MobileBaseCommandGroup: self.translate_mobile_base_index = joint_names.index('translate_mobile_base') return True - def get_num_valid_commands(self): number_of_valid_joints = 0 if self.use_mobile_base_virtual_joint: @@ -379,14 +381,14 @@ class MobileBaseCommandGroup: number_of_valid_joints += 1 return number_of_valid_joints - def set_goal(self, point, invalid_goal_error_func, manipulation_origin): + def set_goal(self, point, invalid_goal_error_func, **kwargs): self.rotate_mobile_base_goal = False self.translate_mobile_base_goal = False self.virtual_joint_mobile_base_goal = False if self.use_mobile_base_virtual_joint: self.goal_mobile_base_virtual_joint_ros = point.positions[self.virtual_joint_mobile_base_index] - self.goal_mobile_base_virtual_joint_mecha = self.ros_to_mechaduino(self.goal_mobile_base_virtual_joint_ros, manipulation_origin) + self.goal_mobile_base_virtual_joint_mecha = self.ros_to_mechaduino(self.goal_mobile_base_virtual_joint_ros, kwargs['manipulation_origin']) self.virtual_joint_mobile_base_goal = True if self.use_mobile_base_inc_rot: @@ -429,13 +431,14 @@ class MobileBaseCommandGroup: else: return (manipulation_origin['x'] + ros_pos) - def init_execution(self, robot_status): + def init_execution(self, **kwargs): + robot_status = kwargs['robot_status'] self.initial_mobile_base_translation_mecha_x = robot_status['base']['x'] self.initial_mobile_base_translation_mecha_y = robot_status['base']['y'] self.initial_mobile_base_rotation_mecha = robot_status['base']['theta'] b = robot_status['base'] - def update_execution(self, robot_status, backlash_state): + def update_execution(self, robot_status, **kwargs): current_mobile_base_translation_mecha_x = robot_status['base']['x'] current_mobile_base_translation_mecha_y = robot_status['base']['y'] current_mobile_base_rotation_mecha = robot_status['base']['theta'] diff --git a/stretch_core/nodes/joint_trajectory_server.py b/stretch_core/nodes/joint_trajectory_server.py index 1178eb8..c28f49e 100644 --- a/stretch_core/nodes/joint_trajectory_server.py +++ b/stretch_core/nodes/joint_trajectory_server.py @@ -56,7 +56,8 @@ class JointTrajectoryAction: command_groups = [self.telescoping_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, self.node.robot_mode) + updates = [c.update(commanded_joint_names, self.invalid_joints_callback, + robot_mode=self.node.robot_mode) for c in command_groups] if not all(updates): # The joint names violated at least one of the command @@ -88,7 +89,8 @@ class JointTrajectoryAction: target point #{1} = <{2}>'.format(self.node.node_name, pointi, point)) valid_goals = [c.set_goal(point, self.invalid_goal_callback, - self.node.mobile_base_manipulation_origin) for c in command_groups] + 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 @@ -97,7 +99,7 @@ class JointTrajectoryAction: return robot_status = self.node.robot.get_status() # uses lock held by robot - [c.init_execution(robot_status) for c in command_groups] + [c.init_execution(robot_status=robot_status) for c in command_groups] goals_reached = [c.goal_reached() for c in command_groups] incremental_commands_executed = False update_rate = rospy.Rate(15.0) @@ -113,15 +115,16 @@ class JointTrajectoryAction: robot_status = self.node.robot.get_status() if self.node.use_lift: - lift_error_m = self.lift_cg.update_execution(robot_status, self.node.backlash_state) - extension_error_m = self.telescoping_cg.update_execution(robot_status, - self.node.backlash_state) + lift_error_m = self.lift_cg.update_execution(robot_status, + backlash_state=self.node.backlash_state) + extension_error_m = \ + self.telescoping_cg.update_execution(robot_status, backlash_state=self.node.backlash_state) mobile_base_error_m, mobile_base_error_rad = \ - self.mobile_base_cg.update_execution(robot_status, self.node.backlash_state) - self.head_pan_cg.update_execution(robot_status, self.node.backlash_state) - self.head_tilt_cg.update_execution(robot_status, self.node.backlash_state) - self.wrist_yaw_cg.update_execution(robot_status, self.node.backlash_state) - self.gripper_cg.update_execution(robot_status, self.node.backlash_state) + self.mobile_base_cg.update_execution(robot_status, backlash_state=self.node.backlash_state) + self.head_pan_cg.update_execution(robot_status, backlash_state=self.node.backlash_state) + self.head_tilt_cg.update_execution(robot_status, backlash_state=self.node.backlash_state) + self.wrist_yaw_cg.update_execution(robot_status, backlash_state=self.node.backlash_state) + self.gripper_cg.update_execution(robot_status, backlash_state=self.node.backlash_state) # Check if a premption request has been received. with self.node.robot_stop_lock: