In [1]:
import numpy as np
from scipy.optimize import fsolve
from matplotlib import pyplot as plt
from math import pi
In [2]:
def jacobian(f, x):
eps = 1e-6
jac = np.zeros((len(x),len(x)))
y = f(x)
for i in range(len(x)):
x[i] = x[i] + eps
jac[:,i] = (f(x) - y) / eps
x[i] -= eps
return jac
print jacobian(lambda x: np.array([x[1], x[2], -(2*x[2] + 2*x[1] + 1*(x[0] - 1.0))]), np.array([0.0,0.0,0.0]))
In [3]:
class Drone:
def __init__(self):
# Components
self.frame = Frame(self) # Main structure holding the other components
self.legs = Legs(self) # Landing legs
self.battery = Battery(self) # Lithium-Polymer battery
self.motors = Motors(self) # Four DC brushless motors
self.propellers = Propellers(self) # Four propellers
self.electronics = Electronics(self) # Main board, sensors, wires & Electronic Speed Controllers
self.payload = Payload(self) # Arbitrary payload (e.g. camera)
self.components = [self.frame, self.legs, self.battery, self.motors, self.propellers, self.electronics, self.payload]
# State
self.time = 0.0 # s # time
self.x = 0.0 # m # x-coordinate (center of gravity)
self.y = 0.0 # m # y-coordinate (center of gravity)
self.teta = 0.0 # rad # angle w.r.t horizontal
self.vx = 0.0 # m/s # speed along the x-axis
self.vy = 0.0 # m/s # speed along the y-axis
self.omega = 0.0 # rad/s # rotation rate
self.energy = self.battery.capacity / 1000.0 # Ah
# Command
self.throttle = (0.0, 0.0)
self.drag_coefficient = 1 # Simplistic model - Only meant to provide a reasonnable order of magnitude
# Simulation
self.controller = None
self.step = 0.01
def atmosphere(self):
temperature = 288.15 - 6.5 * self.y / 1000 # Standard temperature
pressure = 101325 * (1 - 0.00065 * self.y / 288.15)**5.2561 # Standard pressure
rho = pressure / (287.04 * temperature) # Perfect gas formula
return (rho, temperature, pressure)
# Geometry
def draw(self):
mass, cg, inertia = self.get_mass()
forces = self.weight(mass,cg) + self.thrust() + self.drag(cg) # List the forces acting on the drone
forces += self.gears(mass, cg, forces) # Apply gear forces (that require the other forces as an input)
return [shape for c in self.components for shape in c.draw(self.time)] + \
[plt.Circle(mass[1], 0.005) for mass in self.get_masses()] + \
[plt.Arrow(f[1][0], f[1][1], 0.01*self.rel_abs(f[0])[0], 0.01*self.rel_abs(f[0])[1], width=0.1) for f in forces]
def get_plottable_variables(self):
return [var for c in self.components for var in c.get_plottable_variables()]
def reference_area(self):
return 4 * self.propellers.disk_area()
def rel_abs(self, vector):
return np.array([ np.cos(self.teta)*vector[0]+np.sin(self.teta)*vector[1],
-np.sin(self.teta)*vector[0]+np.cos(self.teta)*vector[1]])
def abs_rel(self, vector):
return np.array([np.cos(self.teta)*vector[0]-np.sin(self.teta)*vector[1],
np.sin(self.teta)*vector[0]+np.cos(self.teta)*vector[1]])
# Mass
def get_masses(self):
return [mass for c in self.components for mass in c.get_masses()]
def get_mass(self):
return reduce(self.merge_masses, self.get_masses())
def merge_masses(self, m1,m2):
mass1, cg1, inertia1 = m1
mass2, cg2, inertia2 = m2
mass = mass1 + mass2
cg_x = (cg1[0] * mass1 + cg2[0] * mass2) / mass
cg_y = (cg1[1] * mass1 + cg2[1] * mass2) / mass
i1 = self.transport_inertia(m1, (cg_x,cg_y))
i2 = self.transport_inertia(m2, (cg_x,cg_y))
return (mass, (cg_x, cg_y), i1+i2)
def transport_inertia(self, m, cg):
return m[2] + m[0] * ((cg[0]-m[1][0])**2 + (cg[1]-m[1][1])**2)
# Forces
def weight(self, mass, cg):
return [(np.array([0.0, -mass * 9.81]), np.array(cg))]
def thrust(self):
self.motors.calculate_state()
x = self.frame.width * 0.5 - self.motors.diameter * 0.5
y = self.frame.height * 0.5
y_direction = self.abs_rel(np.array([0.0,1.0]))
return [
(2.0 * self.motors.thrust1 * y_direction, np.array([x,y])),
(2.0 * self.motors.thrust2 * y_direction, np.array([-x,y]))
]
def drag(self, cg):
rho, t, p = self.atmosphere()
speed = (self.vx**2 + self.vy**2)**0.5
if speed == 0:
return [(np.array((0,0)),np.array((0,0)))]
speed_direction = np.array([self.vx, self.vy]) / speed
return [(- 0.5 * rho * speed**2 * self.reference_area() * self.drag_coefficient * speed_direction, np.array(cg))]
def gears(self, mass, cg, forces):
cg_abs = np.array([self.x, self.y]) # absolute coordinates of CG
ref_abs = cg_abs - self.abs_rel(cg) # absolute coordinates of reference point
force_on_x = sum([f[0][0] for f in forces]) # Summing the forces on the X axis
max_friction = force_on_x + mass * self.vx / self.step # maximum friction force to avoid stability problems
return self.legs.ground_reactions(ref_abs, max_friction)
def torque(self, force, cg):
f, f_point = force # Force and point of application
return np.cross(self.abs_rel(f_point-cg), f) # Using cross-product formula
# Newton's equations
def state(self):
return {s: getattr(self,s) for s in ['time','x','y','teta','vx','vy','omega','energy']}
def dstate(self):
mass, cg, inertia = self.get_mass()
forces = self.weight(mass,cg) + self.thrust() + self.drag(cg) # List the forces acting on the drone
forces += self.gears(mass, cg, forces) # Apply gear forces (that require the other forces as an input)
torque = sum([self.torque(f, cg) for f in forces]) # Summing the torques
force_on_x = sum([f[0][0] for f in forces]) # Summing the forces on the X axis
force_on_y = sum([f[0][1] for f in forces]) # Summing the forces on the Y axis
return {
'time' : 1.0,
'x' : self.vx,
'y' : self.vy,
'teta' : self.omega,
'vx': force_on_x / mass,
'vy': force_on_y / mass,
'omega': torque / inertia,
'energy': - (2.0 * self.motors.electrical_power1 + 2.0 * self.motors.electrical_power2) / 3600.0
}
def balance(self):
x0 = np.array(self.throttle + (self.teta,))
def func(x):
self.throttle = (x[0], x[1])
self.teta = x[2]
dstate = self.dstate()
y = np.array([dstate['vx'], dstate['vy'], dstate['omega']])
return y
return fsolve(func, x0)
def simu_step(self):
if self.controller:
self.controller(self.objectives)
# Runge-Kutta 2 integration method
state = self.state()
dstate1 = self.dstate()
for key, val in dstate1.iteritems():
setattr(self, key, state[key] + val * self.step * 0.5)
dstate2 = self.dstate()
for key, val in dstate2.iteritems():
setattr(self, key, state[key] + val * self.step)
# Controls
def direct_control(self, obj):
t1, t2 = obj
self.throttle = (min(max(t1, 0.0),1.0), min(max(t2, 0.0),1.0))
def crossed_control(self, obj):
throttle, diff = obj
self.direct_control((throttle+diff, throttle-diff))
def speed_control(self, obj=(0.0, 0.0)):
vx_target, vy_target = obj
# Calculate gains of speed control law
if not hasattr(self, 'k_vx'):
mass, cg, inertia = self.get_mass()
k_mot = 11.7 # N/throttle/motor (measured from motor graphs)
self.k_speed = 2.0
self.lk_j = self.frame.width*0.5 * 4 * k_mot / inertia
self.throttle0 = self.throttle[0]
# Vertical speed control
self.k_vy = -0.1
# Horizontal speed control
self.k_omega = - (2*self.k_speed) / self.lk_j
self.k_teta = (2*self.k_speed**2) / self.k_omega / self.lk_j
self.k_vx = (self.k_speed**3) / self.k_omega / self.k_teta / self.lk_j / 9.81
throttle = self.throttle0 + self.k_vy * (self.vy - vy_target)
teta_target = self.k_vx * (self.vx - vx_target)
teta_target = max(teta_target,min(teta_target, 45*np.pi/180.0),-45*np.pi/180.0)
omega_target = self.k_teta * (self.teta - teta_target)
diff = self.k_omega * (self.omega - omega_target)
self.crossed_control((throttle, diff))
def position_control(self, obj=(0.0, 0.0)):
x_target, y_target = obj
# Calculate gains of position control law (based on speed control law)
if not hasattr(self, 'k_x'):
self.speed_control()
self.k_y = -1.0
self.k_x = -(self.k_speed**4)/4.0 / self.k_vx / self.k_omega / self.k_teta / self.lk_j / 9.81
vx_target = self.k_x * (self.x - x_target)
vy_target = self.k_y * (self.y - y_target)
self.speed_control((vx_target, vy_target))
#drone = Drone()
#drone.vx = 0.0
#drone.throttle = (0.8,0.5)
#print drone.thrust()
#print [drone.torque(f, np.array([0,0])) for f in drone.thrust()]
#print drone.balance()
#drone.vx = 1.0 # 1m/s
#t1,t2,teta = drone.balance()
#print t1, t2, teta * 180/pi
#print 0.5*1.225*4*drone.propellers.disk_area()/(9.81*np.tan(teta))
#print drone.get_mass()
In [4]:
class Component:
# Geometry
def draw(self, time):
return []
def get_plottable_variables(self):
return []
# Mass
def get_masses(self):
return []
In [5]:
class Frame(Component):
"""
Frame component (includes central structure, and arms / Does not include the landing gear)
"""
def __init__(self, drone):
self.drone = drone
self.width = 0.411 # m # Width of the frame (Motor to Motor)
self.height = 0.05 # m # Height of the central frame
self.arm_length = 0.15 # m # Length of an arm
self.arm_thickness = 0.02 # m # Thickness of an arm
# Geometry
def draw(frame, time):
box_width = frame.width - 2 * frame.arm_length # Width of the central structure
x = box_width * 0.5 # Right arm coordinates
y = frame.height * 0.5
return [
plt.Rectangle((-frame.width*0.5, frame.height*0.5-frame.arm_thickness), frame.width, frame.arm_thickness, fc='none'), # Arms
# plt.Polygon(np.array([[-x,y], [-frame.width/2, y], [-x, y-frame.height]]), fc='none'), # Left arm (triangle)
plt.Rectangle((-x, -y), box_width, frame.height, fc='none') # Central structure (rectangle)
]
def get_plottable_variables(self):
return [
(self, 'width', 0.2, 1.0), # name, min, max
(self, 'height', 0.02, 0.2),
(self, 'arm_length', 0.05, 0.5)
]
# Mass
def get_masses(frame):
box_width = frame.width - 2 * frame.arm_length # Width of the central structure
box_mass = 0.100 # Estimated from http://www.hobbyking.com/hobbyking/store/__77174__HMF_U580_Pro_Carbon_Fiber_Folding_Quadcopter_Kit.html
arm_mass = 0.200
arm_cg_x = box_width * 0.5 + frame.arm_length * 0.5
arm_cg_y = frame.height * 0.5 - frame.arm_thickness * 0.5
return [
(box_mass, (0.0,0.0), box_mass * (box_width**2 + frame.height**2) / 12.0),
(arm_mass, (arm_cg_x,arm_cg_y), arm_mass * frame.arm_length**2 / 12.0),
(arm_mass, (-arm_cg_x,arm_cg_y), arm_mass * frame.arm_length**2 / 12.0)
]
In [31]:
class Legs(Component):
"""
Legs component (includes left & right legs)
"""
def __init__(self, drone):
self.drone = drone
self.width = 0.05 # m # Length of a leg
self.height = 0.1 # m # Height of a leg
self.ground_altitude = -0.5 # m # Altitude of ground
self.stiffness = -500.0 # N/m # Stiffness of landing gear -> ~50g/mm
self.damping = -10.0 # N/m/s # Damping of a landing gear leg -> ~1kg/m/s
self.dynamic_friction = 0.7 # - # Dynamic (sliding) friction coefficient
self.static_friction = 1.0 # - # Static friction coefficient
# Geometry
def draw(self, time):
x = self.drone.frame.width * 0.5 - self.drone.frame.arm_length # Right leg coordinates
y = -self.drone.frame.height * 0.5
return [
plt.Polygon(np.array([[x,y], [x+self.width, y-self.height]]), fc='none'),
plt.Polygon(np.array([[-x,y], [-x-self.width, y-self.height]]), fc='none')
]
def get_plottable_variables(self):
return [
(self, 'width', 0, 0.2),
(self, 'height', 0.02, 0.2)
]
# Mass
def get_masses(self):
mass = 0.100 # Mass per leg (Kg)
cg_y = -self.drone.frame.height * 0.5 - self.height * 0.5
cg_x = self.drone.frame.width * 0.5 - self.drone.frame.arm_length + self.width * 0.5
inertia = mass * (self.width**2 + self.height**2) / 12.0 # Inertia of a bar of length (width^2 + height^2)^0.5
return [
(mass, (cg_x,cg_y), inertia), # Right leg
(mass, (-cg_x,cg_y), inertia), # Left leg
]
# Ground model
def ground_reactions(self, ref_abs, max_friction):
# Right contact point coordinates (body frame)
x_r = self.drone.frame.width * 0.5 - self.drone.frame.arm_length + self.width
y = -self.drone.frame.height * 0.5 - self.height
# Preliminary calculations
pos = np.array([self.drone.x, self.drone.y]) # Position vector
v = np.array([self.drone.vx, self.drone.vy, 0.0]) # Velocity vector
omega = np.array([0.0, 0.0, self.drone.omega]) # Rotation vector
contacts = [np.array([x,y]) for x in [x_r,-x_r] ] # Contact points relative positions
contacts_abs = [ref_abs + self.drone.abs_rel(c) for c in contacts] # Contact points absolute positions
contacts_v = [v + np.cross(omega, c - pos) for c in contacts_abs] # Contact points velocities
# Y reactions
reactions_y = [self.y_reaction(p_, v_) for p_,v_ in zip(contacts_abs, contacts_v) ]
reaction_y = sum(reactions_y) # total reaction on Y
# X reactions
reactions_x = [self.x_reaction(v, p_, v_, r_y, reaction_y, max_friction) for p_,v_,r_y in zip(contacts_abs, contacts_v, reactions_y) ]
reaction_x = sum(reactions_x) # total reaction on X
reactions = [(np.array((r_x, r_y)), cp) for r_x,r_y,cp in zip(reactions_x, reactions_y, contacts)]
# Check that the horizontal reaction does not exceeds the maximum friction force for a given time step
if np.sign(reaction_x + max_friction) != np.sign(max_friction):
for reaction in reactions:
reaction[0][0] = -max_friction * (reaction[0][0] / reaction_x)
return reactions
# Vertical reaction
def y_reaction(self, p, v):
dy = p[1] - self.ground_altitude # Depth of contact point in the ground
return dy * self.stiffness + v[1] * self.damping if dy<0.0 else 0.0 # Simple spring model
# Horizontal reaction : Coulomb model
def x_reaction(self, v, p, v_cp, r_y, total_r_y, max_friction):
if r_y==0.0:
return 0.0
elif abs(v[0]) <= 0.000001: # The drone does not move on X => STATIC FRICTION
return - np.sign(max_friction) * min(self.static_friction * r_y, abs(max_friction) * r_y / total_r_y)
else: # The drone does move on X => DYNAMIC FRICTION
return - np.sign(v_cp[0]) * self.dynamic_friction * r_y
In [8]:
class Battery(Component):
"""
Battery component (Typical 3S LiPo battery)
Discharge profile assumed ideal (constant nominal voltage)
http://www.atomikrc.com/collections/lipo-batteries/products/venom-20c-3s-2100mah-11-1-lipo-battery-with-universal-plug-system
"""
def __init__(self, drone):
self.drone = drone
self.capacity = 5000 # mAh # Capacity
self.density = 130 # Wh/Kg # Energy content per Kg
self.voltage = 11.1 # V # Voltage
self.dimensions = (0.127, 0.043, 0.029) # mm
self.discharge_rate = 35 # C
self.maximum_current = self.discharge_rate * self.capacity / 1000 # Maximum current
# self.Rb = 0.02 # Ohm # Internal resistance (neglected)
def get_masses(self):
mass = (self.capacity / 1000) * self.voltage / self.density # Ah * V / (Wh/Kg) = Kg
return [
# We assume that the battery is located at the center of the frame
(mass, (0,0), mass * (self.dimensions[0]**2+self.dimensions[1]**2) / 12.0)
]
In [9]:
class Motors(Component):
"""
Motors (4 DC brushless motors)
Hacker A30-24M
"""
def __init__(self, drone):
self.drone = drone
self.power = 350 # W # Maximum power per motor
self.Kv = 680 # rpm/V # Speed constant
self.Ri = 0.077 # Ohm # Internal resistance
self.Io = 0.9 # Amp # No-load current
self.mass = 0.103 # Kg # Mass per motor
self.length = 0.0335 # m # Length of motor
self.diameter = 0.0372 # m # Diameter of motor
def calculate_state(self):
self.rho, temperature, pressure = self.drone.atmosphere() # Atmospheric conditions
t1, t2 = self.drone.throttle
self.v1, self.i1, self.rpm1 = self.current(t1**0.5)
self.v2, self.i2, self.rpm2 = self.current(t2**0.5)
self.electrical_power1 = self.v1 * self.i1 # Electric power left motors
self.electrical_power2 = self.v2 * self.i2 # Electric power right motors
self.thrust1, self.mechanical_power1, self.aero_power1 = self.drone.propellers.thrust(self.rho, self.rpm1)
self.thrust2, self.mechanical_power2, self.aero_power2 = self.drone.propellers.thrust(self.rho, self.rpm2)
def rpm(self, v, i):
return self.Kv * (v - self.Ri * i)
def current(self, pwd):
rpm = 0.0
i = 0.0
v = pwd * self.drone.battery.voltage
while abs(self.rpm(v, i) - rpm) > 0.00001: # Iterations are needed to obtain the current and rpm
rpm = self.rpm(v, i)
_, mechanical_power, _ = self.drone.propellers.thrust(self.rho, rpm)
i = self.Io + mechanical_power / (v - self.Ri * i) # Current in the motor
#ibat = i * throttle # Current drawn from the battery
vbat = self.drone.battery.voltage # - self.drone.battery.Rb * ibat # Voltage of the battery (resistance neglected)
v = pwd * vbat # Actual voltage in the motor
return (v, i, rpm)
# Geometry
def draw(self, time):
x = self.drone.frame.width * 0.5 - self.diameter * 0.5
y = self.drone.frame.height * 0.5
return [
plt.Rectangle((x-self.diameter*0.5, y), self.diameter, self.length, fc='none'),
plt.Rectangle((-x-self.diameter*0.5, y), self.diameter, self.length, fc='none')
]
# Mass
def get_masses(self):
cg_x = self.drone.frame.width * 0.5 - self.diameter * 0.5
cg_y = self.drone.frame.height * 0.5 + self.length * 0.5
inertia = 2 * self.mass * (self.diameter**2 + self.length**2) / 12
return [
(2 * self.mass, (cg_x,cg_y), inertia), # Right motor
(2 * self.mass, (-cg_x,cg_y), inertia), # Left motor
]
In [10]:
class Propellers(Component):
"""
Propellers
"""
def __init__(self, drone):
self.drone = drone
self.diameter = 12 * 0.0254 # m # Propeller diameter
self.pitch = 6 * 0.0254 # m # Propeller pitch
self.efficiency = 1.08 # - # Efficiency coefficient
self.mass = 0.046 # Kg # Mass per propeller
self.thickness = 0.01 # m
def disk_area(self):
return pi * (self.diameter * 0.5)**2
def thrust(self, rho, n):
pitch_speed = n * self.pitch / 60
thrust = 0.5 * rho * self.disk_area() * pitch_speed**2
aero_power = thrust * pitch_speed
mechanical_power = aero_power * self.efficiency
return (thrust, mechanical_power, aero_power)
# Geometry
def draw(self, time):
radius = self.diameter * 0.5 # * np.cos(time*10) # Trick to animate the propeller
x = self.drone.frame.width * 0.5 - self.drone.motors.diameter * 0.5 # Right propeller coordinates
y = self.drone.frame.height * 0.5
y_prop = y + self.drone.motors.length
return [
plt.Polygon(np.array([[x,y], [x, y_prop+self.thickness], [x+radius,y_prop+self.thickness*0.5], [x, y_prop], [x-radius,y_prop+self.thickness*0.5], [x, y_prop+self.thickness]]), fc='none'),
plt.Polygon(np.array([[-x,y], [-x, y_prop+self.thickness], [-x+radius,y_prop+self.thickness*0.5], [-x, y_prop], [-x-radius,y_prop+self.thickness*0.5], [-x, y_prop+self.thickness]]), fc='none')
]
def get_plottable_variables(self):
return [
(self, 'diameter', 2 * 0.0254, 20 * 0.0254)
]
# Mass
def get_masses(self):
x = self.drone.frame.width * 0.5 - self.drone.motors.diameter * 0.5
inertia = 0.25 * self.mass * (self.diameter * 0.5)**2 # Inertia of thin disk
return [
(2 * self.mass, (x, self.drone.frame.height * 0.5 + 0.04), 2 * inertia),
(2 * self.mass, (-x, self.drone.frame.height * 0.5 + 0.04), 2 * inertia)
]
In [11]:
class Electronics(Component):
"""
Main board, sensors, wires & Electronic Speed Controllers
"""
def __init__(self, drone):
self.drone = drone
def get_masses(self):
return [(0.2, [0,0], 0)] # 200g concentrated at the CG location
In [12]:
class Payload(Component):
"""
Arbitrary payload (e.g. camera)
"""
def __init__(self, drone):
self.drone = drone
self.payload = 0.2 # Kg # payload
def get_masses(self):
return [(self.payload, [0,0], 0)] # 200g concentrated at the CG location
In [13]:
from matplotlib import animation
from matplotlib.transforms import Affine2D
from matplotlib.widgets import Slider
class Plotter:
def __init__(self, axis):
self.fig, self.ax = plt.subplots()
plt.axis(axis)
def plot_geom(self, drone):
vars = drone.get_plottable_variables()
plt.subplots_adjust(bottom=0.03*(len(vars)+2))
sliders = [self.add_slider_geom(i*0.03, var[0], var[1], var[2], var[3]) for i,var in enumerate(vars)]
anim = animation.FuncAnimation(self.fig, lambda i:self.add_shapes(drone), frames=1, interval=20)
plt.show()
def plot_simu(self, drone, controller, controls):
drone.controller = controller
plt.subplots_adjust(bottom=0.12)
self.sliders = [self.add_slider(i*0.03, c[0], c[1], c[2], c[3], lambda x: None) for i,c in enumerate(controls)]
anim = animation.FuncAnimation(self.fig, lambda i:self.simu_step(drone), interval= drone.step * 1000)
plt.show()
def add_slider_geom(self, pos, component, name, min, max):
return self.add_slider(pos, component.__class__.__name__+' '+name, getattr(component, name), min, max, lambda val: setattr(component, name, val))
def add_slider(self, pos, label, init, min, max, on_changed):
ax = plt.axes([0.25, 0.02+pos, 0.65, 0.02], axisbg='lightgoldenrodyellow')
s = Slider(ax, label, min, max, valinit=init)
s.on_changed(on_changed)
return s
def simu_step(self, drone):
drone.objectives = [s.val for s in self.sliders]
for _ in range(10): # Rendering every timestep is not useful
drone.simu_step()
return self.add_shapes(drone)
def add_shapes(self, drone):
shapes = drone.draw()
trans = Affine2D.identity()
trans.rotate_deg(drone.teta * 180.0 / pi)
trans.translate(drone.x, drone.y)
trans = trans + self.ax.transData
self.ax.clear()
self.text = self.ax.text(self.ax.get_xlim()[1]-1.0, self.ax.get_ylim()[1]-0.3, 'Time: '+str(drone.time), axes=self.ax)
for shape in shapes:
shape.set_transform(trans)
self.ax.add_patch(shape)
self.ax.add_patch(plt.Rectangle((-10, -10), 100, 9.5, fc='none')) # ground
return shapes
In [32]:
drone = Drone()
drone.throttle = (0.5,0.5)
print drone.balance()
throttle0 = drone.throttle[0]
In [14]:
trajectory = []
drone.time = 0.0
drone.x = 5.0
drone.y = 5.0
drone.vx = 0.0
drone.vy = 0.0
drone.teta = 0.0
drone.omega = 0.0
drone.drag_coefficient = 1.0
drone.balance()
drone.controller = drone.position_control
for i in range(5000):
drone.objectives = (5, 5) if drone.time < 1.0 else (100, 5)
drone.simu_step()
state = drone.state()
state['thrust1'] = drone.motors.thrust1
state['thrust2'] = drone.motors.thrust2
state['teta'] *= 180.0 / np.pi
state['omega'] *= 180.0 / np.pi
trajectory.append(state)
times = [state.pop('time', None) for state in trajectory]
for key,val in trajectory[0].iteritems():
plt.plot(times, [state[key] for state in trajectory], label=key)
plt.legend(loc=2)
plt.show()
In [33]:
drone.x = 0.0
drone.y = 0.0
drone.vx = 0.0
drone.vy = 0.0
drone.teta = 0.0
drone.omega = 0.0
drone.balance()
plotter = Plotter([-0.5, 3.0, -1.0, 3.0])
#plotter.plot_simu(drone, drone.direct_control, [('Throttle right', drone.throttle[0], 0.0, 1.0), ('Throttle left', drone.throttle[1], 0.0, 1.0)])
#plotter.plot_simu(drone, drone.crossed_control, [('Throttle', 0.5 * (drone.throttle[0] + drone.throttle[1]), 0.0, 1.0), ('Diff', 0.0, -0.5, 0.5)])
#plotter.plot_simu(drone, drone.speed_control, [('Horizontal speed', drone.vx, -1.0, 1.0), ('Vertical speed', drone.vy, -1.0, 1.0)])
plotter.plot_simu(drone, drone.position_control, [('X', drone.x, 0.0, 5.0), ('Y', drone.y, -1.0, 5.0)])
#plotter.plot_geom(drone)
In [40]:
x = np.zeros((4))
def drone_state(x):
drone.vx = x[0]
drone.teta = x[1]
drone.omega = x[2]
drone.vy = x[3]
drone.speed_control((0.0, 0.0))
dstate = drone.dstate()
return np.array([dstate[key] for key in ['vx','teta','omega', 'vy']])
print jacobian(drone_state, x)
print np.linalg.eig(jacobian(drone_state, x))
drone.balance()
Out[40]:
In [16]:
masses = drone.get_masses()
def merge_masses(m1,m2):
mass1, cg1, inertia1 = m1
mass2, cg2, inertia2 = m2
mass = mass1 + mass2
cg_x = (cg1[0] * mass1 + cg2[0] * mass2) / mass
cg_y = (cg1[1] * mass1 + cg2[1] * mass2) / mass
i1 = transport_inertia(m1, (cg_x,cg_y))
i2 = transport_inertia(m2, (cg_x,cg_y))
return (mass, (cg_x, cg_y), i1+i2)
def transport_inertia(m, cg):
return m[2] + m[0] * ((cg[0]-m[1][0])**2 + (cg[1]-m[1][1])**2)
print masses
drone_mass = reduce(merge_masses, masses)
print drone_mass
comp_names = [c.__class__.__name__ for c in drone.components]
comp_masses = [reduce(merge_masses, c.get_masses())[0] for c in drone.components]
comp_inertia = [100*transport_inertia(reduce(merge_masses, c.get_masses()), drone_mass[1]) for c in drone.components]
fig2 = plt.figure(figsize=(6,12))
ax2 = fig2.add_subplot(211)
ax3 = fig2.add_subplot(212)
print comp_names,comp_inertia
ax2.pie(comp_masses, labels=comp_names, startangle=90)
ax2.set_title('Component masses')
ax3.pie(comp_inertia, labels=comp_names, startangle=90)
ax3.set_title('Component inertia')
plt.show()
In [17]:
throttle = np.zeros(100)
current = np.zeros(100)
thrust = np.zeros(100)
efficiency = np.zeros(100)
specific_thrust = np.zeros(100)
power = np.zeros(100)
drone.payload.payload = 0.0
for i in range(100):
drone.throttle = (i/100.0, 0.0)
drone.motors.calculate_state()
current[i] = drone.motors.i1
thrust[i] = drone.motors.thrust1
throttle[i] = i/100.0
power[i] = drone.motors.electrical_power1
specific_thrust[i] = thrust[i] / power[i] / 9.81 * 1000
efficiency[i] = drone.motors.mechanical_power1 / power[i] if drone.motors.electrical_power1!=0 else 0
fig1 = plt.figure()
ax1 = fig1.add_subplot(111)
hover_throttle = next(throttle[i] for i in range(100) if thrust[i] * 4 >= drone.get_mass()[0] * 9.81)
ax1.plot([hover_throttle,hover_throttle], [0.0,10], 'r+-', label='Hover')
ax1.plot(throttle, thrust, 'b+-', label='Thrust (N)')
ax1.plot(throttle, current, 'g+-', label='Intensity (A)')
ax1.plot(throttle, power * 0.1, 'm+-', label='Power (0.1W)')
ax1.plot(throttle, efficiency * 10.0, 'r+-', label='Efficiency (x10%)')
ax1.plot(throttle, specific_thrust, 'k+-', label='Specific Thrust (g/W)')
plt.xlabel('Throttle')
plt.grid()
plt.legend(loc=2)
plt.show()
In [18]:
drone.throttle = (1.0,0.0)
print drone.motors.calculate_state()
print drone.atmosphere()