Remove backlash constants from stretch_driver

stretch_core/nodes/joint_trajectory_server.py

@@ -41,16 +41,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))
 

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.robot_stop_lock = threading.Lock()
@@ -152,14 +142,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']
 
@@ -461,47 +453,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)