calibration correction for 2 arm plans in main2

src/main1.py

@@ -19,7 +19,8 @@ from Controller import Controller
 
 
 # NEED TO CHANGE
-robot_desc_param = "/r1/robot_description"
+robot_num = 0 # 0 or 1
+robot_desc_param = "/r{}/robot_description".format(robot_num+1)
 
 
 def execute_plan(plan,
@@ -87,7 +88,7 @@ def execute_plan(plan,
 
 
 if __name__ == '__main__':
-    calibration_test = load_plan('test')
+    calibration_test = load_plan('trace')
     #plan2, flexirex_contours = load_plan('flexirex', contours=True)
 
     # Printing
@@ -98,7 +99,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=0)
+    execute_plan(calibration_test, con, confirm=True,arm=robot_num)
 
     # go to origin and stay (to check calibration)
     #plan05 = copy.deepcopy(plan1)

src/main2.py

@@ -13,9 +13,9 @@ 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, calib_corr_single
 from Controller import Controller
-import gcode2contour as gc
+#import gcode2contour as gc
 
 
 def execute_plan(plan,
@@ -32,8 +32,8 @@ def execute_plan(plan,
     skips the first n trajectories in the plan
     """
     # plan to the start position of the plan
-    r1_con.group.go(plan.trajs[0][0].positions[0])
-    r2_con.group.go(plan.trajs[1][0].positions[0])
+    r1_con.group.go(calib_corr_single(plan.trajs[0][0].positions[0],"/r1/robot_description"))
+    r2_con.group.go(calib_corr_single(plan.trajs[1][0].positions[0],"/r2/robot_description"))
     controllers = [r1_con, r2_con]
 
     # execute trajectories in plan
@@ -83,7 +83,7 @@ def execute_plan(plan,
 
         # Extruding with trajectory
         if t.contour is not None and contours is not None:
-            traj2send = speed_multiplier(t, 1/move_speed)
+            traj2send = calibration_correction(speed_multiplier(t, 1/move_speed),"/r{}/robot_description".format(arm+1))
             while not controllers[arm].exec_ctraj(
                     traj2send,
                     contour=contours[t.contour],
@@ -93,7 +93,7 @@ def execute_plan(plan,
 
         # Movement only
         else:
-            traj2send = speed_multiplier(t, 1/move_speed)
+            traj2send = calibration_correction(speed_multiplier(t, 1/move_speed),"/r{}/robot_description".format(arm+1))
             while not controllers[arm].exec_ctraj(traj2send, wait=False) \
                     and retries < max_traj_retries:
                 retries += 1
@@ -117,8 +117,9 @@ def execute_plan(plan,
 
 if __name__ == '__main__':
     #plan2arms, contours = load_plan('octox2', contours=True)
-    plan2arms, contours = load_plan('flatraptor_l1', contours=True)
+    #plan2arms, contours = load_plan('flatraptor_l1', contours=True)
+    plan2arms = load_plan('trace2x')
 
-    r1_con = Controller(robot=1)#, disable_extruder=True)
-    r2_con = Controller(robot=2)#, disable_extruder=True)
-    execute_plan(plan2arms, r1_con, r2_con, move_speed=0.25, contours=contours)
+    r1_con = Controller(robot=1, disable_extruder=True)
+    r2_con = Controller(robot=2, disable_extruder=True)
+    execute_plan(plan2arms, r1_con, r2_con, move_speed=1)#, contours=contours)

src/utils/planning_tools.py

@@ -100,6 +100,19 @@ def calibration_correction(jtraj,robot_desc_param):
     return cal_jtraj
 
 
+def calib_corr_single(j,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)
+    intended_pose = ur_default.forward(j)
+    j_out = list(ik_solver.get_ik(j, *intended_pose))
+    if sum((np.array(j) - np.array(j_out))**2)>0.1:
+        raise Exception("trac-ik solution too far away from original joints")
+    return j_out
+
+
 def speed_multiplier(jtraj, multiplier):
     """
     used for slowing down contour execution for testing