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 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
def run(robot):
rot1 = rpy2r([0, 0, 0], unit='deg')
tran1 = [0.5, 0.5, 0.5]
start = pose.SE3(tran1[0], tran1[1], tran1[2], rot1)
print(start)
rot2 = rpy2r([0, 0, 0], unit='deg')
tran2 = [-0.2, .5, -.2]
stop = pose.SE3(tran2[0], tran2[1], tran2[2], rot2)
print(stop)
# plot poses if needed
# pose.SE3(tran1[0], tran1[1], tran1[2], rot1).plot()
# pose.SE3(tran2[0], tran2[1], tran2[2], rot2).plot()
path = move_lin(robot, start, stop)
# animate robot
robot.animate(stances=path, frame_rate=30, unit='deg')
def animate_robot(robot, start, path, i):
if i != 0:
i2 = i / 2
else:
i2 = i
rot1 = rpy2r([0, 0, 0], unit='deg')
tran1 = [0.1, 0.1, 0.1]
path2 = pose.SE3(tran1[0] + i2, tran1[1] + i2, tran1[2] + i2, rot1)
new_path = move_lin(robot, start, path2)
if i == 0:
path = new_path
else:
path = np.concatenate((path, new_path), axis=0)
i += 1
print(i)
return path2, path, i
rot = rpy2r([0, 0, 0], unit='deg')
tran = [-0.2, 0.0, 1.2]
pos4 = pose.SE3(tran[0], tran[1], tran[2], rot) # Chwytak w punkcie odbioru/nadawania
if testRun:
testPath = move_lin(testRobot, idle, pos1) # move_j nie chce współpracować, move_lin powinien wystarczyć. Dla robota pracującego przy paczkomacie nieprzewidziane ruchy i tak nie są wskazane.
testRobot.animate(testPath, frame_rate=25, unit='deg')
if __name__ == '__main__':
#Uruchomienie poses.py jako skrypt główny spowoduje uruchomienie testowej animacji robota
robot_positions(testRun=True)'''
rot = rpy2r([0, 0, 0], unit='deg')
tran = [0.0, 0.5, 0.5]
idle = pose.SE3(tran[0], tran[1], tran[2], rot) # Pozycja w stanie spoczynku
rot = rpy2r([0, 0, 0], unit='deg')
tran = [0.2, 0.0, 0.5]
pos1 = pose.SE3(tran[0], tran[1], tran[2], rot) # Chwytak przed paczkomatem
rot = rpy2r([0, 0, 0], unit='deg')
tran = [0.2, 0.0, 1.2]
pos2 = pose.SE3(tran[0], tran[1], tran[2], rot) # Chwytak w paczkomacie
rot = rpy2r([0, 0, 0], unit='deg')
tran = [-0.2, 0.0, 0.5]
pos3 = pose.SE3(tran[0], tran[1], tran[2],
rot) # Chwytak przed punktem odbioru/nadawania
rot = rpy2r([0, 0, 0], unit='deg')