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()
def test_fk_ik():
length = {
'coxaLength': 26,
'femurLength': 42,
'tibiaLength': 63
}
channels = [0, 1, 2]
leg = Leg(length, channels)
angles = [0, -70, -90]
pts = leg.fk(*angles)
angles2 = leg.ik(*pts)
pts2 = leg.fk(*angles2)
# angles2 = [r2d(a), r2d(b), r2d(c)]
print('angles (orig):', angles)
print('pts from fk(orig): {:.2f} {:.2f} {:.2f}'.format(*pts))
print('angles2 from ik(pts): {:.2f} {:.2f} {:.2f}'.format(*angles2))
print('pts2 from fk(angle2): {:.2f} {:.2f} {:.2f}'.format(*pts2))
# print('diff:', np.linalg.norm(np.array(angles) - np.array(angles2)))
print('diff [mm]: {:.2f}'.format(np.linalg.norm(pts - pts2)))
time.sleep(1)