diff --git a/stretch_core/nodes/joint_trajectory_server.py b/stretch_core/nodes/joint_trajectory_server.py index 7271f17..d3cdf9e 100644 --- a/stretch_core/nodes/joint_trajectory_server.py +++ b/stretch_core/nodes/joint_trajectory_server.py @@ -42,16 +42,16 @@ 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.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)) diff --git a/stretch_core/nodes/stretch_driver b/stretch_core/nodes/stretch_driver index 4962c03..69af6f3 100755 --- a/stretch_core/nodes/stretch_driver +++ b/stretch_core/nodes/stretch_driver @@ -43,16 +43,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 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} @@ -159,14 +149,16 @@ class StretchBodyNode: # assign relevant head pan status to variables head_pan_status = robot_status['head']['head_pan'] pan_backlash_correction = 0.0 - head_pan_rad = head_pan_status['pos'] + self.head_pan_calibrated_offset_rad + pan_backlash_correction + head_pan_calibrated_offset_rad = 0.0 + head_pan_rad = head_pan_status['pos'] + 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'] tilt_backlash_correction = 0.0 - head_tilt_rad = head_tilt_status['pos'] + self.head_tilt_calibrated_offset_rad + tilt_backlash_correction + head_tilt_calibrated_offset_rad = 0.0 + head_tilt_rad = head_tilt_status['pos'] + head_tilt_calibrated_offset_rad + tilt_backlash_correction head_tilt_vel = head_tilt_status['vel'] head_tilt_effort = head_tilt_status['effort'] @@ -482,47 +474,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)