better observations for fit check & new fit error test

Checking the calibration now collects fewer higher quality observations.

When checking or performing a new fit, the fit error is checked against a threshold that likely indicates a problem with the calibration.

stretch_calibration/nodes/collect_head_calibration_data

@@ -278,8 +278,8 @@ class CollectHeadCalibrationDataNode:
         max_pan_angle_rad = 1.3
 
         if collect_check_data:
-            number_of_tilt_steps = 2 #4 
-            number_of_pan_steps = 3 #7 
+            number_of_tilt_steps = 1 
+            number_of_pan_steps = 3 
         else: 
             number_of_tilt_steps = 3 #4 
             number_of_pan_steps = 5 #7 
@@ -320,37 +320,33 @@ class CollectHeadCalibrationDataNode:
         rospy.loginfo('COLLECT GLOBAL HEAD LOOKING UP DATA')
         rospy.loginfo('*************************************')
         rospy.loginfo('')
-
         
-        if collect_check_data:
-            tilt_angles_rad = [-0.38]
-            pan_angles_rad = [-3.8, 1.33]
-        else:
+        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)
+            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)
+            # 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))
+            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))
                     
         #######################################
         ##

stretch_calibration/nodes/process_head_calibration_data

@@ -252,6 +252,10 @@ class HeadCalibrator:
             marker_array.markers.extend(self.target_markers)
             self.visualization_markers_pub.publish(marker_array)
 
+    def check_fit_error(self, fit_error):
+        fit_error_threshold = 0.05
+        if fit_error > fit_error_threshold:
+            rospy.logerr('The fit error is unusually high: {0} > {1} (fit_error > fit_error_threshold)'.format(fit_error, fit_error_threshold))
 
     def get_calibration_options(self):
         # Return the current calibration options
@@ -587,8 +591,9 @@ class HeadCalibrator:
     def visualize_fit(self, parameters_to_fit):
         self.visualize = True
         self.infinite_duration_visualization = True
-        self.calculate_error(parameters_to_fit)
-
+        fit_error = self.calculate_error(parameters_to_fit)
+        self.check_fit_error(fit_error)
+        
 
     def update_sample_with_offsets_and_backlash(self, sample):
         # This changes the sample passed as an argument using
@@ -945,6 +950,9 @@ class ProcessHeadCalibrationDataNode:
                 'calibration_data_filename': self.calibrator.head_calibration_data_filename,
                 'error_weights': self.calibrator.error_weights
             }
+
+            fit_error = cma_result['best_parameters_error']
+            self.calibrator.check_fit_error(fit_error)
             
             time_string = ca.create_time_string()