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)