From a47189982f1968ec3cdd56649dc50d5a863707c2 Mon Sep 17 00:00:00 2001 From: Binit Shah Date: Wed, 19 Jan 2022 19:45:04 -0800 Subject: [PATCH] Initial pose for dex wrist joints in collect calib data --- .../nodes/collect_head_calibration_data | 24 ++++++++++++++----- 1 file changed, 18 insertions(+), 6 deletions(-) diff --git a/stretch_calibration/nodes/collect_head_calibration_data b/stretch_calibration/nodes/collect_head_calibration_data index 0a43b6e..ea707dc 100755 --- a/stretch_calibration/nodes/collect_head_calibration_data +++ b/stretch_calibration/nodes/collect_head_calibration_data @@ -574,12 +574,21 @@ class CollectHeadCalibrationDataNode: def move_to_initial_configuration(self): # The robot is commanded to move to this pose prior to # beginning to collect calibration data. - initial_pose = {'joint_wrist_yaw': 0.0, - 'wrist_extension': 0.0, - 'joint_lift': 0.3, - 'gripper_aperture': 0.0, - 'joint_head_pan': -1.6947147036864942, - 'joint_head_tilt': -0.4} + initial_pose_map = { + 'joint_wrist_yaw': 0.0, + 'joint_wrist_pitch': 0.0, + 'joint_wrist_roll': 0.0, + 'wrist_extension': 0.0, + 'joint_lift': 0.3, + 'gripper_aperture': 0.0, + 'joint_head_pan': -1.6947147036864942, + 'joint_head_tilt': -0.4 + } + + initial_pose = {} + for j in self.joint_state.name: + if j in initial_pose_map: + initial_pose[j] = initial_pose_map[j] rospy.loginfo('Move to the calibration start pose.') self.move_to_pose(initial_pose) @@ -637,6 +646,9 @@ class CollectHeadCalibrationDataNode: rospy.signal_shutdown('Unable to connect to arm action server. Timeout exceeded.') sys.exit() + rate = rospy.Rate(self.rate) + while self.joint_state is None: + rate.sleep() self.move_to_initial_configuration() self.calibrate_pan_and_tilt(collect_check_data)