make calibration correction limit a function input

src/utils/planning_tools.py

@@ -92,7 +92,7 @@ def JTraj2ROS(jtraj,joint_names=default_joint_names):
     return j
 
 
-def calibration_correction(jtraj, robot_desc_param, max_attempts=50, save=False):
+def calibration_correction(jtraj, robot_desc_param, max_attempts=50, save=False, max_diff=0.7):
     """
     modify joint angles from default ur10 ikfast to calibrated arm
     """
@@ -116,18 +116,21 @@ def calibration_correction(jtraj, robot_desc_param, max_attempts=50, save=False)
             attempts += 1
             temp = np.array(ik_solver.get_ik(orig_angles, *intended_pose))
             if temp is None or None in temp:
+                print("IK failed")
                 continue
             cal_dist_new = np.linalg.norm(np.array(jtraj.positions[i]) - temp)
-            if cal_dist_new < 0.5 and cal_dist_new < cal_dist:
+            if cal_dist_new < max_diff and cal_dist_new < cal_dist:
                 cal_dist = cal_dist_new
                 cal_jtraj.positions.append(list(temp))
                 cal_jtraj.time.append(jtraj.time[i])
+#            if attempts >1 and cal_dist>max_diff:
+#                print(attempts, cal_dist, cal_dist_new)
         if save:
             writer.writerow([list(jtraj.positions[i]), cal_dist])
 
-        if cal_dist>0.5:
+        if cal_dist>max_diff:
             # cal_jtraj.positions[i] = jtraj.positions[i]
-            print("Failed to find a close solution for waypoint {}".format(i))
+            print("Failed to find a close solution for waypoint {}, cal_dist = {}".format(i, cal_dist))
             print("skipping waypoint")
             n_skipped += 1
             if n_skipped>3: