main loop controlling both arms simultaneously (#5)

src/Controller.py

@@ -164,8 +164,8 @@ class Controller(object):
         sj = plan.joint_trajectory.points[0].positions
         if any([0.01 < abs(cj[i]-sj[i]) for i in range(6)]):
             print("Potentially would've failed here, Retrying")
-            #time.sleep(0.1)
-            self.group.go(plan.joint_trajectory.points[0].positions, wait=True)
+            time.sleep(0.1)
+            #self.group.go(plan.joint_trajectory.points[0].positions, wait=True)
             return False
 
         if contour is not None:

src/main2.py

@@ -23,8 +23,7 @@ def execute_plan(plan,
         r2_con,
         contours=None,
         contour_speed=0.2,
-        move_speed=1,
-        n_trajs=None):
+        move_speed=1):
     """
     Executes a plan on MoveIt
 
@@ -37,7 +36,7 @@ def execute_plan(plan,
     controllers = [r1_con, r2_con]
 
     # execute trajectories in plan
-    plan_len = max([a[-1] for a in plan.cumul_time.values()])
+    plan_len = max([a[-1] for a in plan.cumul_time.values()])/move_speed
 
     s = time.time()
     expected_end_time = s + plan_len
@@ -46,26 +45,47 @@ def execute_plan(plan,
 
         # Find which arm and which trajectory is next to be executed
         next_arm = np.argmin([plan.cumul_time[0][i_r[0]], plan.cumul_time[1][i_r[1]]])
-        next_time = min(plan.cumul_time[0][i_r[0]], plan.cumul_time[1][i_r[1]])
+        next_time = min(plan.cumul_time[0][i_r[0]], plan.cumul_time[1][i_r[1]])/move_speed
         i_r[next_arm] += 1
+
+        # if last trajectory has already been sent
+        if i_r[next_arm] == len(plan.trajs[next_arm]):
+            print("Last Trajectory sent, wait for completion")
+            time.sleep(expected_end_time - time.time())
+            print("Complete")
+            break
         t = plan.trajs[next_arm][i_r[next_arm]]
 
         # wait till time to execute next trajectory
-        time.sleep(next_time - (time.time() - s ) - 1) # 1s extra
+        sleep_time = next_time - (time.time() - s )
+        if sleep_time > 0:
+            print("Sleeping for {}s until time for next traj".format(
+                sleep_time
+                ))
+            time.sleep(sleep_time)
 
-        print("Printing {}th trajectory in the plan, which is contour {}".format(i, t.contour))
+        # Execute next Trajectory
+        print("Printing {}th trajectory of arm {}, which is contour {}".format(
+            i_r[next_arm], next_arm, t.contour
+            ))
         if t.contour is not None and contours is not None:
-            controllers[next_arm].exec_ctraj(speed_multiplier(t, 1/contour_speed), contour=contours[t.contour])
+            traj2send = speed_multiplier(t, 1/contour_speed)
+            while not controllers[next_arm].exec_ctraj(
+                    traj2send,
+                    contour=contours[t.contour],
+                    wait=False):
+                pass
         else:
-            controllers[next_arm].exec_ctraj(speed_multiplier(t, 1/move_speed))
-
+            traj2send = speed_multiplier(t, 1/move_speed)
+            while not controllers[next_arm].exec_ctraj(traj2send, wait=False):
+                pass
     return
 
 
 if __name__ == '__main__':
-    plan2arms = load_plan('two_arm_test', contours=True)
+    plan2arms = load_plan('two_arm_test')
 
     r1_con = Controller(disable_extruder=True, robot=1)
     r2_con = Controller(disable_extruder=True, robot=2)
 
-    execute_plan(plan2arms, r1_con, r2_con, trajs=200, move_speed=0.5)
+    execute_plan(plan2arms, r1_con, r2_con, move_speed=0.5)

src/update_test_plans.sh

-# Copy new calibration files to planner
-cp ../calibrations/r1_tforms_alt.yaml ../../twins-planner/src/calibrations/r1_tforms.yaml
-
-# Create new test plans
-cd ../../twins-planner/src
-conda activate planner
-python test_plan_gen.py
-mv *.plan ../../twins-controller/src/plans
-cd ../twins-controller/src/plans