def test_world_frame(self):
pos = (0, 0, 0)
attitude = [0, 0, 0]
quadcopter = Quadcopter(pos, attitude)
frame = quadcopter.world_frame()
# let's simply test the dimension of output matrix for now
self.assertEqual((3, 6), frame.shape)
def test_update(self):
pos = (0, 0, 0)
attitude = [0, 0, 0]
quadcopter = Quadcopter(pos, attitude)
M = np.array([[1, 2, 3]]).T
dt = 0.01
F = 0.0
quadcopter.update(dt, F, M)
def main():
quad = Quadcopter()
plotter = QuadPlotter()
pd_controller = PD()
quad.position = (0, 0, 3) # put it a bit above the ground
dt = 0.01
plotter.add_marker((0, 0, 5), RED)
plotter.add_marker((0, 0, 3), YELLOW)
setpoint = 5
while plotter.show_step():
#P_controller(quad, sp_z=setpoint)
pd_controller(quad, dt, sp_z=setpoint)
#lift_controller(quad)
#yaw_controller(quad)
quad.step(dt)
if quad.t > 3:
setpoint = 3
plotter.update(quad)
def test_state_dot(self):
pos = (0, 0, 0)
attitude = [0, 0, np.pi / 6]
quadcopter = Quadcopter(pos, attitude)
F = 1.831655
M = np.array([[
1.45879171444454e-05, -2.09414787558272e-05, 0.000133122686296450
]]).T
quadcopter.state = np.array([
0.099008422415915, 0.0990164877426797, 0.0989554489338614,
0.257998046804099, 0.257989778737415, 0.257530910432372,
0.998488763307995, 0.0165084622039285, -0.0182176180214827,
-0.0491505705276828, 0.00909049179030169, -0.0110849112574632,
0.257885042514016
])
dt = 0.01
s_dot = quadcopter.state_dot(quadcopter.state, dt, F, M)
print "s_dot", s_dot
def test_run(self):
pos = (0, 0, 0)
attitude = [0, 0, np.pi / 6]
quad = Quadcopter(pos, attitude)
time = 0.0
des_state = trajGen.genLine(time)
F, M = controller.run(quad, des_state)
print "des_state", des_state
print "F", F
print "M", M
def main():
#pdb.set_trace()
pos = (0,0,0)
attitude = (0,0,np.pi/2)
quadcopter = Quadcopter(pos, attitude)
# Simulation Loop
while time[0] <= SIMTIME:
attitudeControl(quadcopter,time)
plt.plot(t,quadcopter.hist)
plt.show()
def test_run_reset(self):
quad = Quadcopter()
assert quad.t == 0
quad.run(t=1)
assert quad.t == approx(1)
quad.reset()
assert quad.t == 0
def main():
pos = (0.5, 0, 0)
attitude = (0, 0, 0)
quadcopter = Quadcopter(pos, attitude)
waypoints = trajGen3D.get_helix_waypoints(sim_time, 9)
(coeff_x, coeff_y, coeff_z) = trajGen3D.get_MST_coefficients(waypoints)
def control_loop(i):
for _ in range(int(control_iterations)):
attitudeControl(quadcopter, time, waypoints, coeff_x, coeff_y,
coeff_z)
return quadcopter.world_frame()
plot_quad_3d(waypoints, control_loop)
def main():
pos = (0.5, 0, 0)
attitude = (0, 0, 0)
quadcopter = Quadcopter(pos, attitude)
sched = scheduler.Scheduler()
waypoints = trajGen3D.get_helix_waypoints(0, 9)
(coeff_x, coeff_y, coeff_z) = trajGen3D.get_MST_coefficients(waypoints)
sched.add_task(attitudeControl, dt,
(quadcopter, time, waypoints, coeff_x, coeff_y, coeff_z))
kEvent_Render = sched.add_event(render, (quadcopter, ))
try:
plt.plot_quad_3d(waypoints, (sched, kEvent_Render))
except KeyboardInterrupt:
print("attempting to close threads.")
sched.stop()
print("terminated.")
def main():
pos = (0.5, 0, 0)
attitude = (0, 0, 0)
quadcopter = Quadcopter(pos, attitude)
waypoints = trajGen3D.get_helix_waypoints(sim_time, 9)
(coeff_x, coeff_y, coeff_z) = trajGen3D.get_MST_coefficients(waypoints)
def control_loop(i):
for _ in range(control_iterations):
attitudeControl(quadcopter, time, waypoints, coeff_x, coeff_y,
coeff_z)
return quadcopter.world_frame()
plot_quad_3d(waypoints, control_loop)
if (True): # save inputs and states graphs
print("Saving figures...")
record("df3_airdrag.jpg")
print("Closing.")
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)
def test_attitude(self):
pos = (0, 0, 0)
attitude = [0, 0, np.pi / 6]
quadcopter = Quadcopter(pos, attitude)
attitude = quadcopter.attitude()
print "attitude", attitude
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