main2.py 3.94 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
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,
25
26
27
        move_speed=1,
        retry_time=0.1,
        max_traj_retries=20):
28
29
30
31
32
33
34
35
36
37
38
39
    """
    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
40
    plan_len = max([a[-1] for a in plan.cumul_time.values()])/move_speed
41
42
43

    s = time.time()
    expected_end_time = s + plan_len
44
45
46
    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
47
    arm_complete= None
48
49
50
    while time.time() < expected_end_time + max(delays):

        print("----------")
51
52

        # Find which arm and which trajectory is next to be executed
53
54
55
56
57
        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
58
        i_r[arm] += 1
59

60
        # if the last trajectory for this arm has already been sent
61
        if i_r[arm] == len(plan.trajs[arm]):
62
63
64
65
66
67
68
69
            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

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

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

79
        print("Printing {}th trajectory of arm {}, which is contour {}".format(
80
            i_r[arm], arm, t.contour
81
            ))
82
        retries = 0
83
84

        # Extruding with trajectory
85
        if t.contour is not None and contours is not None:
86
            traj2send = speed_multiplier(t, 1/move_speed)
87
            while not controllers[arm].exec_ctraj(
88
89
                    traj2send,
                    contour=contours[t.contour],
90
91
92
                    wait=False) and retries < max_traj_retries:
                retries += 1
                time.sleep(retry_time)
93
94

        # Movement only
95
        else:
96
            traj2send = speed_multiplier(t, 1/move_speed)
97
98
99
100
101
102
            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:
103
            print("\nFailed after {} retries, quitting".format(max_traj_retries))
104
            sys.exit(0)
105
106
107

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

115
116
117
118
    return


if __name__ == '__main__':
119
    plan2arms = load_plan('octox2')
120
121
122
123

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

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