cccccccc
/
stretch_ros
mirror of https://github.com/hello-robot/stretch_ros.git
1
0
Fork 0
Code Issues 0 Projects 0 Releases 0 Wiki Activity
Browse Source

Remove backlash constants from stretch_driver

pull/52/head
Binit Shah 2 years ago
parent
32a157111a
commit
80730f6b9e
2 changed files with 41 additions and 56 deletions
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. +6
    -6
      stretch_core/nodes/joint_trajectory_server.py
  2. +35
    -50
      stretch_core/nodes/stretch_driver

+ 6
- 6
stretch_core/nodes/joint_trajectory_server.py View File

@ -42,16 +42,16 @@ class JointTrajectoryAction:
r.end_of_arm.motors['stretch_gripper'].world_rad_to_pct(gripper_range_rad[1])) 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.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.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.wrist_yaw_cg = WristYawCommandGroup(wrist_yaw_range_rad)
self.gripper_cg = GripperCommandGroup(gripper_range_robotis) self.gripper_cg = GripperCommandGroup(gripper_range_robotis)
self.telescoping_cg = TelescopingCommandGroup(tuple(r.arm.params['range_m']), 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.lift_cg = LiftCommandGroup(tuple(r.lift.params['range_m']))
self.mobile_base_cg = MobileBaseCommandGroup(virtual_range_m=(-0.5, 0.5)) self.mobile_base_cg = MobileBaseCommandGroup(virtual_range_m=(-0.5, 0.5))

+ 35
- 50
stretch_core/nodes/stretch_driver View File

@ -43,16 +43,6 @@ class StretchBodyNode:
self.default_goal_timeout_s = 10.0 self.default_goal_timeout_s = 10.0
self.default_goal_timeout_duration = rospy.Duration(self.default_goal_timeout_s) 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.gripper_conversion = GripperConversion()
self.mobile_base_manipulation_origin = None #{'x':None, 'y':None, 'theta':None} 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 # assign relevant head pan status to variables
head_pan_status = robot_status['head']['head_pan'] head_pan_status = robot_status['head']['head_pan']
pan_backlash_correction = 0.0 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_vel = head_pan_status['vel']
head_pan_effort = head_pan_status['effort'] head_pan_effort = head_pan_status['effort']
# assign relevant head tilt status to variables # assign relevant head tilt status to variables
head_tilt_status = robot_status['head']['head_tilt'] head_tilt_status = robot_status['head']['head_tilt']
tilt_backlash_correction = 0.0 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_vel = head_tilt_status['vel']
head_tilt_effort = head_tilt_status['effort'] head_tilt_effort = head_tilt_status['effort']
@ -482,47 +474,40 @@ class StretchBodyNode:
if self.broadcast_odom_tf: if self.broadcast_odom_tf:
self.tf_broadcaster = tf2_ros.TransformBroadcaster() 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') filename = rospy.get_param('~controller_calibration_file')
rospy.loginfo('Loading controller calibration parameters for the head from YAML file named {0}'.format(filename)) rospy.loginfo('Loading controller calibration parameters for the head from YAML file named {0}'.format(filename))
with open(filename, 'r') as fid: 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.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) self.angular_velocity_radps = 0.0 # rad/s ROS SI standard for cmd_vel (REP 103)

Write Preview
Loading…
Cancel
Save