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')