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)