Commit ec4b8b6e authored by Jayant Khatkar's avatar Jayant Khatkar

main loop controlling both arms simultaneously (#5)

parent 2f159411
......@@ -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:
......
......@@ -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)
# 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
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment