catch double time stamps and track-ik failures better

src/main2.py

@@ -111,7 +111,7 @@ def execute_plan(plan,
 
 
 if __name__ == '__main__':
-    plan2arms = load_plan('y')
+    plan2arms = load_plan('CuteOcto_contour')
 
     r1_con = Controller(robot=1, disable_extruder=True)
     r2_con = Controller(robot=2, disable_extruder=True)

src/utils/planning_tools.py

@@ -75,6 +75,12 @@ def JTraj2ROS(jtraj,joint_names=default_joint_names):
     j.joint_names = joint_names
 
     for i in range(len(jtraj.positions)):
+        if i>0 and jtraj.time[i-1] >= jtraj.time[i]:
+            print("Caught Double time stamp")
+            if not np.allclose(jtraj.positions[i-1],jtraj.positions[i]):
+                raise Exception("Time stamps equal but position changed")
+            # catch double time stamps
+            continue
         jp = JointTrajectoryPoint()
         jp.positions = jtraj.positions[i]
         jp.time_from_start = rospy.Duration(jtraj.time[i])
@@ -83,7 +89,7 @@ def JTraj2ROS(jtraj,joint_names=default_joint_names):
     return j
 
 
-def calibration_correction(jtraj, robot_desc_param, max_attempts = 10):
+def calibration_correction(jtraj, robot_desc_param, max_attempts=50):
     """
     modify joint angles from default ur10 ikfast to calibrated arm
     """
@@ -94,13 +100,22 @@ def calibration_correction(jtraj, robot_desc_param, max_attempts = 10):
 
     for i in range(len(jtraj.positions)):
         attempts = 0
-        cal_dist = 1
+        cal_dist = 10
         intended_pose = ur_default.forward(jtraj.positions[i])
-        while cal_dist>0.1 and attempts < max_attempts:
+        while cal_dist>0.2 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:
+            temp = list(ik_solver.get_ik(jtraj.positions[i], *intended_pose))
+            cal_dist_new = np.linalg.norm(np.array(jtraj.positions[i]) - temp)
+            if cal_dist_new < cal_dist:
+                cal_dist = cal_dist_new
+                cal_jtraj.positions[i] = temp
+
+        if cal_dist>0.2:
+            # cal_jtraj.positions[i] = jtraj.positions[i]
+            print("Failed to find a close solution - using closest")
+            print("Shit could go south")
+            print(cal_dist)
+        if cal_dist>1:
             raise Exception("trac-ik sol too far away from original joints, dist={}, attempts={}".format(cal_dist, attempts))
     return cal_jtraj