from motion import motion
# for windows
if name == 'nt':
CST_LSS_Port = "COM3"
# for mac and linux(here, os.name is 'posix')
else:
CST_LSS_Port = "/dev/ttyUSB0"
# Constants
#CST_LSS_Port = "/dev/ttyUSB0" # For Linux/Unix platforms
# CST_LSS_Port = "COM1" # For windows platforms
CST_LSS_Baud = lssc.LSS_DefaultBaud
# Create and open a serial port
lss.initBus(CST_LSS_Port, CST_LSS_Baud)
# Create LSS objects
mt = motion(lss)
# define our clear function
def clear():
# for windows
if name == 'nt':
_ = system('cls')
# for mac and linux(here, os.name is 'posix')
else:
_ = system('clear')
from math import atan2, sqrt, cos, sin, acos, pi, copysign
import time
import os
from gtts import gTTS
import VisionModule as vm
import lss
import lss_const as lssc
import platform
import Interface as inter
if platform.system() == 'Windows':
port = "COM3"
elif platform.system() == 'Linux':
port = "/dev/ttyUSB0"
lss.initBus(port, lssc.LSS_DefaultBaud)
base = lss.LSS(1)
shoulder = lss.LSS(2)
elbow = lss.LSS(3)
wrist = lss.LSS(4)
gripper = lss.LSS(5)
allMotors = lss.LSS(254)
allMotors.setAngularHoldingStiffness(0)
allMotors.setMaxSpeed(100)
base.setMaxSpeed(60)
shoulder.setMotionControlEnabled(0)
elbow.setMotionControlEnabled(0)
CST_ANGLE_MIN = -90
CST_ANGLE_MAX = 90
def run_thread(self):
global state
# Initialize serial bus
if platform.system() == "Windows":
# Eric
comport = 'COM7'
# # Riley
# comport = 'COM4'
else:
comport = "/dev/ttyUSB0"
baudrate = lssc.LSS_DefaultBaud
print("Connecting to LSS")
lss.initBus(comport, baudrate)
fingers = [
Finger([lss.LSS(11), lss.LSS(12),
lss.LSS(13)], 0),
Finger([lss.LSS(21), lss.LSS(22),
lss.LSS(23)], 0),
Finger([lss.LSS(31), lss.LSS(32),
lss.LSS(33)], 0),
Finger([lss.LSS(41), lss.LSS(42),
lss.LSS(43)], 0),
Finger([lss.LSS(51), lss.LSS(52),
lss.LSS(53)], 0)
]
fangles = [
0, 72 / 180 * pi, 72 * 2 / 180 * pi, 72 * 3 / 180 * pi,
72 * 4 / 180 * pi
]
radius = 69
hand = Hand(fingers, fangles, radius)
print("Reseting Hand")
hand.reset()
hand.changehandcolor(0)
print("Loading point cloud")
x, y, z, data0, data1, data2 = pickle.load(
open("IK_pointcloud.1.p", "rb"))
# Local Point Cloud Interpolation Generation
# finger = Finger([lss.LSS(69)], 0)
# gridpoints = np.array([100, 100, 50])
# gridstart = np.array([-100, -100, 100])
# gridend = np.array([100, 100, 220])
# x, y, z, pointcloud = TrajectoryGen.cubicpointcloudgen(gridstart, gridend, gridpoints, plotFlag=True)
# ikinehelper = lambda pd, guess: finger.ikine(pd, guess)
# data0, data1, data2 = TrajectoryGen.inversepointcloud(x, y, z, ikinehelper)
# pickle.dump([x, y, z, data0, data1, data2], open("IK_pointcloud.p", "wb"))
# Generate global interpolation functions for each finger
interpfun = TrajectoryGen.interpfungen(x, y, z, data0, data1, data2)
interpfunglobal = []
offset = [-1.5, 0, -2, 0, 0]
print("Generating interpolation functions")
for i in range(0, len(hand.fingers)):
temp = TrajectoryGen.interpfungenglobal(np.linalg.inv(hand.Tas[i]),
interpfun, offset[i])
interpfunglobal.append(temp)
# Create Spin Trajectories
points = 800
trajoffset = 2 * pi / 180
spinr = 125
spinh = 175
spindepth = 30
spintraj = []
spinitraj = []
spinitraj2 = []
print("Creating Spin Trajectories")
for i in range(0, len(hand.fingers)):
hand.changehandcolor(i)
print("Spin trajectory for finger " + str(i + 1))
spintraj.append(
TrajectoryGen.spin(spinr,
spinh,
spindepth,
points,
4,
i,
trajoffset,
plotflag=False))
# ikinehelper = lambda pd, guess: hand.ikine(pd, guess, fingernum=i)
# spinitraj.append(TrajectoryGen.inverse(traj[i], ikinehelper, plotFlag=1))
spinitraj2.append(
TrajectoryGen.inverse(spintraj[i],
interpfunglobal[i],
plotFlag=False))
# _ = input("ready to start: ")
print("Moving Hand to Neutral position")
hand.changehandcolor(1)
for j in range(0, 5):
hand.move(spinitraj2[j][:, 0], j)
# hand.move(np.array([0, 0, 0]), j)
time.sleep(3)
print(
"Motion Controller Ready. Place plate on claw and press enter to continue."
)
while True:
# Modes:
# 0 = pause
# 1 = spin
# 2 = linear correction
# 3 = shut off motors
if mode == 0:
hand.changehandcolor(4)
if mode == 1:
hand.changehandcolor(6)
state = 1
for i in range(0, max(spinitraj2[0].shape)):
for j in range(0, 5):
if mode == 0:
hand.changehandcolor(4)
while mode == 0:
time.sleep(0.1)
hand.changehandcolor(4)
hand.changehandcolor(6)
if mode == 3:
break
hand.move(spinitraj2[j][:, i], j)
if mode == 2:
hand.changehandcolor(5)
# Center
# Move to center finger positions one at a time
state = 2
lintraj1 = []
ilintraj1 = []
epostracker = [0, 0, 0, 0, 0]
for i in range(0, 5):
linpoints1 = 375
spos = spintraj[i][:, 0]
eposl = np.array([0, spinr - 69, spinh])
T_ = hand.Tas[i]
temp_ = np.array(eposl).reshape(3, 1)
postemp_ = np.vstack((temp_, 1))
temp2_ = T_ @ postemp_
epos = temp2_[0:3]
epos = epos.reshape(3)
epostracker[i] = epos
if i == 4:
lintraj1.append(
TrajectoryGen.linear(spos,
epos,
linpoints1,
plotflag=False))
ilintraj1.append(
TrajectoryGen.inverse(lintraj1[i],
interpfunglobal[i],
plotFlag=False))
else:
lintraj1.append(
TrajectoryGen.linear_parabolicz(spos,
epos,
linpoints1,
30,
plotflag=False))
ilintraj1.append(
TrajectoryGen.inverse(lintraj1[i],
interpfunglobal[i],
plotFlag=False))
# Move all fingers in target direction
lintraj2 = []
ilintraj2 = []
for i in range(0, len(hand.fingers)):
spos = epostracker[i]
epos = spos - correction
epostracker[i] = epos
linpoints2 = 125
lintraj2.append(
TrajectoryGen.linear(spos,
epos,
linpoints2,
plotflag=False))
ilintraj2.append(
TrajectoryGen.inverse(lintraj2[i],
interpfunglobal[i],
plotFlag=False))
# Move all fingers back to starting spin position
lintraj3 = []
ilintraj3 = []
for i in range(0, len(hand.fingers)):
spos = epostracker[i]
epos = spintraj[i][:, 0]
epos = epos.reshape(3)
linpoints3 = 375
lindepth = 30
if i == 4:
lintraj3.append(
TrajectoryGen.linear(spos,
epos,
linpoints3,
plotflag=False))
ilintraj3.append(
TrajectoryGen.inverse(lintraj3[i],
interpfunglobal[i],
plotFlag=False))
else:
lintraj3.append(
TrajectoryGen.linear_parabolicz(spos,
epos,
linpoints3,
lindepth,
plotflag=False))
ilintraj3.append(
TrajectoryGen.inverse(lintraj3[i],
interpfunglobal[i],
plotFlag=False))
for i in range(4, -1, -1):
for j in range(0, max(lintraj1[0].shape)):
hand.move(ilintraj1[i][:, j], i)
if mode == 0:
hand.changehandcolor(4)
while mode == 0:
time.sleep(0.1)
hand.changehandcolor(4)
hand.changehandcolor(5)
if mode == 3:
break
for i in range(0, max(lintraj2[0].shape)):
for j in range(0, 5):
hand.move(ilintraj2[j][:, i], j)
if mode == 0:
hand.changehandcolor(4)
while mode == 0:
time.sleep(0.1)
hand.changehandcolor(4)
hand.changehandcolor(5)
if mode == 3:
break
for i in range(0, 5):
for j in range(0, max(lintraj3[0].shape)):
hand.move(ilintraj3[i][:, j], i)
if mode == 0:
hand.changehandcolor(4)
while mode == 0:
time.sleep(0.1)
hand.changehandcolor(4)
hand.changehandcolor(5)
if mode == 3:
break
if mode == 3:
state = 3
for i in range(0, len(hand.fingers)):
hand.fingers[i].limp()
lss.closeBus()
return
pass
