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.

stretch_calibration/launch/check_head_calibration.launch

@@ -0,0 +1,58 @@
+<launch>
+
+  <arg name="optimization_result_yaml_file"/>
+  <arg name="calibrated_controller_yaml_file"/>
+  <arg name="calibrated_urdf_file"/>
+  
+  <arg name="calibration_directory" value="$(env HELLO_FLEET_PATH)/$(env HELLO_FLEET_ID)/calibration_ros/"/>
+  <arg name="uncalibrated_controller_calibration_filename" value="$(find stretch_core)/config/controller_calibration_head_factory_default.yaml"/>
+  <arg name="uncalibrated_urdf_filename" value="$(find stretch_description)/urdf/stretch_uncalibrated.urdf"/>
+  
+  <!-- PROCESS CALIBRATION DATA -->
+  <rosparam command="load" file="$(find stretch_calibration)/config/head_calibration_options.yaml" />
+  <node name="process_head_calibration_data" pkg="stretch_calibration" type="process_head_calibration_data" output="screen" args="--check --only_vis --load $(arg optimization_result_yaml_file)">
+    <param name="calibration_directory" type="string" value="$(arg calibration_directory)"/>
+    <param name="uncalibrated_controller_calibration_filename" type="string" value="$(arg uncalibrated_controller_calibration_filename)"/>
+    <param name="uncalibrated_urdf_filename" type="string" value="$(arg uncalibrated_urdf_filename)"/>
+  </node>
+  <!-- -->
+  
+  <!-- STRETCH DRIVER -->
+  <param name="/stretch_driver/broadcast_odom_tf" type="bool" value="false"/>
+  
+  <param name="robot_description"
+	 textfile="$(arg calibrated_urdf_file)"/>
+  
+  <node
+      name="joint_state_publisher"
+      pkg="joint_state_publisher"
+      type="joint_state_publisher" >
+    <param name="rate" value="15.0"/>
+    <param name="use_gui" value="False"/>
+    <rosparam param="source_list">
+      [/stretch/joint_states]
+    </rosparam>
+  </node>
+
+  <node
+      name="robot_state_publisher"
+      pkg="robot_state_publisher"
+      type="robot_state_publisher" >
+    <param name="publish_frequency" value="15.0"/>
+  </node>
+  
+  <node name="stretch_driver" pkg="stretch_core" type="stretch_driver" output="screen">
+    <param name="rate" type="double" value="25.0"/>
+    <param name="timeout" type="double" value="0.5"/>
+    <remap from="cmd_vel" to="/stretch/cmd_vel" />
+    <remap from="joint_states" to="/stretch/joint_states" />
+    <param name="controller_calibration_file" type="string" value="$(arg calibrated_controller_yaml_file)"/>
+  </node>
+  <!-- -->
+
+  
+  <!-- VISUALIZE CALIBRATION -->
+  <node name="rviz" pkg="rviz" type="rviz" output="screen" args="-d $(find stretch_calibration)/rviz/head_calibration.rviz" />
+  <!-- -->
+  
+</launch>

stretch_calibration/launch/collect_check_head_calibration_data.launch

@@ -0,0 +1,58 @@
+<launch>
+
+  <arg name="uncalibrated_urdf_file" value="$(find stretch_description)/urdf/stretch_uncalibrated.urdf"/>
+  <arg name="uncalibrated_controller_yaml_file" value="$(find stretch_core)/config/controller_calibration_head_factory_default.yaml"/>
+  <arg name="calibration_directory" value="$(env HELLO_FLEET_PATH)/$(env HELLO_FLEET_ID)/calibration_ros/"/>
+  
+  <!-- REALSENSE D435i -->
+  <include file="$(find stretch_core)/launch/d435i_high_resolution.launch"></include>
+  <node name="d435i_configure" pkg="stretch_core" type="d435i_configure" output="screen">
+    <param name="initial_mode" type="string" value="High Accuracy"/>
+  </node>
+  <!-- -->
+
+  <!-- STRETCH DRIVER -->
+  <param name="/stretch_driver/broadcast_odom_tf" type="bool" value="false"/>
+  <param name="/stretch_driver/fail_out_of_range_goal" type="bool" value="false"/>
+
+  <param name="robot_description"
+	 textfile="$(arg uncalibrated_urdf_file)"/>
+  
+  <node
+      name="joint_state_publisher"
+      pkg="joint_state_publisher"
+      type="joint_state_publisher" >
+    <param name="rate" value="15.0"/>
+    <rosparam param="source_list">
+      [/stretch/joint_states]
+    </rosparam>
+  </node>
+
+  <node
+      name="robot_state_publisher"
+      pkg="robot_state_publisher"
+      type="robot_state_publisher" >
+    <param name="publish_frequency" value="15.0"/>
+  </node>
+  
+  <node name="stretch_driver" pkg="stretch_core" type="stretch_driver" output="screen">
+    <param name="rate" type="double" value="25.0"/>
+    <param name="timeout" type="double" value="0.5"/>
+    <remap from="cmd_vel" to="/stretch/cmd_vel" />
+    <remap from="joint_states" to="/stretch/joint_states" />
+    <param name="controller_calibration_file" type="string" value="$(arg uncalibrated_controller_yaml_file)"/>
+  </node>
+  <!-- -->
+
+  <!-- ARUCO MARKER DETECTOR -->
+  <include file="$(find stretch_core)/launch/stretch_aruco.launch"></include>
+  <!-- -->
+
+  <!-- COLLECT CALIBRATION DATA -->
+  <node name="collect_head_calibration_data" pkg="stretch_calibration" type="collect_head_calibration_data" output="screen" args="--check">
+    <param name="controller_calibration_file" type="string" value="$(arg uncalibrated_controller_yaml_file)"/>
+    <param name="calibration_directory" type="string" value="$(arg calibration_directory)"/>
+  </node>
+  <!-- -->
+  
+</launch>

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

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
 

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)