def main():
"""""
1. Set path from /setup/examplePath.py
2. Set sequences for each robot in /automat/StateMachine.py
If you want to check which states your path contains:
use pathFinder.showPath(start_point, end_point)
To generate process graph:
use procesGrpah.generateGraph()
""" ""
# generate graph
# genereateGraph()
# execute statemachine to get the path for robots
statesL, statesR = StateMachineStart()
# load model
if str(args.model).lower() == "puma":
model = robot.Puma560()
elif str(args.model).lower() == "orion":
model = robot.Orion5()
else:
print("Bad model specified. Try again.")
sys.exit()
# Start robot simulations
joint_move.run(model, statesR)
joint_move.run(model, statesL)
def see_end_effector(q1, q2, q3, q4, q5, q6):
robot = model.Puma560()
qe = np.asmatrix([q1, q2, q3, q4, q5, q6])
qs = np.asmatrix([0, 0, 0, 0, 0, 0])
Te = robot.fkine(qe)
Ts = robot.fkine(qs)
N = 100
traj = ut.ctraj(Ts, Te, N)._list
q = np.zeros((N, 6))
for i in range(N):
if i == 0:
q[i, :] = robot.ikine((traj[i]), q0=[0, 0, 0, 0, 0, 0], unit="rad")
else:
q[i, :] = robot.ikine((traj[i]), q0=q[i - 1, :], unit="rad")
print("the final pose of the end effector is \n %r" % (Te))
print("The poses through which it passes are:")
for i in range(N):
print("-----\n", traj[i])
plot_robot(q)
def see_joint_angles(Px, Py, Pz):
robot = model.Puma560()
T = transl(Px, Py, Pz)
qs = robot.ikine(T)
print("-----\n q1 = %r, q2 = %r, q3 = %r, q4 = %r, q5 = %r, q6 = %r" %
(qs[0, 0], qs[0, 1], qs[0, 2], qs[0, 3], qs[0, 4], qs[0, 5]))
plot_robot(qs)
def nad_animate():
testRobot = model.Puma560()
Path9 = move_lin(testRobot, idle, pos3)
Path4 = move_lin(testRobot, pos3, pos4)
Path13 = move_lin(testRobot, pos4, pos1)
Path2 = move_lin(testRobot, pos1, pos2)
Path14 = move_lin(testRobot, pos2, idle)
Path_nad = np.concatenate((Path9, Path4, Path13, Path2, Path14), axis=0)
testRobot.animate(Path_nad, frame_rate=10, unit='deg')
def odb_animate():
testRobot = model.Puma560()
Path1 = move_lin(testRobot, idle, pos1)
Path2 = move_lin(testRobot, pos1, pos2)
Path3 = move_lin(testRobot, pos2, pos3)
Path4 = move_lin(testRobot, pos3, pos4)
Path5 = move_lin(testRobot, pos4, idle)
Path_odb = np.concatenate((Path1, Path2, Path3, Path4, Path4, Path5),
axis=0)
testRobot.animate(Path_odb, frame_rate=10, unit='deg')
def main():
robot = model.Puma560()
a = np.transpose(np.asmatrix(np.linspace(1, -180, 500)))
b = np.transpose(np.asmatrix(np.linspace(1, 180, 500)))
c = np.transpose(np.asmatrix(np.linspace(1, 90, 500)))
d = np.transpose(np.asmatrix(np.linspace(1, 450, 500)))
e = np.asmatrix(np.zeros((500, 1)))
f = np.concatenate((d, b, a, e, c, d), axis=1)
robot.animate(stances=f, frame_rate=30, unit='deg')
def main():
robot = model.Puma560()
a = np.transpose(np.asmatrix(np.linspace(1, -180, 500)))
b = np.transpose(np.asmatrix(np.linspace(1, 180, 500)))
c = np.transpose(np.asmatrix(np.linspace(1, 90, 500)))
d = np.transpose(np.asmatrix(np.linspace(1, 450, 500)))
e = np.asmatrix(np.zeros((500, 1)))
f = np.concatenate((d, b, a, e, c, d), axis=1)
# model that is using forward kinematics, it is angular, interpolation
robot.animate(stances=f, 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 main():
# load model
if str(args.model).lower() == "puma":
model = robot.Puma560()
elif str(args.model).lower() == "orion":
model = robot.Orion5()
else:
print("Bad model specified. Try again.")
sys.exit()
# start exercises - uncomment one
# load_robot_and_display_00.run(model)
#joint_move_01.run(model)
linear_move_02.run(model)
def main():
robot = model.Puma560()
'''zero angle'''
qn = np.matrix([0,0,0,0,0,0])
robot.plot(qn)
'''ready, the arm is straight and vertical''
qn = np.matrix([0,np.pi/2,-np.pi/2,0,0,0])
robot.plot(qn)
'''stretch, the arm is straight and horizontal'''
qn = np.matrix([0,0,-np.pi/2,0,0,0])
robot.plot(qn)
'''nominal, the arm is in a dextrous working pose'''
qn = np.matrix([0,(np.pi/4),-np.pi,0,(np.pi/4),0])
robot.plot(qn)
def main():
robot = model.Puma560()
robot.plot('qn')
def plot_robot(qs):
robot = model.Puma560()
robot.animate(stances=qs, frame_rate=30, unit='rad')
del robot
plt.show()
def automata(path, des_id):
update_graph(visor_pos, visor, "wait")
def start_point(robot):
rot1 = rpy2r([0, 0, 0], unit='deg')
tran1 = [0.0, 0.0, 0.0]
start = pose.SE3(tran1[0], tran1[1], tran1[2], rot1)
rot2 = rpy2r([0, 0, 0], unit='deg')
tran2 = [0.1, 0.1, 0.1]
stop = pose.SE3(tran2[0], tran2[1], tran2[2], rot2)
new_path = move_lin(robot, start, stop)
return new_path, stop
"""
rot1 = rpy2r([0, 0, 0], unit='deg')
tran1 = [0.0, 0, 0]
start = pose.SE3(tran1[0], tran1[1], tran1[2], rot1)
rot1 = rpy2r([0, 0, 0], unit='deg')
tran1 = [0.01, 0.01, 0.01]
stop = pose.SE3(tran1[0], tran1[1], tran1[2], rot1)
path_a = move_lin(robot, start, stop)
"""
#path_a = None
# create a supervisor
supervisor = Generator.create_master(master_states, master_transition)
print(des_path[des_id])
time.sleep(3.0)
des_id = des_id + 1
print('\n' + str(supervisor))
i = 0
# run supervisor for exemplary path
print("Executing path: {}".format(path))
model = robot.Puma560()
path_a, start = start_point(model)
for event in path:
# launch a transition in our supervisor
master_transition[event]._run(supervisor)
events = [
'camera', 'move_base', 'grab_obj', 'move_arm', 'check_box_pos',
'arm_box', 'put_obj'
]
print(supervisor.current_state)
val = supervisor.current_state.value
slave_states = None
slave_transition = None
slave_path = None
if val == "camera":
print("cam")
update_graph(visor_pos, visor, "cam")
slave_states = camera_states
slave_transition = camera_transitions
slave_path = paths_camera
if val == "move_base":
update_graph(visor_pos, visor, "move_b")
slave_states = move_base_states
slave_transition = move_base_transitions
slave_path = paths_move_base
print("move base")
print(path_a)
if val == "grab_obj":
update_graph(visor_pos, visor, "grab")
print("grab obj")
slave_states = grab_obj_states
slave_path = paths_grab_obj
slave_transition = grab_obj_transitions
if val == "move_arm":
update_graph(visor_pos, visor, "move_a")
slave_transition = move_arm_transitions
slave_states = move_arm_states
slave_path = paths_move_arm
print("move arm")
start, path_a, i = animate_robot(model, start, path_a, i)
if val == 'check_box_pos':
update_graph(visor_pos, visor, "check_box_pos")
slave_transition = check_box_pose_transitions
slave_states = check_box_pose_states
slave_path = paths_check_box_pos
print("check box pos")
if val == "arm_box":
update_graph(visor_pos, visor, "is_arm")
slave_transition = arm_box_transitions
slave_states = arm_box_states
slave_path = paths_arm_box
print("arm box")
if val == "put_obj":
update_graph(visor_pos, visor, "put_o")
slave_transition = put_obj_transitions
slave_states = put_obj_states
slave_path = paths_put_obj
print('put obj')
#create slave automata
if slave_states is not None:
slave = Generator.create_master(slave_states, slave_transition)
#for now first path
ss = slave_path[0]
for x in ss:
slave_transition[x]._run(slave)
print(slave.current_state)
time.sleep(1.0)
time.sleep(0.5)
print("")
plt.clf()
model = robot.Puma560()
model.animate(stances=path_a, frame_rate=30, unit='deg')
print('Supervisor done!')
import sys
import numpy as np
import time
from robopy.base import pose
from robopy import rpy2r
from commands.moves import move_lin, move_j
from statemachine import StateMachine, State, Transition
from Generator import Generator
from positions import *
from options import *
from paths import *
from input_seq import *
model = robot.Puma560()
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)
