main2.py 3.96 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
import numpy as np
import pickle
import time
import copy
import sys

# ROS Libraries
import rospy
from std_msgs.msg import Bool
from trajectory_msgs.msg import (
    JointTrajectoryPoint,
    JointTrajectory
)

## Our libraries
from utils import load_plan, JTraj2ROS, speed_multiplier
from Controller import Controller
import gcode2contour as gc


def execute_plan(plan,
        r1_con,
        r2_con,
        contours=None,
        contour_speed=0.2,
26
27
28
        move_speed=1,
        retry_time=0.1,
        max_traj_retries=20):
29
30
31
32
33
34
35
36
37
38
39
40
    """
    Executes a plan on MoveIt

    Single arm only
    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])
    controllers = [r1_con, r2_con]

    # execute trajectories in plan
41
    plan_len = max([a[-1] for a in plan.cumul_time.values()])/move_speed
42
43
44

    s = time.time()
    expected_end_time = s + plan_len
45
46
47
    delays = [0,0] # measure delays of each arm
    relative_delay = 0 # how far arm 0 is behind arm 2
    i_r = [0,0] # index of which traj we're up to for each arm
48
    arm_complete= None
49
50
51
    while time.time() < expected_end_time + max(delays):

        print("----------")
52
53

        # Find which arm and which trajectory is next to be executed
54
55
56
57
58
        if arm_complete is not None:
            arm = 1 - arm_complete # if one arm complete, only need to do other
        else:
            arm = np.argmin([plan.cumul_time[0][i_r[0]], plan.cumul_time[1][i_r[1]]])
        next_time = plan.cumul_time[arm][i_r[arm]]/move_speed
59
        i_r[arm] += 1
60

61
        # if the last trajectory for this arm has already been sent
62
        if i_r[arm] == len(plan.trajs[arm]):
63
64
65
66
67
68
69
70
            print("Last Trajectory for this arm has been sent")
            if arm_complete is None:
                arm_complete = arm # mark this arm as complete
                continue
            else:
                print("All arms done (unless Logic error), leaving main loop")
                break

71
        t = plan.trajs[arm][i_r[arm]]
72

73
        # wait till it's time to execute the next trajectory
74
75
        rel_delay_arm = max(0, -relative_delay * (-1)**arm)
        sleep_time = next_time - (time.time() - s ) + rel_delay_arm
76
77
        if sleep_time > 0:
            print("Sleeping for {}s until time for next traj".format(
78
                sleep_time))
79
            time.sleep(sleep_time)
80

81
        # Execute next trajectory
82
        print("Printing {}th trajectory of arm {}, which is contour {}".format(
83
            i_r[arm], arm, t.contour
84
            ))
85
        retries = 0
86
        if t.contour is not None and contours is not None:
87
            traj2send = speed_multiplier(t, 1/contour_speed)
88
            while not controllers[arm].exec_ctraj(
89
90
                    traj2send,
                    contour=contours[t.contour],
91
92
93
                    wait=False) and retries < max_traj_retries:
                retries += 1
                time.sleep(retry_time)
94
        else:
95
            traj2send = speed_multiplier(t, 1/move_speed)
96
97
98
99
100
101
102
103
            while not controllers[arm].exec_ctraj(traj2send, wait=False) \
                    and retries < max_traj_retries:
                retries += 1
                time.sleep(retry_time)

        if retries == max_traj_retries:
            print("---\nFailed after {} retries, quitting".format(max_traj_retries))
            sys.exit(0)
104
105
106

        # Traj started now, measure delay
        delays[arm] = (time.time() - s ) - next_time
107
        print("Arm {} is behind by {}s when executing {}th Traj".format(
108
109
110
111
112
113
            arm,
            delays[arm],
            i_r[arm]))
        relative_delay = delays[0]-delays[1]
        print("RELATIVE DELAY: arm 0 is {}s behind".format(relative_delay))

114
115
116
117
    return


if __name__ == '__main__':
118
    plan2arms = load_plan('two_arm_test2')
119
120
121
122

    r1_con = Controller(disable_extruder=True, robot=1)
    r2_con = Controller(disable_extruder=True, robot=2)

123
    execute_plan(plan2arms, r1_con, r2_con, move_speed=0.5)