def main(): s0 = Servo(0) s1 = Servo(1) s2 = Servo(2) s0.all_stop() for i in range(0, 2): loop(s0, s1, s2) s0.all_stop()
def test_full_fk_ik(c=[0, 1, 2]):
length = {
'coxaLength': 26,
'femurLength': 42,
'tibiaLength': 63
}
channels = c
leg = Leg(length, channels)
servorange = [[-90, 90], [-90, 90], [-180, 0]]
for s in range(0, 3):
leg.servos[s].setServoRangeAngle(*servorange[s])
for i in range(1, 3):
for a in range(-70, 70, 10):
angles = [0, 0, -10]
if i == 2: a -= 90
angles[i] = a
pts = leg.fk(*angles)
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): {} {}'.format(angles[0], angles[1], angles[2], angle_error, pos_error))
leg.move(*pts)
time.sleep(0.1)
Servo.all_stop()
