def run(robot):
# start pose
rot1 = rpy2r([0, 0, 0], unit='deg')
tran1 = [0.5, 0.5, 0.5]
start = pose.SE3(tran1[0], tran1[1], tran1[2], rot1)
# auxiliary poses
rot2 = rpy2r([0, 0, 0], unit='deg')
tran2 = [0.1, 0.1, 0.0]
mid = pose.SE3(tran2[0], tran2[1], tran2[2], rot2)
mid2 = pose.SE3(tran2[0], tran2[1], tran2[2] + 0.5, rot2)
# final pose
rot3 = rpy2r([0, 0, 0], unit='deg')
tran3 = [0.1, 0.1, 0.0]
stop = pose.SE3(tran3[0], tran3[1], tran3[2], rot3)
# compute movements
path1 = move_j(robot, start, mid)
path2 = move_lin(robot, mid, mid2)
path3 = move_j(robot, mid2, stop)
# print size of paths
print(path1.shape)
print(path2.shape)
print(path3.shape)
# create final path
path = np.concatenate((path1, path2, path3), axis=0)
# animate robot
robot.animate(stances=path, frame_rate=30, unit='deg')
def addPoint(robot, path, start, stop):
middle = [stop[0], start[1], start[2],
start[3], start[4], start[5]]
# middle = [stop[0], start[1], 0., 0., 0., 0.]
path1 = move_j(robot, start, middle)
path2 = move_j(robot, middle, stop)
path = np.concatenate((path, path1, path2), axis=0)
return path
def run(robot):
# define joint positions
start = [0.0, 90.0, 0.0, 0.0, 0.0, 0.0]
middle = [90.0, 90.0, 0.0, 0.0, 0.0, 0.0]
stop = [90.0, 90.0, 30.0, 0.0, 0.0, 0.0]
# compute path
path1 = move_j(robot, start, middle)
path2 = move_j(robot, middle, stop)
# concatenate entire path
path = np.concatenate((path1, path2), axis=0)
print(path)
# animate robot
robot.animate(stances=path, frame_rate=30, unit='deg')
def moveRobot(self, stateNames):
model = robot.Puma560() # defining robot as Puma 560
jointStates = [self.joints[x] for x in stateNames
] # getting joints for each given state
paths = []
for i in range(0, len(jointStates) - 1):
path = move_j(model, jointStates[i], jointStates[i + 1])
paths.append(path)
path = np.concatenate(paths, axis=0)
print(path)
model.animate(stances=path, unit='deg', frame_rate=30)
def run(robot, pathToExecute):
# compute path
all_points = []
for idx, config in enumerate(pathToExecute):
if idx <= (len(pathToExecute) - 2):
Path = move_j(robot, config, pathToExecute[idx + 1])
all_points.append(Path)
# concatenate entire path
path = np.concatenate(tuple(all_points), axis=0)
print(path)
# animate robot
robot.animate(stances=path, frame_rate=30, unit='deg')
def init(robot, path):
start = [90.0, 90.0, 0.0, 0.0, 0.0, 0.0]
path1 = move_j(robot, start, start)
path = np.concatenate((path1), axis=0)
return start, path
def run():
master_states = [State(**opt) for opt in options]
form_to = [[0, [1, 9]], [1, [0, 2, 9]], [2, [3, 9]], [3, [4, 9]],
[4, [1, 5, 6, 7, 9]], [5, [1, 9]], [6, [8, 9]], [7, [8, 9]],
[8, [1, 9]], [9, [10]], [10, [0, 1, 2, 3, 4, 5, 6, 7, 8]]]
master_transitions = {}
for indices in form_to:
from_idx, to_idx_tuple = indices
for to_idx in to_idx_tuple:
op_identifier = "m_{}_{}".format(from_idx, to_idx)
transition = Transition(master_states[from_idx],
master_states[to_idx],
identifier=op_identifier)
master_transitions[op_identifier] = transition
master_states[from_idx].transitions.append(transition)
input = input_sequence
paths = [path_bases[key] for key in input]
init_state = "idle"
path_array = []
start = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
previous_position = start
for path in paths:
supervisor = Generator.create_master(master_states, master_transitions)
print('\n' + str(supervisor))
print("Executing path: {}".format(path))
for event in path:
master_transitions[event]._run(supervisor)
print(supervisor.current_state)
if supervisor.current_state.value == "idle":
path1 = move_j(robot, previous_position, start)
previous_position = start
path_array.append(path1)
print("Supervisor done!")
if supervisor.current_state.value == "scan":
path1 = move_j(robot, previous_position, scan)
previous_position = scan
path_array.append(path1)
if supervisor.current_state.value == "grip":
path1 = move_j(robot, previous_position, grip)
previous_position = grip
path_array.append(path1)
if supervisor.current_state.value == "evaluate":
path1 = move_j(robot, previous_position, evaluate)
previous_position = evaluate
path_array.append(path1)
if supervisor.current_state.value == "trash":
path1 = move_j(robot, previous_position, trash)
previous_position = trash
path_array.append(path1)
if supervisor.current_state.value == "transport_a":
path1 = move_j(robot, previous_position, transport_a)
previous_position = transport_a
path_array.append(path1)
if supervisor.current_state.value == "transport_b":
path1 = move_j(robot, previous_position, transport_b)
previous_position = transport_b
path_array.append(path1)
if supervisor.current_state.value == "detach":
if previous_position == trash:
path1 = move_j(robot, previous_position, detach)
previous_position = detach
elif previous_position == transport_a:
path1 = move_j(robot, previous_position, detach_a)
previous_position = detach_a
elif previous_position == transport_b:
path1 = move_j(robot, previous_position, detach_b)
previous_position = detach_b
path_array.append(path1)
if random.randint(0, 100) > 90:
idx = options_idx[supervisor.current_state.value]
master_transitions[f'm_{idx}_9']._run(supervisor)
print(supervisor.current_state)
master_transitions['m_9_10']._run(supervisor)
print(supervisor.current_state)
print("Monkeys are repairing machine...")
n = 100
for i in range(n):
if i % 10 == 0:
print("Repairing machine ({}%)".format(100 * i // n))
time.sleep(0.01)
master_transitions[f'm_{10}_{idx}']._run(supervisor)
print("Recovered from fatal crash!")
print(supervisor.current_state)
path = np.concatenate(path_array, axis=0)
model.animate(stances=path, frame_rate=30, unit='deg')