def test_fk_ik2():
length = {
'coxaLength': 26,
'femurLength': 42,
'tibiaLength': 63
}
channels = [0, 1, 2]
limits = [[-90, 90], [-90, 90], [-180, 0]]
offsets = [150, 150, 150+90]
leg = Leg(length, channels, DummySerial('test_port'), limits, offsets)
angles = [0, -90, 0]
# angles = [0, 45, -145]
# angles = [0, 0, 0]
pts = leg.fk(*angles)
angles2 = leg.ik(*pts)
pts2 = leg.fk(*angles2)
# angles2 = [r2d(a), r2d(b), r2d(c)]
print('angles (orig):', angles, 'deg')
print('pts from fk(orig): {:.2f} {:.2f} {:.2f} mm'.format(*pts))
print('angles2 from ik(pts): {:.2f} {:.2f} {:.2f} deg'.format(*angles2))
print('pts2 from fk(angle2): {:.2f} {:.2f} {:.2f} mm'.format(*pts2))
# print('diff:', np.linalg.norm(np.array(angles) - np.array(angles2)))
print('diff [mm]: {:.2f}'.format(np.linalg.norm(pts - pts2)))
print('diff [deg]: {:.2f}'.format(sqrt((angles[0]-angles2[0])**2+(angles[1]-angles2[1])**2+(angles[2]-angles2[2])**2)))
def test_full_fk_ik2():
length = {
'coxaLength': 26,
'femurLength': 42,
'tibiaLength': 63
}
channels = [0, 1, 2]
serial = DummySerial('test_port')
limits = [[-90, 90], [-90, 90], [-180, 0]]
offset = [150, 150, 150+90]
leg = Leg(length, channels, serial, limits, offset)
# servorange = [[-90, 90], [-90, 90], [-180, 0]]
# for s in range(0, 3):
# leg.servos[s].setServoRangeAngle(*servorange[s])
delta = 15
for a in range(-45, 45, delta):
for b in range(-45, 90, delta):
for g in range(-160, 0, delta):
print('------------------------------------------------')
angles = [a, b, g]
pts = leg.fk(*angles)
angles2 = leg.ik(*pts)
if angles2 is None:
print('Bad Pos: {:.1f} {:.1f} {:.1f}'.format(pts[0], pts[1], pts[2]))
continue
pts2 = leg.fk(*angles2)
angle_error = np.linalg.norm(np.array(angles) - np.array(angles2))
pos_error = np.linalg.norm(pts - pts2)
# print(angle_error, pos_error)
if angle_error > 0.0001:
print('Angle Error')
printError(pts, pts2, angles, angles2)
exit()
elif pos_error > 0.0001:
print('Position Error')
printError(pts, pts2, angles, angles2)
exit()
def test_full_fk_ik():
length = {
'coxaLength': 26,
'femurLength': 42,
'tibiaLength': 63
}
channels = [0, 1, 2]
serial = DummySerial('test_port')
limits = [[-90, 90], [-90, 90], [-180, 0]]
offset = [150, 150, 150]
leg = Leg(length, channels, serial, limits, offset)
for i in range(1, 3):
print('------------------------------------------------')
for a in range(limits[i][0], limits[i][1], 10):
angles = [0, 0, -10]
# if i == 2: a -= 90
angles[i] = a
pts = leg.fk(*angles)
if not pts.all():
continue
angles2 = leg.ik(*pts)
pts2 = leg.fk(*angles2)
angle_error = np.linalg.norm(np.array(angles) - np.array(angles2))
pos_error = np.linalg.norm(pts - pts2)
# print(angle_error, pos_error)
if angle_error > 0.0001:
print('Angle Error')
printError(pts, pts2, angles, angles2)
exit()
elif pos_error > 0.0001:
print('Position Error')
printError(pts, pts2, angles, angles2)
exit()
else:
print('Good: {} {} {} error(deg,mm): {:.4} {:.4}\n'.format(angles[0], angles[1], angles[2], angle_error, pos_error))
leg.move(*pts)
def run():
# sport = '/dev/serial0'
sport = None
if sport:
ser = ServoSerial(sport)
else:
ser = DummySerial('test')
Servo.ser = ser # set static servo serial comm
leg = Leg([1, 2, 3])
foot0 = leg.foot0
print('foot0', foot0)
angles = [0, 0, 0]
pts = leg.fk(*angles) # allows angles out of range
print('pts', pts)
leg.moveFoot(*pts)
sit = Pose(leg.foot0)
sit.run()
time.sleep(2)
print('Bye')
pkt = Packet.makeServoPacket(ID, 150)
serial.sendPkt(pkt)
sleep(1)
# modify these for your system
ID = xl320.XL320_BROADCAST_ADDR # I seem to get better performance on braodcast
# port = '/dev/tty.usbserial-A5004Flb'
# port = '/dev/tty.usbserial-A700h2xE'
# port = '/dev/tty.usbserial-A5004Fnd'
# port = '/dev/tty.usbserial-A5004Flb'
port = '/dev/tty.usbserial-AL034G2K'
speed = 0.5 # 50% speed
if port is 'dummy':
serial = DummySerial(port) # use this for simulation
else:
serial = ServoSerial(port)
serial.open()
pkt = Packet.makeServoSpeedPacket(ID, speed) # set servo speed
serial.sendPkt(pkt)
try:
while True:
run(ID, serial, 300, 0, 10)
print('>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>')
run(ID, serial, 0, 300, -10)
print('<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<')
sleep(2)
# parser.add_argument('-g', '--gpio', help='Raspberry Pi GPIO pin number', type=int, default=17)
args = vars(parser.parse_args())
return args
args = handleArgs()
port = args['port']
angle = args['angle']
if port:
print('Opening: {}'.format(port))
serial = ServoSerial(port) # use this if you want to talk to real servos
else:
print('Using dummy serial port for testing')
serial = DummySerial(port) # use this for simulation
serial.open()
data = [(1, angle), (2, angle), (3, angle), (4, angle), (5, angle), (6, angle),
(7, angle), (8, angle), (9, angle), (10, angle), (11, angle),
(12, angle)]
pkt = Packet.makeBulkAnglePacket(data)
serial.write(pkt) # there is no returned status packet, so just write
# serial.write(pkt)
print('Raw Packet:', pkt)
serial.close()
#!/usr/bin/env python
# from pyxl320 import ServoSerial
from pyxl320 import Packet
from pyxl320 import DummySerial
ID = 1
port = '/dev/tty.usbserial-A5004Flb'
angle = 158.6
# serial = ServoSerial(port) # use this if you want to talk to real servos
serial = DummySerial(port) # use this for simulation
serial.open()
pkt = Packet.makeServoPacket(ID, angle) # move servo 1 to 158.6 degrees
err_no, err_str = serial.sendPkt(pkt) # send packet to servo
if err_no:
print('Oops ... something went wrong!')