def target_euclidean(leg, position, a3=False): angles = Finesse.inverse_pack((7.5, 7.5), position, a3=a3) if angles is not None: leg[0].set_target(angles[0]) leg[1].set_target(angles[1]) leg[2].set_target(angles[2]) else: print('Unable to reach position (%s, %s, %s)!' % position) go_home(leg)
def moveToEuclidean(position, a3=False): angles = Finesse.inverse_pack((7.5, 7.5), position, a3=a3) if angles is not None: servo4.set_target(angles[0]) servo5.set_target(angles[1]) servo6.set_target(angles[2]) maestro.end_together(servos, update=True, t=200) else: print('Unable to reach position (%s, %s, %s)!' % position) goHome()
def jump(): agility.zero() time.sleep(3) a0 = Finesse.inverse_pack(robot.legs[0].lengths, (0, 0, -12)) a1 = Finesse.inverse_pack(robot.legs[1].lengths, (0, 0, -12)) instructions = [] instructions.append((IR.MOVE, 0, a0, 500)) instructions.append((IR.MOVE, 1, a1, 500)) instructions.append((IR.WAIT_ALL,)) a0 = Finesse.inverse_pack(robot.legs[0].lengths, (0, 0, -15)) a1 = Finesse.inverse_pack(robot.legs[1].lengths, (0, 0, -15)) a2 = Finesse.inverse_pack(robot.legs[2].lengths, (14, 0, 0)) a3 = Finesse.inverse_pack(robot.legs[3].lengths, (14, 0, 0)) instructions.append((IR.MOVE, 0, a0, 1)) instructions.append((IR.MOVE, 1, a1, 1)) instructions.append((IR.MOVE, 2, a2, 130)) instructions.append((IR.MOVE, 3, a3, 130)) instructions.append((IR.WAIT_ALL,)) agility.execute_ir(instructions)
def lift(leg): instructions = [] target = (15, 0, 0) agility.zero() time.sleep(3) angles = Finesse.inverse_pack(robot.legs[leg].lengths, target) instructions.append((IR.MOVE, leg, angles, 0)) instructions.append((IR.WAIT_ALL,)) agility.execute_ir(instructions)
def transform(): instructions = [] target = (-15, 0, 0) agility.zero() time.sleep(3) for leg in range(2, 4): angles = Finesse.inverse_pack(robot.legs[leg].lengths, target) instructions.append((IR.MOVE, leg, angles, 0)) instructions.append((IR.WAIT_ALL,)) agility.execute_ir(instructions)