#!/usr/bin/env python from __future__ import print_function from sensor_msgs.msg import JointState from geometry_msgs.msg import Twist from nav_msgs.msg import Odometry import rospy import actionlib from control_msgs.msg import FollowJointTrajectoryAction from control_msgs.msg import FollowJointTrajectoryGoal from trajectory_msgs.msg import JointTrajectoryPoint import message_filters from visualization_msgs.msg import Marker from visualization_msgs.msg import MarkerArray from tf.transformations import quaternion_from_euler, euler_from_quaternion, quaternion_from_matrix from sensor_msgs.msg import Imu import math import time import threading import sys import argparse as ap import numpy as np import ros_numpy from copy import deepcopy import yaml import time class CollectHeadCalibrationDataNode: def __init__(self): self.rate = 10.0 self.joint_state = None self.acceleration = None self.data_time = None self.marker_time = None self.data_lock = threading.Lock() def calibration_data_callback(self, joint_state, accel, marker_array): # Prepare and assign data for calibration to member # variables. The configuration of the robot's joints, the # D435i acceleration, and the 3D fiducial estimates made via # D435i imagery are expected to be synchronized for this # callback. with self.data_lock: self.data_time = joint_state.header.stamp self.joint_state = joint_state self.acceleration = [accel.linear_acceleration.x, accel.linear_acceleration.y, accel.linear_acceleration.z] self.wrist_inside_marker_pose = None self.wrist_top_marker_pose = None self.base_left_marker_pose = None self.base_right_marker_pose = None self.shoulder_marker_pose = None self.marker_time = None for marker in marker_array.markers: # set marker_time to the earliest marker time if self.marker_time is None: self.marker_time = marker.header.stamp elif marker.header.stamp < self.marker_time: self.marker_time = marker.header.stamp if marker.id == self.wrist_inside_marker_id: self.wrist_inside_marker_pose = marker.pose if marker.id == self.wrist_top_marker_id: self.wrist_top_marker_pose = marker.pose if marker.id == self.base_left_marker_id: self.base_left_marker_pose = marker.pose if marker.id == self.base_right_marker_id: self.base_right_marker_pose = marker.pose if marker.id == self.shoulder_marker_id: self.shoulder_marker_pose = marker.pose def move_to_pose(self, pose): # Prepare and send a goal pose to which the robot should move. joint_names = [key for key in pose] self.trajectory_goal.trajectory.joint_names = joint_names joint_positions = [pose[key] for key in joint_names] self.point.positions = joint_positions self.trajectory_goal.trajectory.points = [self.point] self.trajectory_goal.trajectory.header.stamp = rospy.Time.now() self.trajectory_client.send_goal(self.trajectory_goal) self.trajectory_client.wait_for_result() def get_samples(self, pan_angle_center, tilt_angle_center, wrist_extension_center, number_of_samples, first_move=False): # Collect N observations (samples) centered around the # provided head and wrist extension pose. If first_move is # true, it indicates that this is the first movement to a new # region for calibration, which can induce more vibrations and # thus benefit from more time to settle prior to collecting a # sample. head_motion_settle_time = 1.0 #5.0 first_pan_tilt_extra_settle_time = 1.0 wait_before_each_sample_s = 0.2 # generate pan and wrist backlash samples # tilt backlash tilt_backlash_transition = self.tilt_angle_backlash_transition_rad # Ensure that the head is clearly in one of the two head tilt # backlash regimes and not in the indeterminate transition # region. tilt_backlash_safety_margin = 0.08 tilt_backlash_looking_down_max = tilt_backlash_transition - tilt_backlash_safety_margin # looking down tilt_backlash_looking_up_min = tilt_backlash_transition + tilt_backlash_safety_margin # looking up tilt_angle = tilt_angle_center if (tilt_angle_center > tilt_backlash_looking_down_max) and (tilt_angle_center < tilt_backlash_looking_up_min): # Commanded tilt_angle_center is in an uncertain backlash range # of tilt, so change it to be in a confident range. down_diff = abs(tilt_backlash_looking_down_max - tilt_angle_center) up_diff = abs(tilt_backlash_looking_up_min - tilt_angle_center) if up_diff < down_diff: tilt_angle = tilt_backlash_looking_up_min else: tilt_angle = tilt_backlash_looking_down_max if tilt_angle > tilt_backlash_transition: joint_head_tilt_looking_up = True else: joint_head_tilt_looking_up = False # represent all head pan and wrist extension backlash states backlash_combinations = ((False, False), (False, True), (True, True), (True, False)) pan_backlash_change_rad = 0.07 # ~8 degrees wrist_backlash_change_m = 0.01 # 1 cm samples = [] # collect samples for all head pan and wrist extension # backlash states for joint_head_pan_looked_left, wrist_extension_retracted in backlash_combinations: if joint_head_pan_looked_left: pan_angle = pan_angle_center + pan_backlash_change_rad else: pan_angle = pan_angle_center - pan_backlash_change_rad if wrist_extension_retracted: wrist_extension = wrist_extension_center - wrist_backlash_change_m else: wrist_extension = wrist_extension_center + wrist_backlash_change_m # move to backlash state head_arm_pose = {'joint_head_pan': pan_angle, 'joint_head_tilt': tilt_angle, 'wrist_extension': wrist_extension} self.move_to_pose(head_arm_pose) # wait for the joints and sensors to settle rospy.sleep(head_motion_settle_time) settled_time = rospy.Time.now() # The first move to a pose is typically larger and can # benefit from more settling time due to induced # vibrations. if first_move: rospy.sleep(first_pan_tilt_extra_settle_time) for sample_number in range(number_of_samples): rospy.sleep(wait_before_each_sample_s) observation = {'joints': {'joint_head_pan': None, 'joint_head_tilt': None, 'wrist_extension' : None, 'joint_lift': None }, 'backlash_state': { 'joint_head_pan_looked_left': None, 'joint_head_tilt_looking_up': None, 'wrist_extension_retracted': None }, 'camera_measurements': { 'd435i_acceleration': None, 'wrist_inside_marker_pose': None, 'wrist_top_marker_pose': None, 'base_left_marker_pose': None, 'base_right_marker_pose': None, 'shoulder_marker_pose': None} } # wait for a sample taken after the robot has # settled timeout_duration = rospy.Duration(secs=10) start_wait = rospy.Time.now() timeout = False data_ready = False while (not data_ready) and (not timeout): # check the timing of the current sample with self.data_lock: if (self.data_time is not None) and (self.data_time > settled_time): if (self.marker_time is None): # joint_states and acceleration # were taken after the robot # settled and there are no aruco # marker poses data_ready = True elif (self.marker_time > settled_time): # joint_states, acceleration, # and aruco marker poses were # taken after the robot # settled data_ready = True if not data_ready: #rospy.sleep(0.2) #0.1 rospy.sleep(1.0) timeout = (rospy.Time.now() - start_wait) > timeout_duration if timeout: rospy.logerr('collect_head_calibration_data get_samples: timeout while waiting for sample.') raise Exception('Timed out waiting for joint_states/accelerations/markers messages after 10 seconds') with self.data_lock: #set backlash joint state backlash_state = observation['backlash_state'] backlash_state['joint_head_pan_looked_left'] = joint_head_pan_looked_left backlash_state['joint_head_tilt_looking_up'] = joint_head_tilt_looking_up backlash_state['wrist_extension_retracted'] = wrist_extension_retracted # record the settled joint values joints = observation['joints'] for joint_name in joints: print('Joint name %s'%joint_name) joint_index = self.joint_state.name.index(joint_name) joint_value = self.joint_state.position[joint_index] joints[joint_name] = joint_value # record the settled camera measurments camera_measurements = observation['camera_measurements'] camera_measurements['d435i_acceleration'] = self.acceleration # record the settled aruco measurements if self.wrist_inside_marker_pose is not None: camera_measurements['wrist_inside_marker_pose'] = ros_numpy.numpify(self.wrist_inside_marker_pose).tolist() if self.wrist_top_marker_pose is not None: camera_measurements['wrist_top_marker_pose'] = ros_numpy.numpify(self.wrist_top_marker_pose).tolist() if self.base_left_marker_pose is not None: camera_measurements['base_left_marker_pose'] = ros_numpy.numpify(self.base_left_marker_pose).tolist() if self.base_right_marker_pose is not None: camera_measurements['base_right_marker_pose'] = ros_numpy.numpify(self.base_right_marker_pose).tolist() if self.shoulder_marker_pose is not None: camera_measurements['shoulder_marker_pose'] = ros_numpy.numpify(self.shoulder_marker_pose).tolist() samples.append(observation) return samples def calibrate_pan_and_tilt(self, collect_check_data=False): # Collects observations of fiducial markers on the robot's # body while moving the head pan, head tilt, arm lift, and arm # extension. There are five markers in total with the # following locations: front left of the mobile base, front # right of the mobile base, top of the shoulder, top of the # wrist, and inside the wrist. # # When collect_check_data is True, fewer samples are collected # with the intention of using them to check the current # calibration. When collect_check_data is False, more samples # are collected with the intention of fully calibrating the # robot. calibration_data = [] number_of_samples_per_head_pose = 1 #3 wrist_motion_settle_time = 1.0 ######################################## ## ## COLLECT GLOBAL HEAD LOOKING DOWN DATA ## ######################################## rospy.loginfo('') rospy.loginfo('*************************************') rospy.loginfo('COLLECT GLOBAL HEAD LOOKING DOWN DATA') rospy.loginfo('*************************************') rospy.loginfo('') # looking down data min_tilt_angle_rad = -1.4 max_tilt_angle_rad = -0.5 min_pan_angle_rad = -3.8 max_pan_angle_rad = 1.3 if collect_check_data: number_of_tilt_steps = 1 number_of_pan_steps = 3 else: number_of_tilt_steps = 3 #4 number_of_pan_steps = 5 #7 pan_angles_rad = np.linspace(min_pan_angle_rad, max_pan_angle_rad, number_of_pan_steps) tilt_angles_rad = np.linspace(min_tilt_angle_rad, max_tilt_angle_rad, number_of_tilt_steps) rospy.loginfo('Move to a new arm pose.') wrist_extension = 0.12 wrist_pose = {'wrist_extension': wrist_extension} self.move_to_pose(wrist_pose) lift_m = 0.16 lift_pose = {'joint_lift': lift_m} self.move_to_pose(lift_pose) # wait for the joints and sensors to settle rospy.loginfo('Wait {0} seconds for the robot to settle after motion of the arm.'.format(wrist_motion_settle_time)) rospy.sleep(wrist_motion_settle_time) rospy.loginfo('Starting to collect global head looking down data (expect to collect {0} samples).'.format(number_of_tilt_steps * number_of_pan_steps)) first_pan_tilt = True for pan_angle in pan_angles_rad: for tilt_angle in tilt_angles_rad: observation = self.get_samples(pan_angle, tilt_angle, wrist_extension, number_of_samples_per_head_pose, first_move=first_pan_tilt) first_pan_tilt = False calibration_data.extend(observation) n = len(calibration_data) if (n % 10) == 0: rospy.loginfo('{0} samples collected so far.'.format(n)) ####################################### ## ## COLLECT GLOBAL HEAD LOOKING UP DATA ## ####################################### rospy.loginfo('') rospy.loginfo('*************************************') rospy.loginfo('COLLECT GLOBAL HEAD LOOKING UP DATA') rospy.loginfo('*************************************') rospy.loginfo('') if not collect_check_data: tilt_angles_rad = [-0.3, 0.38] pan_angles_rad = [-3.8, -1.66, 1.33] rospy.loginfo('Move to a new arm pose.') lift_m = 0.92 lift_pose = {'joint_lift': lift_m} self.move_to_pose(lift_pose) wrist_extension = 0.29 wrist_pose = {'wrist_extension': wrist_extension} self.move_to_pose(wrist_pose) # wait for the joints and sensors to settle rospy.loginfo('Wait {0} seconds for the robot to settle after motion of the arm.'.format(wrist_motion_settle_time)) rospy.sleep(wrist_motion_settle_time) rospy.loginfo('Starting to collect global head looking up data (expect to collect {0} samples).'.format(len(pan_angles_rad) * len(tilt_angles_rad))) first_pan_tilt = True for pan_angle in pan_angles_rad: for tilt_angle in tilt_angles_rad: observation = self.get_samples(pan_angle, tilt_angle, wrist_extension, number_of_samples_per_head_pose, first_move=first_pan_tilt) first_pan_tilt = False calibration_data.extend(observation) n = len(calibration_data) if (n % 10) == 0: rospy.loginfo('{0} samples collected so far.'.format(n)) ####################################### ## ## COLLECT HIGH SHOULDER DATA ## ####################################### rospy.loginfo('') rospy.loginfo('*************************************') rospy.loginfo('COLLECT HIGH SHOULDER DATA') rospy.loginfo('*************************************') rospy.loginfo('') ############################################################## ### poses that see the shoulder marker # # lift 0.8 # pan -3.82 to -3.619 # tilt -0.918 to -0.49 # # lift 0.2 # pan -3.85 to -2.48 # tilt -1.23 to -0.87 # # two poses with good coverage # (needs to account for +/- 0.07 rad for pan backlash) # pan -3.7, tilt -0.7 # pan -3.7, tilt -1.1 # # Marker detected with point clouds # lift 0.5, pan -3.7, tilt = -1.4 to -1.2 # lift 0.65, pan -3.85, tilt = -1.0 # lift 0.74, pan -3.85, tilt = -0.78 to -1.4 (-1.4 also sees the base markers) ############################################################## wrist_extension = 0.29 # low shoulder height pan_angle_rad = -3.7 tilt_angle_rad_1 = -1.4 tilt_angle_rad_2 = -1.2 lift_m = 0.5 pose = {'joint_lift': lift_m} rospy.loginfo('Move to first shoulder capture height.') self.move_to_pose(pose) rospy.loginfo('Wait {0} seconds for the robot to settle after motion of the arm.'.format(wrist_motion_settle_time)) rospy.sleep(wrist_motion_settle_time) first_pan_tilt = True observation = self.get_samples(pan_angle_rad, tilt_angle_rad_1, wrist_extension, number_of_samples_per_head_pose, first_move=first_pan_tilt) calibration_data.extend(observation) if not collect_check_data: first_pan_tilt = False observation = self.get_samples(pan_angle_rad, tilt_angle_rad_2, wrist_extension, number_of_samples_per_head_pose, first_move=first_pan_tilt) calibration_data.extend(observation) # high shoulder height pan_angle_rad = -3.85 tilt_angle_rad_1 = -0.8 tilt_angle_rad_2 = -1.4 lift_m = 0.74 pose = {'joint_lift': lift_m} rospy.loginfo('Move to second shoulder capture height.') self.move_to_pose(pose) rospy.loginfo('Wait {0} seconds for the robot to settle after motion of the arm.'.format(wrist_motion_settle_time)) rospy.sleep(wrist_motion_settle_time) first_pan_tilt = True observation = self.get_samples(pan_angle_rad, tilt_angle_rad_1, wrist_extension, number_of_samples_per_head_pose, first_move=first_pan_tilt) calibration_data.extend(observation) if not collect_check_data: first_pan_tilt = False observation = self.get_samples(pan_angle_rad, tilt_angle_rad_2, wrist_extension, number_of_samples_per_head_pose, first_move=first_pan_tilt) calibration_data.extend(observation) ####################################### ## ## COLLECT ARM FOCUSED DATA ## ####################################### rospy.loginfo('') rospy.loginfo('*************************************') rospy.loginfo('COLLECT ARM FOCUSED DATA') rospy.loginfo('*************************************') rospy.loginfo('') ############################################################## ### poses that see both of the arm markers # # lift height 0.8 # extension 0.4 tilt -0.52 pan -2.06 to -1.45 # extension 0.03 tilt -1.27 pan -2.33 to -1.48 # # lift height 0.2 # extension 0.4 tilt -0.9 pan -2.9 to -0.93 # extension 0.03 tilt -1.23 pan -3.2 to -0.4 (full range) # # ranges with good coverage # (needs to account for +/- 0.07 rad for pan backlash) # tilt -1.2 to -0.5 # pan -2.0 to -1.45 # ############################################################## wrist_focused_min_pan = -2.0 wrist_focused_max_pan = -1.45 wrist_focused_min_tilt = -1.2 wrist_focused_max_tilt = -0.5 if collect_check_data: num_wrist_focused_pan_steps = 1 #3 num_wrist_focused_tilt_steps = 1 #3 else: num_wrist_focused_pan_steps = 2 #3 num_wrist_focused_tilt_steps = 2 #3 wrist_focused_pan_angles_rad = np.linspace(wrist_focused_min_pan, wrist_focused_max_pan, num_wrist_focused_pan_steps) wrist_focused_tilt_angles_rad = np.linspace(wrist_focused_min_tilt, wrist_focused_max_tilt, num_wrist_focused_tilt_steps) if collect_check_data: wrist_extensions = [0.2] # Already have data from a single extension at a height of # 1.2m from global camera looking down, so try using only 2 # heights. # # 0.46 m = (0.8 m + 0.12 m) / 2.0 wrist_heights = [0.46, 0.8] #[0.2, 0.5, 0.8] wrist_poses = [{'wrist_extension': e, 'joint_lift': h} for h in wrist_heights for e in wrist_extensions] num_wrist_poses = len(wrist_poses) else: wrist_extensions = [0.03, 0.4] # Already have data from a single extension at a height of # 1.2m from global camera looking down, so try using only 2 # heights. # # 0.46 m = (0.8 m + 0.12 m) / 2.0 wrist_heights = [0.46, 0.8] #[0.2, 0.5, 0.8] wrist_poses = [{'wrist_extension': e, 'joint_lift': h} for h in wrist_heights for e in wrist_extensions] num_wrist_poses = len(wrist_poses) rospy.loginfo('Starting to collect arm focused samples (expect to collect {0} samples).'.format(num_wrist_poses * num_wrist_focused_pan_steps * num_wrist_focused_tilt_steps)) for wrist_pose in wrist_poses: rospy.loginfo('Move to a new arm pose.') self.move_to_pose(wrist_pose) # wait for the joints and sensors to settle rospy.loginfo('Wait {0} seconds for the robot to settle after motion of the arm.'.format(wrist_motion_settle_time)) rospy.sleep(wrist_motion_settle_time) first_pan_tilt = True for pan_angle in wrist_focused_pan_angles_rad: for tilt_angle in wrist_focused_tilt_angles_rad: observation = self.get_samples(pan_angle, tilt_angle, wrist_pose['wrist_extension'], number_of_samples_per_head_pose, first_move=first_pan_tilt) first_pan_tilt = False calibration_data.extend(observation) n = len(calibration_data) if (n % 10) == 0: rospy.loginfo('{0} samples collected so far.'.format(n)) ####################################### ## ## FINISHED COLLECTING DATA - SAVE IT ## ####################################### rospy.loginfo('') rospy.loginfo('*************************************') rospy.loginfo('FINISHED COLLECTING DATA') rospy.loginfo('*************************************') rospy.loginfo('') rospy.loginfo('Collected {0} samples in total.'.format(len(calibration_data))) t = time.localtime() capture_date = str(t.tm_year) + str(t.tm_mon).zfill(2) + str(t.tm_mday).zfill(2) + str(t.tm_hour).zfill(2) + str(t.tm_min).zfill(2) if collect_check_data: filename = self.calibration_directory + 'check_head_calibration_data_' + capture_date + '.yaml' else: filename = self.calibration_directory + 'head_calibration_data_' + capture_date + '.yaml' rospy.loginfo('Saving calibration_data to a YAML file named {0}'.format(filename)) fid = open(filename, 'w') yaml.dump(calibration_data, fid) fid.close() rospy.loginfo('') rospy.loginfo('*************************************') rospy.loginfo('') ####################################### def move_to_initial_configuration(self): # The robot is commanded to move to this pose prior to # beginning to collect calibration data. 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) def main(self, collect_check_data): rospy.init_node('collect_head_calibration_data') self.node_name = rospy.get_name() rospy.loginfo("{0} started".format(self.node_name)) # Obtain the ArUco marker ID numbers. self.marker_info = rospy.get_param('/aruco_marker_info') for k in self.marker_info.keys(): m = self.marker_info[k] if m['link'] == 'link_aruco_left_base': self.base_left_marker_id = int(k) if m['link'] == 'link_aruco_right_base': self.base_right_marker_id = int(k) if m['link'] == 'link_aruco_inner_wrist': self.wrist_inside_marker_id = int(k) if m['link'] == 'link_aruco_top_wrist': self.wrist_top_marker_id = int(k) if m['link'] == 'link_aruco_shoulder': self.shoulder_marker_id = int(k) filename = rospy.get_param('~controller_calibration_file') rospy.loginfo('Loading factory default tilt backlash transition angle from the YAML file named {0}'.format(filename)) fid = open(filename, 'r') controller_parameters = yaml.load(fid) fid.close() self.tilt_angle_backlash_transition_rad = controller_parameters['tilt_angle_backlash_transition'] deg_per_rad = 180.0/math.pi rospy.loginfo('self.tilt_angle_backlash_transition_rad in degrees = {0}'.format(self.tilt_angle_backlash_transition_rad * deg_per_rad)) self.calibration_directory = rospy.get_param('~calibration_directory') rospy.loginfo('Using the following directory for calibration files: {0}'.format(self.calibration_directory)) # Setup time synchronization for calibration data. joint_state_subscriber = message_filters.Subscriber('/stretch/joint_states', JointState) accel_subscriber = message_filters.Subscriber('/camera/accel/sample_corrected', Imu) aruco_subscriber = message_filters.Subscriber('/aruco/marker_array', MarkerArray) slop_time = 0.1 self.synchronizer = message_filters.ApproximateTimeSynchronizer([joint_state_subscriber, accel_subscriber, aruco_subscriber], 10, slop_time, allow_headerless=True) self.synchronizer.registerCallback(self.calibration_data_callback) self.trajectory_client = actionlib.SimpleActionClient('/stretch_controller/follow_joint_trajectory', FollowJointTrajectoryAction) self.trajectory_goal = FollowJointTrajectoryGoal() self.trajectory_goal.goal_time_tolerance = rospy.Time(1.0) self.point = JointTrajectoryPoint() self.point.time_from_start = rospy.Duration(0.0) server_reached = self.trajectory_client.wait_for_server(timeout=rospy.Duration(60.0)) if not server_reached: 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) if __name__ == '__main__': parser = ap.ArgumentParser(description='Collect head calibration data.') parser.add_argument('--check', action='store_true', help='Collect data to check the current calibration, instead of data to perform a new calibration.') args, unknown = parser.parse_known_args() collect_check_data = args.check try: node = CollectHeadCalibrationDataNode() node.main(collect_check_data) except rospy.ROSInterruptException: pass