multiple trac-ik attempts when it finds incorrect sol

src/main1.py

@@ -43,6 +43,10 @@ def execute_plan(plan,
     # plan to the start position of the plan
     controller.group.go(plan.trajs[arm][skip_first].positions[0])
 
+    # sanity check calibration correction works for full print before starting
+    for t in plan.trajs[arm]:
+        calibration_correction(t, robot_desc_param)
+
     # execute trajectories in plan
     for i, t in enumerate(plan.trajs[arm]):
         print("---")
@@ -88,7 +92,7 @@ def execute_plan(plan,
 
 
 if __name__ == '__main__':
-    calibration_test = load_plan('trace')
+    calibration_test = load_plan('r1_sq')
     #plan2, flexirex_contours = load_plan('flexirex', contours=True)
 
     # Printing
@@ -99,7 +103,7 @@ if __name__ == '__main__':
     con = Controller(robot=1, disable_extruder=True) # r1 when both arms running
     #con = Controller(robot=2, disable_extruder=True) # r2 when both arms running
     #con = Controller(disable_extruder=True)         # any when one arm  running
-    execute_plan(calibration_test, con, confirm=True,arm=robot_num)
+    execute_plan(calibration_test, con, confirm=False,arm=robot_num)
 
     # go to origin and stay (to check calibration)
     #plan05 = copy.deepcopy(plan1)

src/utils/planning_tools.py

@@ -83,7 +83,7 @@ def JTraj2ROS(jtraj,joint_names=default_joint_names):
     return j
 
 
-def calibration_correction(jtraj,robot_desc_param):
+def calibration_correction(jtraj, robot_desc_param, max_attempts = 10):
     """
     modify joint angles from default ur10 ikfast to calibrated arm
     """
@@ -93,10 +93,15 @@ def calibration_correction(jtraj,robot_desc_param):
     cal_jtraj = deepcopy(jtraj)
 
     for i in range(len(jtraj.positions)):
+        attempts = 0
+        cal_dist = 1
         intended_pose = ur_default.forward(jtraj.positions[i])
-        cal_jtraj.positions[i] = list(ik_solver.get_ik(jtraj.positions[i], *intended_pose))
-        if sum((np.array(jtraj.positions[i]) - np.array(cal_jtraj.positions[i]))**2)>0.1:
-            raise Exception("trac-ik solution too far away from original joints")
+        while cal_dist>0.1 and attempts < max_attempts:
+            attempts += 1
+            cal_jtraj.positions[i] = list(ik_solver.get_ik(jtraj.positions[i], *intended_pose))
+            cal_dist = sum((np.array(jtraj.positions[i]) - np.array(cal_jtraj.positions[i]))**2)**0.5
+        if cal_dist>0.1:
+            raise Exception("trac-ik sol too far away from original joints, dist={}, attempts={}".format(cal_dist, attempts))
     return cal_jtraj