def test_Quadcopter_yaw(self):
quad = Quadcopter()
# only two motors on, to produce a yaw
quad.set_thrust(0, 10, 0, 10)
quad.run(t=1)
assert quad.rpy.yaw > 0
assert quad.rpy.pitch == 0
assert quad.rpy.roll == 0
#
# now try to yaw in the opposite direction
quad.reset()
quad.set_thrust(10, 0, 10, 0)
quad.run(t=1)
assert quad.rpy.yaw < 0
assert quad.rpy.pitch == 0
assert quad.rpy.roll == 0
def run(self, creature):
quad = Quadcopter()
quad.position = (0, 0, self.z0)
z_setpoint = self.z1 # first task: go to setpoint (0, 0, z1)
fitness = 0
while quad.t < self.total_t:
## if quad.t >= 2:
## # switch to second task
## z_setpoint = self.z2
#
inputs = [z_setpoint, quad.position.z]
outputs = creature.run_step(inputs)
assert len(outputs) == 1
pwm = outputs[0]
quad.set_thrust(pwm, pwm, pwm, pwm)
quad.step(self.dt)
fitness += self.compute_fitness(quad, z_setpoint)
self.show_step(quad)
return fitness
def test_lift(self):
quad = Quadcopter(mass=0.5, motor_thrust=0.5)
assert quad.position == (0, 0, 0)
assert quad.rpy == (0, 0, 0)
#
# power all the motors, to lift the quad vertically. The motors give a
# total acceleration of 4g. Considering the gravity, we have a total
# net acceleration of 3g.
t = 1 # second
g = 9.81 # m/s**2
z = 0.5 * (3 * g) * t**2 # d = 1/2 * a * t**2
#
quad.set_thrust(1, 1, 1, 1)
quad.run(t=1, dt=0.0001)
pos = quad.position
assert pos.x == 0
assert pos.y == 0
assert pos.z == approx(z, rel=1e-3) # the simulated z is a bit
# different than the computed one
assert quad.rpy == (0, 0, 0)