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()