use trac-ik to preprocess joint angles using calibrated urdf

src/main1.py

@@ -13,11 +13,15 @@ from trajectory_msgs.msg import (
 )
 
 ## Our libraries
-from utils import load_plan, JTraj2ROS, speed_multiplier
+from utils import load_plan, JTraj2ROS, speed_multiplier, calibration_correction
 from Controller import Controller
 #import gcode2contour as gc
 
 
+# NEED TO CHANGE
+robot_desc_param = "/r1/robot_description"
+
+
 def execute_plan(plan,
         controller,
         contours=None,
@@ -60,7 +64,7 @@ def execute_plan(plan,
         # Extruding with trajectory
         retries = 0
         if t.contour is not None and contours is not None:
-            traj2send = speed_multiplier(t, 1/contour_speed)
+            traj2send = calibration_correction(speed_multiplier(t, 1/contour_speed), robot_desc_param)
             while not controller.exec_ctraj(
                     traj2send,
                     contour=contours[t.contour],
@@ -70,7 +74,7 @@ def execute_plan(plan,
 
         # Movement only
         else:
-            traj2send = speed_multiplier(t, 1/move_speed)
+            traj2send = calibration_correction(speed_multiplier(t, 1/move_speed), robot_desc_param)
             while not controller.exec_ctraj(traj2send, wait=False) \
                     and retries < max_traj_retries:
                 retries += 1
@@ -91,10 +95,10 @@ if __name__ == '__main__':
     #execute_plan(plan2, con, contours=flexirex_contours, confirm=True, n_trajs=10)
 
     # Test calibration
-    #con = Controller(robot=1, disable_extruder=True) # r1 when both arms running
+    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=1)
+    #con = Controller(disable_extruder=True)         # any when one arm  running
+    execute_plan(calibration_test, con, confirm=True,arm=0)
 
     # go to origin and stay (to check calibration)
     #plan05 = copy.deepcopy(plan1)

src/utils/planning_tools.py

@@ -2,15 +2,23 @@
 Useful functions
 """
 import numpy as np
+from copy import deepcopy
+# pip install git+https://github.com/cambel/ur_ikfast@3c9c8b5a1400ea7946b609a4097adae9c8d3c7a8
+from ur_ikfast import ur_kinematics
+
 
 # ROS Libraries
 import rospy
+from trac_ik_python.trac_ik import IK
 from std_msgs.msg import Bool
 from trajectory_msgs.msg import (
     JointTrajectoryPoint,
     JointTrajectory
 )
 
+
+ur_default = ur_kinematics.URKinematics('ur10')
+
 class JTrajectory(object):
 
     def __init__(self, pos, times, robot, contour=None):
@@ -75,6 +83,23 @@ def JTraj2ROS(jtraj,joint_names=default_joint_names):
     return j
 
 
+def calibration_correction(jtraj,robot_desc_param):
+    """
+    modify joint angles from default ur10 ikfast to calibrated arm
+    """
+    urdf_str = rospy.get_param(robot_desc_param)
+    ik_solver = IK("world", "tool0", urdf_string=urdf_str)
+
+    cal_jtraj = deepcopy(jtraj)
+
+    for i in range(len(jtraj.positions)):
+        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")
+    return cal_jtraj
+
+
 def speed_multiplier(jtraj, multiplier):
     """
     used for slowing down contour execution for testing