From e428305d5adeec00001593ecca2139f8d3098d1b Mon Sep 17 00:00:00 2001 From: Charlie Kemp <31106448+hello-ck@users.noreply.github.com> Date: Thu, 1 Oct 2020 18:10:52 -0400 Subject: [PATCH] efficiently check calibration Code to check the existing calibration. It does the following: + Collects a reduced number of new observations with which to check the current calibration. + Visualizes and computes the fit of the current calibration to the new observations. Future work: + Improve the reduced number of observations to increase efficiency. + Check the numeric results of the fit and provide a warning if the quality is low. --- .../launch/check_head_calibration.launch | 58 +++++++++++ ...collect_check_head_calibration_data.launch | 58 +++++++++++ .../nodes/check_head_calibration.sh | 44 +++++++++ .../nodes/collect_head_calibration_data | 97 +++++++++++++------ .../nodes/process_head_calibration_data | 21 ++-- 5 files changed, 242 insertions(+), 36 deletions(-) create mode 100644 stretch_calibration/launch/check_head_calibration.launch create mode 100644 stretch_calibration/launch/collect_check_head_calibration_data.launch create mode 100755 stretch_calibration/nodes/check_head_calibration.sh diff --git a/stretch_calibration/launch/check_head_calibration.launch b/stretch_calibration/launch/check_head_calibration.launch new file mode 100644 index 0000000..2b63a7d --- /dev/null +++ b/stretch_calibration/launch/check_head_calibration.launch @@ -0,0 +1,58 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + [/stretch/joint_states] + + + + + + + + + + + + + + + + + + + + + + diff --git a/stretch_calibration/launch/collect_check_head_calibration_data.launch b/stretch_calibration/launch/collect_check_head_calibration_data.launch new file mode 100644 index 0000000..ca9abb4 --- /dev/null +++ b/stretch_calibration/launch/collect_check_head_calibration_data.launch @@ -0,0 +1,58 @@ + + + + + + + + + + + + + + + + + + + + + + + [/stretch/joint_states] + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/stretch_calibration/nodes/check_head_calibration.sh b/stretch_calibration/nodes/check_head_calibration.sh new file mode 100755 index 0000000..2b15280 --- /dev/null +++ b/stretch_calibration/nodes/check_head_calibration.sh @@ -0,0 +1,44 @@ +#!/bin/bash + +echo "-----------------------------------------------------------" +echo "Find the most recent optimization results file." + +FILENAME=$HELLO_FLEET_PATH/$HELLO_FLEET_ID"/calibration_ros/head_calibration_result_" + +for file in `ls $FILENAME*.yaml | sort -r`; do + MOSTRECENT_OPTIMIZATION_FILE=$file + break 1 +done + +echo "Found $MOSTRECENT_OPTIMIZATION_FILE" + +echo "-----------------------------------------------------------" +echo "Find the most recent controller calibration file." + +FILENAME=$HELLO_FLEET_PATH/$HELLO_FLEET_ID"/calibration_ros/controller_calibration_head_" + +for file in `ls $FILENAME*.yaml | sort -r`; do + MOSTRECENT_CONTROLLER_FILE=$file + break 1 +done + +echo "Found $MOSTRECENT_CONTROLLER_FILE" + +echo "-----------------------------------------------------------" +echo "Find the most recent calibrated URDF file." + +FILENAME=$HELLO_FLEET_PATH/$HELLO_FLEET_ID"/calibration_ros/head_calibrated_" + +for file in `ls $FILENAME*.urdf | sort -r`; do + MOSTRECENT_URDF=$file + break 1 +done + +echo "Found $MOSTRECENT_URDF" + +echo "-----------------------------------------------------------" + +echo "Launch the visualization using these three files." +echo "roslaunch stretch_calibration visualize_head_calibration.launch optimization_result_yaml_file:=$MOSTRECENT_OPTIMIZATION_FILE calibrated_controller_yaml_file:=$MOSTRECENT_CONTROLLER_FILE calibrated_urdf_file:=$MOSTRECENT_URDF" + +roslaunch stretch_calibration check_head_calibration.launch optimization_result_yaml_file:=$MOSTRECENT_OPTIMIZATION_FILE calibrated_controller_yaml_file:=$MOSTRECENT_CONTROLLER_FILE calibrated_urdf_file:=$MOSTRECENT_URDF diff --git a/stretch_calibration/nodes/collect_head_calibration_data b/stretch_calibration/nodes/collect_head_calibration_data index 9beafa2..f496964 100755 --- a/stretch_calibration/nodes/collect_head_calibration_data +++ b/stretch_calibration/nodes/collect_head_calibration_data @@ -26,6 +26,8 @@ import time import threading import sys +import argparse as ap + import numpy as np import ros_numpy @@ -250,7 +252,7 @@ class CollectHeadCalibrationDataNode: - def calibrate_pan_and_tilt(self): + def calibrate_pan_and_tilt(self, collect_check_data=False): calibration_data = [] @@ -275,8 +277,12 @@ class CollectHeadCalibrationDataNode: min_pan_angle_rad = -3.8 max_pan_angle_rad = 1.3 - number_of_tilt_steps = 3 #4 - number_of_pan_steps = 5 #7 + if collect_check_data: + number_of_tilt_steps = 2 #4 + number_of_pan_steps = 3 #7 + 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) @@ -315,8 +321,13 @@ class CollectHeadCalibrationDataNode: rospy.loginfo('*************************************') rospy.loginfo('') - tilt_angles_rad = [-0.3, 0.38] - pan_angles_rad = [-3.8, -1.66, 1.33] + + if collect_check_data: + tilt_angles_rad = [-0.38] + pan_angles_rad = [-3.8, 1.33] + else: + 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 @@ -391,9 +402,11 @@ class CollectHeadCalibrationDataNode: 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) - 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) + + 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 @@ -410,9 +423,11 @@ class CollectHeadCalibrationDataNode: 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) - 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) + + 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) ####################################### @@ -450,21 +465,39 @@ class CollectHeadCalibrationDataNode: wrist_focused_min_tilt = -1.2 wrist_focused_max_tilt = -0.5 - num_wrist_focused_pan_steps = 2 #3 - num_wrist_focused_tilt_steps = 2 #3 + + 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) - 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] + 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) + 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)) @@ -501,7 +534,10 @@ class CollectHeadCalibrationDataNode: 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) - filename = self.calibration_directory + 'head_calibration_data_' + capture_date + '.yaml' + 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) @@ -525,7 +561,7 @@ class CollectHeadCalibrationDataNode: self.move_to_pose(initial_pose) - def main(self): + 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)) @@ -579,14 +615,19 @@ class CollectHeadCalibrationDataNode: self.move_to_initial_configuration() - self.calibrate_pan_and_tilt() + self.calibrate_pan_and_tilt(collect_check_data) -if __name__ == '__main__': - +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() + node.main(collect_check_data) except rospy.ROSInterruptException: pass diff --git a/stretch_calibration/nodes/process_head_calibration_data b/stretch_calibration/nodes/process_head_calibration_data index c33e9bf..fb6eac5 100755 --- a/stretch_calibration/nodes/process_head_calibration_data +++ b/stretch_calibration/nodes/process_head_calibration_data @@ -206,10 +206,13 @@ class HeadCalibrator: rgba = [1.0, 0.0, 1.0, 0.2] self.error_measures.append(ca.ArucoError('base_right', 'base', 'link_aruco_right_base', aruco_urdf, self.meters_per_deg, rgba)) - def load_data(self, parameters): + def load_data(self, parameters, use_check_calibration_data=False): # Load data to use for calibration. - - filenames = glob.glob(self.calibration_directory + 'head_calibration_data' + '_*[0-9].yaml') + + if use_check_calibration_data: + filenames = glob.glob(self.calibration_directory + 'check_head_calibration_data' + '_*[0-9].yaml') + else: + filenames = glob.glob(self.calibration_directory + 'head_calibration_data' + '_*[0-9].yaml') filenames.sort() most_recent_filename = filenames[-1] self.head_calibration_data_filename = most_recent_filename @@ -280,7 +283,6 @@ class HeadCalibrator: parameter_names.extend(['head_assembly_offset_xyz_x', 'head_assembly_offset_xyz_y', 'head_assembly_offset_xyz_z', 'head_assembly_offset_rotvec_x', 'head_assembly_offset_rotvec_y', 'head_assembly_offset_rotvec_z', - 'pan_assembly_offset_xyz_x', 'pan_assembly_offset_xyz_y', 'pan_assembly_offset_xyz_z', 'pan_assembly_offset_rotvec_x', 'pan_assembly_offset_rotvec_y', 'pan_assembly_offset_rotvec_z', 'tilt_assembly_offset_xyz_x', 'tilt_assembly_offset_xyz_y', 'tilt_assembly_offset_xyz_z', @@ -776,7 +778,7 @@ class ProcessHeadCalibrationDataNode: } - def main(self): + def main(self, use_check_calibration_data): rospy.init_node('process_head_calibration_data') self.node_name = rospy.get_name() @@ -841,7 +843,7 @@ class ProcessHeadCalibrationDataNode: if self.visualize_only: print('Loading the most recent data file.') - self.calibrator.load_data(fit_parameters) + self.calibrator.load_data(fit_parameters, use_check_calibration_data=use_check_calibration_data) print('Visualizing how well the model fits the data.') print('Wait to make sure that RViz has time to load.') rospy.sleep(5.0) @@ -979,12 +981,15 @@ if __name__ == '__main__': parser.add_argument('--load_prev', action='store_true', help='Do not perform an optimization and instead load the most recent CMA-ES optimization results.') parser.add_argument('--only_vis', action='store_true', help='Only visualize the fit of the CMA-ES optimization results. This does not save any results.') parser.add_argument('--no_vis', action='store_true', help='Do not calculate or publish any visualizations. This results in faster fitting.') + parser.add_argument('--check', action='store_true', help='Use data collected to check the current calibration, instead of data collected to fit a new calibration.') + args, unknown = parser.parse_known_args() opt_results_file_to_load = args.load load_most_recent_opt_results = args.load_prev visualize_only = args.only_vis turn_on_visualization = not args.no_vis - + use_check_calibration_data = args.check + node = ProcessHeadCalibrationDataNode(opt_results_file_to_load = opt_results_file_to_load, load_most_recent_opt_results = load_most_recent_opt_results, visualize_only = visualize_only, visualize=turn_on_visualization) - node.main() + node.main(use_check_calibration_data)