Skip to content

Instantly share code, notes, and snippets.

@jaeandersson

jaeandersson/quadcopter.py

Last active August 29, 2015 14:06
Show Gist options
  • Save jaeandersson/0f93a39663be991d28a2 to your computer and use it in GitHub Desktop.
Save jaeandersson/0f93a39663be991d28a2 to your computer and use it in GitHub Desktop.
Download ZIP
Quadcopter model by Joris Gillis
Raw
quadcopter.py
from casadi import *
from casadi.tools import *
import numpy
from numpy import cos,sin, vstack, hstack, multiply
class Quadcopter:
"""
Quadcopter model
::
states = [
p_x \\
p_y (p: centre of mass position)
p_z /
v_x \\
v_y (v: centre of mass velocity)
v_z /
q_0 \\
q_1 (q: orientation quaternions)
q_2 /
q_3 /
w_0 \\
w_1 (w: angular velocity)
w_2 /
r_0 \\
r_1 (r: rotor speeds)
r_2 /
r_3 /
]
controls = [
CR_0 \\
CR_1 (CR: motor torques)
CR_2 /
CR_3 /
]
Main usage:
-----------
from model import *
model = QuadcopterModel()
-----------
"""
def __init__(self):
self._model = QuadcopterModel()
self.x = self._model.states
self.dx = self._model.dstates
self.u = self._model.controls
self.r = substitute([self._model.res_],
[self.dx,self.x,self.u],
[self._model.scaling_states*SX(self.dx),
self._model.scaling_states*SX(self.x),
self._model.scaling_controls*SX(self.u)
])[0]
self.r0 = 403.60380987049399/self._model.scaling_states["r",0]
self.u0 = 0.00261746256/self._model.scaling_controls["CR",0]
self.x0 = self.x(0)
self.x0["p","z"] = 1
self.x0["q",3] = 1
self.x0["r",:] = self.r0
R = jacobian(self.r,self.dx)
rhs = - casadi.solve(R, substitute(self.r, self.dx, 0) )
self.f = SXFunction( [ self.x, self.u ], [ rhs ] )
self.f.init()
class QuadcopterModel:
"""
Quadcopter model
::
states = [
p_x \\
p_y (p: centre of mass position)
p_z /
v_x \\
v_y (v: centre of mass velocity)
v_z /
q_0 \\
q_1 (q: orientation quaternions)
q_2 /
q_3 /
w_0 \\
w_1 (w: angular velocity)
w_2 /
r_0 \\
r_1 (r: rotor speeds)
r_2 /
r_3 /
]
controls = [
CR_0 \\
CR_1 (CR: motor torques)
CR_2 /
CR_3 /
]
Main usage:
-----------
from model import *
model = QuadcopterModel()
x = model.x # states
dx = model.dx # state derivatives
u = model.u # controls
-----------
"""
def __init__(self,debug=False,quatnorm=False,qstab=False):
"""
Keyword arguments:
debug -- wether to print out debug info
quatnorm -- add the quaternion norm to the DAE rhs
"""
# ----------- syistem states and their derivatives ----
pos = struct_symSX(["x","y","z"]) # rigid body centre of mass position [m] {0}
v = struct_symSX(["x","y","z"]) # rigid body centre of mass position velocity [m/s] {0}
NR = 4 # Number of rotors
states = struct_symSX([
entry("p",struct=pos),
entry("v",struct=v),
entry("q",shape=4), # quaternions {0} -> {1}
entry("w",shape=3), # rigid body angular velocity w_101 [rad/s] {1}
entry("r",shape=NR) # spin speed of rotor, wrt to platform. [rad/s] Should be positive!
# The signs are such that positive means lift generating, regardless of spin direction.
])
pos, v, q, w, r = states[...]
# ------------------------------------------------
dist = struct_symSX([
entry("Faer",shape=3), # Disturbance on aerodynamic forcing [N]
entry("Caer",shape=3) # Disturbance on aerodynamic torques [Nm]
])
# ----------------- Controls ---------------------
controls = struct_symSX([
entry("CR",shape=NR) # [Nm]
# Torques of the motors that drive the rotors, acting from platform on propeller
# The torque signs are always positive when putting energy in the propellor,
# regardless of spin direction.
#
])
CR = controls["CR"]
# ------------------------------------------------
# ---------------- Temporary symbols --------------
F = SX.sym("F",3) # Forces acting on the platform in {1} [N]
C = SX.sym("C",3) # Torques acting on the platform in {1} [Nm]
rotors_Faer = [SX.sym("Faer_%d" %i,3,1) for i in range(NR)] # Placeholder for aerodynamic force acting on propeller {1} [N]
rotors_Caer = [SX.sym("Caer_%d" %i,3,1) for i in range(NR)] # Placeholder for aerodynamic torques acting on propeller {1} [Nm]
# ---------------------------------------------------
# ----------------- Parameters ---------------------
rotor_model = struct_symSX([
"c", # c Cord length [m]
"R", # R Radius of propeller [m]
"CL_alpha", # CL_alpha Lift coefficient [-]
"alpha_0", # alpha_0
"CD_alpha", # CD_alpha Drag coefficient [-]
"CD_i", # CD_i Induced drag coefficient [-]
])
p = struct_symSX([
entry("rotors_model",repeat=NR,struct=rotor_model), # Parameters that describe the rotor model
entry("rotors_I",repeat=NR,shape=Sparsity.diag(3)), # Inertias of rotors [kg.m^2]
entry("rotors_spin",repeat=NR), # Direction of spin from each rotor. 1 means rotation around positive z.
entry("rotors_p",repeat=NR,shape=3), # position of rotors in {1} [m],
entry("I",sym=diag(SX.sym("[Ix,Iy,Iz]"))), # Inertia of rigid body [kg.m^2]
"m", # Mass of the whole system [kg]
"g", # gravity [m/s^2]
"rho", # Air density [kg/m^3]
])
I,m,g,rho = p[["I","m","g","rho"]]
# --------------------------------------------------
# ----------------- Parameters fillin's ---------------------
p_ = p()
p_["rotors_spin"] = [1,-1,1,-1]
p_["rotors_model",:,{}] = { "c": 0.01, "R" : 0.127, "CL_alpha": 6.0, "alpha_0": 0.15, "CD_alpha": 0.02, "CD_i": 0.05} # c Cord length [m]
p_["m"] = 0.5 # [kg]
p_["g"] = 9.81 # [N/kg]
p_["rho"] = 1.225 # [kg/m^3]
L = 0.25
I_max = p_["m"] * L**2 # Inertia of a point mass at a distance L
I_ref = I_max/5
#print "q I_max", I_max
#print "q I_ref", I_ref
#print "q I_ref/2", I_ref/2
p_["I"] = diag([I_ref/2,I_ref/2,I_ref]) # [N.m^2]
p_["rotors_p",0] = DMatrix([L,0,0])
p_["rotors_p",1] = DMatrix([0,L,0])
p_["rotors_p",2] = DMatrix([-L,0,0])
p_["rotors_p",3] = DMatrix([0,-L,0])
for i in range(NR):
R_ = p_["rotors_model",i,"R"] # Radius of propeller [m]
m_ = 0.01 # Mass of a propeller [kg]
I_max = m_ * R_**2 # Inertia of a point mass
#print "rotor I_max", I_max
I_ref = I_max/5
#print "rotor I_ref", I_ref
#print "rotor I_ref/2", I_ref/2
p_["rotors_I",i] = diag([I_ref/2,I_ref/2,I_ref])
if debug:
print p.vecNZcat()
dist_ = dist(0)
# ----------------- Scaling ---------------------
scaling_states = states(1)
scaling_controls = controls(1)
scaling_states["r"] = 500
scaling_controls["CR"] = 0.005
scaling_dist = dist()
scaling_dist["Faer"] = float(p_["m"]*p_["g"]/NR)
scaling_dist["Caer"] = 0.0026
# ----------- Frames ------------------
T_10 = mul(tr(*pos),Tquat(*q))
T_01 = kin_inv(T_10)
R_10 = T2R(T_10)
R_01 = R_10.T
# -------------------------------------
dstates = struct_symSX(states)
dp,dv,dq,dw,dr = dstates[...]
res = struct_SX(states) # DAE residual hand side
# ----------- Dynamics of the body ----
res["p"] = v - dp
# Newton, but transform the force F from {1} to {0}
res["v"] = mul(R_10,F) - m*dv
# Kinematics of the quaterion.
res["q"] = mul(quatDynamics(*q),w)-dq
#print mul(quatDynamics(*q),w)
#Skew = jacobian(mul(quatDynamics(*q),w),q)
#print "skew=",Skew
#print "skew2=",mul(jacobian(quatDynamics(*q),q),w).reshape((4,4))
#d = mul([q.T,Skew,q])
#a = DMatrix([1,2,0.3,0.7])
#a=a/sqrt(sumAll(a**2))
#print jacobian(substitute(d,q,a),w)
# This is a trick by Sebastien Gros to stabilize the quaternion evolution equation
if qstab:
res["q"] += -q*(sumAll(q**2)-1)
# Agular impulse H_1011
H = mul(p["I"],w) # Due to platform
for i in range(NR):
H+= mul(p["rotors_I",i], w + vertcat([0,0,p["rotors_spin",i]*r[i]])) # Due to rotor i
self.H = H
dH = mul(jacobian(H,w),dw) + mul(jacobian(H,q),dq) + mul(jacobian(H,r),dr) + cross(w,H)
# dH = mul(jacobian(H,vertcat([w,q,r])),vertcat([dw,dq,dr])) + cross(w,H) #same
# More expensive..
#temp = SXFunction([vertcat([w,q,r])],[H])
#temp.init()
#d = temp.derivative(1,0)
#d.init()
#dH = d([vertcat([w,q,r]),vertcat([dw,dq,dr])])[1]+ cross(w,H)
#dotdraw(dH)
res["w"] = C - dH
for i in range(NR):
res["r",i] = CR[i] - rotors_Caer[i][2] - p["rotors_I",i][2,2]*(dr[i]+p["rotors_spin",i]*dw[2]) # Dynamics of rotor i
# ---------------------------------
# Make a vector of f ?
#if quatnorm:
# f = vertcat(f+[sumAll(q**2)-1])
#else:
# f = vertcat(f)
# ------------ Force model ------------
Fg = mul(R_01,vertcat([0,0,-g*m]))
F_total = Fg + sum(rotors_Faer) + dist["Faer"] # Total force acting on the platform
C_total = SX([0,0,0]) + dist["Caer"] # Total torque acting on the platform
for i in range(NR):
C_total[:2] -= p["rotors_spin",i]*rotors_Caer[i][:2] # The x and y components propagate
# There are no components..
# The rotor inertia arising from I*dw[2] is already present in the platform dynamic balance
#C_total[2] -= p["rotors_spin",i]*CR[i]-p["rotors_I",i][2,2]*dw[2] # the z compent moves through a serparate system
C_total[2]-= p["rotors_spin",i]*rotors_Caer[i][2]
C_total += cross(p["rotors_p",i],rotors_Faer[i]) # Torques due to thrust
res = substitute(res,F,F_total)
res = substitute(res,C,C_total)
subs_before = []
subs_after = []
v_global = mul(R_01,v)
u_z = SX([0,0,1])
# Now fill in the aerodynamic forces
for i in range(NR):
c,R,CL_alpha,alpha_0, CD_alpha, CD_i = p["rotors_model",i,...]
#Bristeau P-jean, Martin P, Salaun E, Petit N. The role of propeller aerodynamics in the model of a quadrotor UAV. In: Proceedings of the European Control Conference 2009.; 2009:683-688.
v_local = v_global + (cross(w,p["rotors_p",i])) # Velocity at rotor i
rotors_Faer_physics = (rho*c*R**3*r[i]**2*CL_alpha*(alpha_0/3.0-v_local[2]/(2.0*R*r[i]))) * u_z
subs_before.append(rotors_Faer[i])
subs_after.append(rotors_Faer_physics)
rotors_Caer_physics = rho*c*R**4*r[i]**2*(CD_alpha/4.0+CD_i*alpha_0**2*(alpha_0/4.0-2.0*v_local[2]/(3.0*r[i]*R))-CL_alpha*v_local[2]/(r[i]*R)*(alpha_0/3.0-v_local[2]/(2.0*r[i]*R))) * u_z
subs_before.append(rotors_Caer[i])
subs_after.append(rotors_Caer_physics )
res = substitute(res,veccat(subs_before),veccat(subs_after))
# Make an explicit ode
rhs = - solve(jacobian(res,dstates),substitute(res,dstates,0))
# --------------------------------------
self.res_w = res
self.res = substitute(res,dist,dist_)
self.res_ = substitute(self.res,p,p_)
resf = SXFunction([dstates, states, controls ],[self.res_])
resf.setOption("name","resf")
resf.init()
self.resf = resf
self.rhs_w = rhs
self.rhs = substitute(rhs,dist,dist_)
self.rhs_ = substitute(self.rhs,p,p_)
self.H = substitute(substitute(H,p,p_),dist,dist_)
t = SX.sym("t")
# We end up with a DAE that captures the system dynamics
dae = SXFunction(daeIn(t=t,x=states,p=controls),daeOut(ode=self.rhs_))
dae.setOption("name","dae")
dae.init()
self.dae = dae
cdae = SXFunction(controldaeIn(t=t, x=states, u= controls,p=p),daeOut(ode=self.rhs))
cdae.setOption("name","cdae")
cdae.init()
self.cdae = cdae
self.states = states
self.dstates = dstates
self.p = p
self.p_ = p_
self.controls = controls
self.NR = NR
self.w = dist
self.w_ = dist_
self.t = t
self.states_ = states()
self.dstates_ = states()
self.controls_ = controls()
self.scaling_states = scaling_states
self.scaling_controls = scaling_controls
self.scaling_dist = scaling_dist
casadiAvailable = False
casadiTypes = set()
try:
import casadi as c
casadiAvailable = True
casadiTypes = set([type(c.SXElement()),type(c.SX())])
except ImportError:
pass
def TRx(a):
constr = numpy.matrix
if casadiAvailable and type(a) in casadiTypes:
constr = c.blockcat
return constr([[1,0,0,0],[0,cos(a),-sin(a),0],[0,sin(a),cos(a),0],[0,0,0,1]])
def TRy(a):
constr = numpy.matrix
if casadiAvailable and type(a) in casadiTypes:
constr = c.blockcat
return constr([[cos(a),0,sin(a),0],[0,1,0,0],[-sin(a),0,cos(a),0],[0,0,0,1]])
def TRz(a):
constr = numpy.matrix
if casadiAvailable and type(a) in casadiTypes:
constr = c.blockcat
return constr([[cos(a),-sin(a),0,0],[sin(a),cos(a),0,0],[0,0,1,0],[0,0,0,1]])
def tr(x,y,z):
return c.blockcat([[1,0,0,x],[0,1,0,y],[0,0,1,z],[0,0,0,1]])
def scale(a):
return numpy.matrix([[a,0,0],[0,a,0],[0,0,a]])
def Tscale(a):
return R2T(scale(a))
def Tquat(q0,q1,q2,q3):
return R2T(quat(q0,q1,q2,q3))
def quat(q0,q1,q2,q3):
"""
From Jeroen's presentation. q = [e*sin(theta/2); cos(theta/2)]
"""
constr = c.blockcat
types = set([type(q) for q in [q0,q1,q2,q3]])
#if not(types.isdisjoint(casadiTypes)):
# constr = c.blockcat
rho = constr([[q0],[q1],[q2]])
rho_skew = skew(rho)
I_3 = constr([[1.0,0,0],[0,1.0,0],[0,0,1.0]])
#A = multiply(I_3,(numpy.dot(rho.T,-rho)+q3*q3))+numpy.dot(rho,rho.T)*2.0-q3*rho_skew*2.0
b = q0
c_ = q1
d = q2
a = q3
a2 = a**2
b2 = b**2
c2 = c_**2
d2 = d**2
am2 = -a2
bm2 = -b2
cm2 = -c2
dm2 = -d2
bb = 2*b
aa = 2*a
bc2 = bb*c_
bd2 = bb*d
ac2 = aa*c_
ab2 = aa*b
ad2 = aa*d
cd2 = 2*c_*d
A = constr([[a2+b2+cm2+dm2, bc2 - ad2, bd2 + ac2],[bc2 + ad2, a2+bm2+c2+dm2, cd2 - ab2], [ bd2 -ac2, cd2 + ab2, a2+bm2+cm2+d2]]).T
return A.T
def quatOld(q0,q1,q2,q3):
"""
From Shabana AA. Dynamics of multibody systems. Cambridge Univ Pr; 2005.
defined as [ cos(theta/2) e*sin(theta/2) ]
"""
constr = numpy.matrix
types = set([type(q) for q in [q0,q1,q2,q3]])
#if not(types.isdisjoint(casadiTypes)):
# constr = c.blockcat
E = constr([[-q1, q0, -q3, q2],[-q2, q3, q0, -q1],[-q3,-q2,q1,q0]])
Eb = constr([[-q1, q0, q3, -q2],[-q2, -q3, q0, q1],[-q3,q2,-q1,q0]])
if not(types.isdisjoint(casadiTypes)):
constr = c.blockcat
return constr(numpy.dot(E,Eb.T))
def fullR(R_0_0,R_1_0,R_2_0, R_0_1, R_1_1, R_2_1, R_0_2, R_1_2, R_2_2):
constr = numpy.matrix
types = set([type(q) for q in [R_0_0,R_1_0,R_2_0, R_0_1, R_1_1, R_2_1, R_0_2, R_1_2, R_2_2]])
if not(types.isdisjoint(casadiTypes)):
constr = c.blockcat
return constr([[R_0_0, R_0_1, R_0_2],[R_1_0, R_1_1, R_1_2 ],[R_2_0, R_2_1, R_2_2 ]])
def TfullR(R_0_0,R_1_0,R_2_0, R_0_1, R_1_1, R_2_1, R_0_2, R_1_2, R_2_2):
return R2T(fullR(R_0_0,R_1_0,R_2_0, R_0_1, R_1_1, R_2_1, R_0_2, R_1_2, R_2_2))
def origin() :
return tr(0,0,0)
def trp(T):
return numpy.matrix(T)[:3,3]
def kin_inv(T):
R=numpy.matrix(T2R(T).T)
constr = numpy.matrix
if type(T) in casadiTypes:
constr = c.blockcat
return constr(vstack((hstack((R,-numpy.dot(R,trp(T)))),numpy.matrix([0,0,0,1]))))
def vectorize(vec):
"""
Make sure the result is something you can index with single index
"""
if hasattr(vec,"shape"):
if vec.shape[0] > 1 and vec.shape[1] > 1:
raise Exception("vectorize: got real matrix instead of vector like thing: %s" % str(vec))
if vec.shape[1] > 1:
vec = vec.T
if hasattr(vec,"tolist"):
vec = [ i[0] for i in vec.tolist()]
return vec
def skew(vec):
myvec = vectorize(vec)
x = myvec[0]
y = myvec[1]
z = myvec[2]
constr = numpy.matrix
types = set([type(q) for q in [x,y,z]])
if not(types.isdisjoint(casadiTypes)):
constr = c.blockcat
return constr([[0,-z,y],[z,0,-x],[-y,x,0]])
def invskew(S):
return c.SX([S[2,1],S[0,2],S[1,0]])
def cross(a,b):
return c.mul(skew(a),b)
def T2R(T):
"""
Rotational part of transformation matrix
"""
return T[0:3,0:3]
def R2T(R):
"""
Pack a rotational matrix in a homogenous form
"""
T = c.SX([[0,0,0,0],[0,0,0,0],[0,0,0,0],[0,0,0,1.0]])
T[:3,:3] = R
return T
def T2w():
"""
skew(w_100) = T2w(T_10)
"""
def T2W(T,p,dp):
"""
w_101 = T2W(T_10,p,dp)
"""
R = T2R(T)
dR = c.reshape(c.mul(c.jacobian(R,p),dp),(3,3))
return invskew(c.mul(R.T,dR))
def quatDynamics(q0,q1,q2,q3):
"""
dot(q) = quatDynamics(q)*w_101
"""
constr = numpy.matrix
if type(q0) in casadiTypes:
constr = c.blockcat
B = constr([[q3,-q2,q1],[q2,q3,-q0],[-q1,q0,q3],[-q0,-q1,-q2]])*0.5
return B
def T2WJ(T,p):
"""
w_101 = T2WJ(T_10,p).diff(p,t)
"""
R = T2R(T)
RT = R.T
temp = []
for i,k in [(2,1),(0,2),(1,0)]:
#temp.append(c.mul(c.jacobian(R[:,k],p).T,R[:,i]).T)
temp.append(c.mul(RT[i,:],c.jacobian(R[:,k],p)))
return c.vertcat(temp)
def evalModel(model):
states = model.states
dstates = model.dstates
controls = model.controls
p = model.p
states_ = model.states_
dstates_ = model.dstates_
controls_ = model.controls_
p_ = model.p_
rhs = model.rhs
res = model.res
# We end up with a DAE that captures the system dynamics
dyn = SXFunction([dstates, states, controls ],[res])
dyn.setOption("name","dyn")
dyn.init()
NR = model.NR
resd = dyn([SX(dstates_), SX(states_), SX(controls_)])[0]
res = dyn([SX(dstates_), SX(states_), SX(controls_)])[0]
res_ = substitute(res,p,p_)
for v in zip(states.cat,resd,res,res_):
print v[0]
for i in v[1:]:
print " ", i
return (res,res_)
def unittests():
model = QuadcopterModel()
states = model.states
dstates = model.dstates
controls = model.controls
p = model.p
states_ = model.states_
dstates_ = model.dstates_
controls_ = model.controls_
p_ = model.p_
NR = model.NR
# ----------------- do quick checks on the model -----
print "zero input -> gravity"
controls_["CR"]= [0,0,0,0]
states_["p"] = [0,0,0]
dstates_["p"] = [0,0,0]
states_["v"] = [0,0,0]
dstates_["v"] = [0,0,0]
states_["r"] = [0,0,0,0]
dstates_["r"] = [0,0,0,0]
states_["q"] = [0,0,0,1]
dstates_["q"] = [0,0,0,0]
states_["w"] = [0,0,0]
dstates_["w"] = [0,0,0]
[res, res_] = evalModel(model)
for i in range(res.size()):
print res[i]
if i == states.f["v"][2]:
print res_[i].toScalar()
assert(res_[i].toScalar().getValue()==-p_["g"]*p_["m"])
else:
assert(res_[i].toScalar().getValue()==0)
print "platform rotating around +z"
controls_["CR"]= [0,0,0,0]
states_["p"] = [0,0,0]
dstates_["p"] = [0,0,0]
states_["v"] = [0,0,0]
dstates_["v"] = [0,0,0]
states_["r"] = [0,0,0,0]
dstates_["r"] = [0,0,0,0]
states_["q"] = [0,0,0,1]
dstates_["q"] = [0,0,0,0]
states_["w"] = [0,0,1]
dstates_["w"] = [0,0,0]
[res, res_] = evalModel(model)
for i in range(res.size()):
print res[i]
if i == states.f["v"][2]:
assert(res_[i].toScalar().getValue()==-p_["g"]*p_["m"])
elif i == states.f["q"][2]:
assert(res[i].toScalar().isEqual(0.5)) # The quaternion is changing
else:
assert(res_[i].toScalar().getValue()==0)
print "platform accelerates around +z"
controls_["CR"]= [0,0,0,0]
states_["p"] = [0,0,0]
dstates_["p"] = [0,0,0]
states_["v"] = [0,0,0]
dstates_["v"] = [0,0,0]
states_["r"] = [0,0,0,0]
dstates_["r"] = [-1,1,-1,1] # The rotors keep steady in the inertial frame
states_["q"] = [0,0,0,1]
dstates_["q"] = [0,0,0,0]
states_["w"] = [0,0,0]
dstates_["w"] = [0,0,1]
[res, res_] = evalModel(model)
for i in range(res.size()):
print res[i]
if i == states.f["v"][2]:
assert(res_[i].toScalar().getValue()==-p_["g"]*p_["m"])
elif i in list(states.f["r"]):
assert(res_[i].toScalar().getValue()==0) # since the rotors are mounted in vertical bearing
elif i == states.f["w"][2]:
assert(res_[i].toScalar().getValue()<0)
else:
assert(res_[i].toScalar().getValue()==0)
print "One rotor works"
controls_["CR"]= [0,0,0,0]
states_["p"] = [0,0,0]
dstates_["p"] = [0,0,0]
states_["v"] = [0,0,0]
dstates_["v"] = [0,0,0]
states_["r"] = [100,0,0,0]
dstates_["r"] = [0,0,0,0]
states_["q"] = [0,0,0,1]
dstates_["q"] = [0,0,0,0]
states_["w"] = [0,0,0]
dstates_["w"] = [0,0,0]
[res, res_] = evalModel(model)
for i in range(res.size()):
print states.cat[i],res[i], res_[i].toScalar()
if i == states.f["v"][2]:
assert(res_[i].toScalar().getValue()>-p_["g"]) # gravity pulls down
elif i == states.f["w"][1]:
assert(res_[i].toScalar().getValue()<0) # platform rotates over -y
elif i == states.f["w"][2]:
assert(res_[i].toScalar().getValue()<0) # platform rotates over -z
elif i == states.f["r"][0]:
assert(res_[i].toScalar().getValue()<0) # rotor slows down
else:
assert(res_[i].toScalar().isEqual(0))
thrust = res_[states.f["v"][2]].toScalar().getValue()+p_["g"]
angular = res_[states.f["w"][1]].toScalar().getValue()
slowdown = res_[states.f["r"][0]].toScalar().getValue()
print "One rotor works while platform moves upwards"
controls_["CR"]= [0,0,0,0]
states_["p"] = [0,0,0]
dstates_["p"] = [0,0,0]
states_["v"] = [0,0,0.01]
dstates_["v"] = [0,0,0]
states_["r"] = [100,0,0,0]
dstates_["r"] = [0,0,0,0]
states_["q"] = [0,0,0,1]
dstates_["q"] = [0,0,0,0]
states_["w"] = [0,0,0]
dstates_["w"] = [0,0,0]
[res, res_] = evalModel(model)
for i in range(res.size()):
print states.cat[i], res[i], res_[i].toScalar()
if i == states.f["p"][2]:
assert(res_[i].toScalar().getValue()==states_["v"][2]) # dx = v
elif i == states.f["v"][2]:
assert(res_[i].toScalar().getValue()+p_["g"]<thrust) # Translation speed makes for decreased propeller lift
elif i == states.f["w"][1]:
assert(res_[i].toScalar().getValue()<0) # platform rotates over -y
elif i == states.f["w"][2]:
assert(res_[i].toScalar().getValue()<0) # platform rotates over -z
elif i == states.f["r"][0]:
assert(abs(res_[i].toScalar().getValue())<abs(slowdown)) # rotor slows down less than without the upward movement, because lift is smaller
else:
assert(res_[i].toScalar().isEqual(0))
print "One rotor works while platform moves sideways"
controls_["CR"]= [0,0,0,0]
states_["p"] = [0,0,0]
dstates_["p"] = [0,0,0]
states_["v"] = [1,0,0]
dstates_["v"] = [0,0,0]
states_["r"] = [100,0,0,0]
dstates_["r"] = [0,0,0,0]
states_["q"] = [0,0,0,1]
dstates_["q"] = [0,0,0,0]
states_["w"] = [0,0,0]
dstates_["w"] = [0,0,0]
[res, res_] = evalModel(model)
for i in range(res.size()):
print res[i], res_[i].toScalar(),abs(slowdown)
if i == states.f["p"][0]:
assert(res_[i].toScalar().getValue()==states_["v"][0]) # dx = v
elif i == states.f["v"][2]:
print "debug:" , res_[i].toScalar().getValue()+p_["g"], thrust
assert(res_[i].toScalar().getValue()+p_["g"]<=thrust) # Translation speed makes for decreased propeller lift
elif i == states.f["w"][1]:
assert(res_[i].toScalar().getValue()<0) # platform rotates over -y
elif i == states.f["w"][2]:
assert(res_[i].toScalar().getValue()<0) # platform rotates over -z
elif i == states.f["r"][0]:
assert(abs(res_[i].toScalar().getValue())==abs(slowdown)) # rotor slows down less, because lift is smaller
else:
assert(res_[i].toScalar().isEqual(0))
print "One rotor works while platform rotates around positive local x-axis"
controls_["CR"]= [0,0,0,0]
states_["p"] = [0,0,0]
dstates_["p"] = [0,0,0]
states_["v"] = [0,0,0]
dstates_["v"] = [0,0,0]
states_["r"] = [100,0,0,0]
dstates_["r"] = [0,0,0,0]
states_["q"] = [0,0,0,1]
dstates_["q"] = [0.05,0,0,0]
states_["w"] = [0.1,0,0]
dstates_["w"] = [0,0,0]
[res, res_] = evalModel(model)
for i in range(res.size()):
print res[i], res_[i].toScalar(),abs(thrust)
if i == states.f["v"][2]:
assert(res_[i].toScalar().getValue()+p_["g"]==thrust) # No change
elif i == states.f["w"][1]:
assert(res_[i].toScalar().getValue()<0) # platform rotates over -y + some gyro effects
elif i == states.f["w"][2]:
assert(res_[i].toScalar().getValue()<0) # platform rotates over -z
elif i == states.f["r"][0]:
assert(res_[i].toScalar().getValue()==slowdown) # No change
else:
assert(res_[i].toScalar().isEqual(0))
print "One rotor works while platform rotates around axis through rotor center and parallel to y-axis (positive)"
controls_["CR"]= [0,0,0,0]
states_["p"] = [0,0,0]
dstates_["p"] = [0,0,0]
states_["v"] = [0,0,0.25*0.1] #L*omega
dstates_["v"] = [0,0,0]
states_["r"] = [100,0,0,0]
dstates_["r"] = [0,0,0,0]
states_["q"] = [0,0,0,1]
dstates_["q"] = [0,0.05,0,0]
states_["w"] = [0,0.1,0]
dstates_["w"] = [0,0,0]
[res, res_] = evalModel(model)
for i in range(res.size()):
print res[i], res_[i].toScalar()
if i == states.f["p"][2]:
assert(res_[i].toScalar().getValue()==states_["v"][2])
elif i == states.f["w"][0]:
assert(res_[i].toScalar().getValue()<0) # Gyroscopic effect: accelerarion around -x
elif i == states.f["v"][2]:
assert(res_[i].toScalar().getValue()+p_["g"]==thrust) # No change
elif i == states.f["w"][1]:
assert(res_[i].toScalar().getValue()<0) # platform rotates over -y
elif i == states.f["w"][2]:
assert(res_[i].toScalar().getValue()<0) # platform rotates over -z
elif i == states.f["r"][0]:
assert(res_[i].toScalar().getValue()==slowdown) # rotor slows down less, because lift is smaller
else:
assert(res_[i].toScalar().isEqual(0))
print "One rotor works while platform is tilted 45 degrees along -y"
controls_["CR"]= [0,0,0,0]
states_["p"] = [0,0,0]
dstates_["p"] = [0,0,0]
states_["v"] = [0,0,0]
dstates_["v"] = [0,0,0]
states_["r"] = [100,0,0,0]
dstates_["r"] = [0,0,0,0]
states_["q"] = [0,-sin(pi/8),0,cos(pi/8)]
dstates_["q"] = [0,0,0,0]
states_["w"] = [0,0,0]
dstates_["w"] = [0,0,0]
[res, res_] = evalModel(model)
for i in range(res.size()):
print states.cat[i], res[i], res_[i].toScalar()
if i == states.f["v"][0]:
assert(res_[i].toScalar().getValue()<0) # Thrust in -x direction
elif i == states.f["v"][2]:
assert(res_[i].toScalar().getValue()+p_["g"]<thrust) # Inclination makes for smaller upward force
elif i == states.f["w"][1]:
assert(res_[i].toScalar().getValue()<0) # platform rotates over -y
elif i == states.f["w"][2]:
assert(res_[i].toScalar().getValue()<0) # platform rotates over -z
elif i == states.f["r"][0]:
assert(res_[i].toScalar().getValue()<0) # rotor slows down
else:
assert(res_[i].toScalar().isEqual(0))
print "One rotor works while platform is tilted 45 degrees along +y"
# @TODO: make this unittest
controls_["CR"]= [0,0,0,0]
states_["p"] = [0,0,0]
dstates_["p"] = [0,0,0]
states_["v"] = [0,0,0]
dstates_["v"] = [0,0,0]
states_["r"] = [100,0,0,0]
dstates_["r"] = [0,0,0,0]
states_["q"] = [0,sin(pi/8),0,cos(pi/8)]
dstates_["q"] = [0,0,0,0]
states_["w"] = [0,0,0]
dstates_["w"] = [0,0,0]
[res, res_] = evalModel(model)
for i in range(res.size()):
if i == states.f["v"][0]:
assert(res_[i].toScalar().getValue()>0) # Thrust in -x direction
elif i == states.f["v"][2]:
assert(res_[i].toScalar().getValue()+p_["g"]<thrust) # Inclination makes for smaller upward force
elif i == states.f["w"][1]:
assert(res_[i].toScalar().getValue()<0) # platform rotates over -y
elif i == states.f["w"][2]:
assert(res_[i].toScalar().getValue()<0) # platform rotates over -z
elif i == states.f["r"][0]:
assert(res_[i].toScalar().getValue()<0) # rotor slows down
else:
assert(res_[i].toScalar().isEqual(0))
print "platform rotating around +x"
controls_["CR"]= [0,0,0,0]
states_["p"] = [0,0,0]
dstates_["p"] = [0,0,0]
states_["v"] = [0,0,0]
dstates_["v"] = [0,0,0]
states_["r"] = [0,0,0,0]
dstates_["r"] = [0,0,0,0]
states_["q"] = [0,0,0,1]
dstates_["q"] = [0.5,0,0,0]
states_["w"] = [1,0,0]
dstates_["w"] = [0,0,0]
[res, res_] = evalModel(model)
for i in range(res.size()):
print res[i]
if i == states.f["v"][2]:
assert(res[i].toScalar().isEqual((-p["g"]*p["m"]).at(0),2))
else:
assert(res[i].toScalar().isEqual(0))
print "platform rotating around -y with one rotor spinning"
controls_["CR"]= [0,0,0,0]
states_["p"] = [0,0,0]
dstates_["p"] = [0,0,0]
states_["v"] = [0,0,0]
dstates_["v"] = [0,0,0]
states_["r"] = [100,0,0,0]
dstates_["r"] = [0,0,0,0]
states_["q"] = [0,0,0,1]
dstates_["q"] = [0,-0.05,0,0] # The quaternion is changing
states_["w"] = [0,-0.1,0]
dstates_["w"] = [0,0,0]
[res, res_] = evalModel(model)
for i in range(res.size()):
print res[i], res_[i].toScalar()
if i == states.f["v"][2]:
assert(res_[i].toScalar()+p_["g"]*p_["m"]<thrust) # Decreased lift due to local velocity at the rotor
elif i == states.f["q"][1]:
assert(res_[i].toScalar().getValue()==0)
elif i == states.f["w"][0]:
assert(res_[i].toScalar().getValue()>0) # Gyroscopic effect: accelerarion around +x
elif i == states.f["w"][1]:
assert(res_[i].toScalar().getValue()<0) # Acceleration around - y due to rotor thrust
elif i == states.f["w"][2]:
assert(res_[i].toScalar().getValue()<0) # platform rotates over -z
elif i == states.f["r"][0]:
assert(res_[i].toScalar().getValue()<0) # rotor slows down
else:
assert(res[i].toScalar().isEqual(0))
print "platform rotating around -y with all rotors spinning"
controls_["CR"]= [0,0,0,0]
states_["p"] = [0,0,0]
dstates_["p"] = [0,0,0]
states_["v"] = [0,0,0]
dstates_["v"] = [0,0,0]
states_["r"] = [100,100,100,100]
dstates_["r"] = [0,0,0,0]
states_["q"] = [0,0,0,1]
dstates_["q"] = [0,-0.005,0,0] # The quaternion is changing
states_["w"] = [0,-0.01,0]
dstates_["w"] = [0,0,0]
[res, res_] = evalModel(model)
for i in range(res.size()):
print res[i], res_[i].toScalar().getValue(), angular
if i == states.f["v"][2]:
assert(res_[i].toScalar()>-p_["g"]*p_["m"])
elif i == states.f["w"][0]:
assert(res_[i].toScalar().getValue()==0) # Gyroscopic effects cancel out
#elif i == states.f["w"][1]:
# assert(res_[i].toScalar().getValue()==0) # Thrusts cancel out # improved aero: thrusts do not cancel exactly
elif i == states.f["w"][1]:
assert(abs(res_[i].toScalar().getValue())<=0.2*abs(angular)) # improved aero: thrusts do not cancel exactly, let's say they must cancel up to 20%
elif i == states.f["w"][2]:
assert(res_[i].toScalar().getValue()<0) # platform rotates over -z: torques do not cancel out completely
elif i == states.f["q"][1]:
assert(res_[i].toScalar().getValue()==0)
elif i in states.f["r"]:
assert(res_[i].toScalar().getValue()<0) # rotor slows down
else:
assert(res[i].toScalar().isEqual(0))
print "One rotor with positive spin accelerates"
controls_["CR"]= [1,0,0,0]
states_["p"] = [0,0,0]
dstates_["p"] = [0,0,0]
states_["v"] = [0,0,0]
dstates_["v"] = [0,0,0]
states_["r"] = [100,0,0,0]
dstates_["r"] = [0,0,0,0]
states_["q"] = [0,0,0,1]
dstates_["q"] = [0,0,0,0]
states_["w"] = [0,0,0]
dstates_["w"] = [0,0,0]
[res, res_] = evalModel(model)
for i in range(res.size()):
if i == states.f["v"][2]:
assert(res_[i].toScalar().getValue()>-p_["g"]) # gravity pulls down
elif i == states.f["w"][1]:
assert(res_[i].toScalar().getValue()<0) # platform rotates over -y
elif i == states.f["w"][2]:
assert(res_[i].toScalar().getValue()<0) # platform rotates over -z
elif i == states.f["r"][0]:
print p_["rotors_I",0,2,2]
assert(res_[i].toScalar().getValue()<controls_["CR"][0]/p_["rotors_I",0,2,2])
# Rotor accelerates, but less than in vacuum
else:
assert(res_[i].toScalar().isEqual(0))
print "One rotor with negative spin accelerates"
controls_["CR"]= [0,1,0,0]
states_["p"] = [0,0,0]
dstates_["p"] = [0,0,0]
states_["v"] = [0,0,0]
dstates_["v"] = [0,0,0]
states_["r"] = [0,100,0,0]
dstates_["r"] = [0,0,0,0]
states_["q"] = [0,0,0,1]
dstates_["q"] = [0,0,0,0]
states_["w"] = [0,0,0]
dstates_["w"] = [0,0,0]
[res, res_] = evalModel(model)
for i in range(res.size()):
if i == states.f["v"][2]:
assert(res_[i].toScalar().getValue()>-p_["g"]) # gravity pulls down
elif i == states.f["w"][0]:
assert(res_[i].toScalar().getValue()>0) # platform rotates over +x
elif i == states.f["w"][2]:
assert(res_[i].toScalar().getValue()>0) # platform rotates over +z
elif i == states.f["r"][1]:
assert(res_[i].toScalar().getValue()<controls_["CR"][1]/p_["rotors_I",1,2,2])
# Rotor accelerates, but less than in vacuum
else:
assert(res_[i].toScalar().isEqual(0))
print "All rotors rotate constant"
controls_["CR"]= [0,0,0,0]
states_["p"] = [0,0,0]
dstates_["p"] = [0,0,0]
states_["v"] = [0,0,0]
dstates_["v"] = [0,0,0]
states_["r"] = [100,100,100,100]
dstates_["r"] = [0,0,0,0]
states_["q"] = [0,0,0,1]
dstates_["q"] = [0,0,0,0]
states_["w"] = [0,0,0]
dstates_["w"] = [0,0,0]
[res, res_] = evalModel(model)
for i in range(res.size()):
if i == states.f["v"][2]:
assert(res_[i].toScalar().getValue()>-p_["g"]) # gravity pulls down
elif i in states.f["r"]:
assert(res_[i].toScalar().getValue()<0) # all rotors slow down
else:
assert(res_[i].toScalar().isEqual(0))
print "All rotors accelerate"
controls_["CR"]= [1,1,1,1]
states_["p"] = [0,0,0]
dstates_["p"] = [0,0,0]
states_["v"] = [0,0,0]
dstates_["v"] = [0,0,0]
states_["r"] = [100,100,100,100]
dstates_["r"] = [0,0,0,0]
states_["q"] = [0,0,0,1]
dstates_["q"] = [0,0,0,0]
states_["w"] = [0,0,0]
dstates_["w"] = [0,0,0]
[res, res_] = evalModel(model)
for i in range(res.size()):
if i == states.f["v"][2]:
assert(res_[i].toScalar().getValue()>-p_["g"]) # gravity pulls down
elif i in states.f["r"]:
assert(res_[i].toScalar().getValue()<controls_["CR",int(i-states.f["r"][0])]/p_["rotors_I",int(i-states.f["r"][0]),2,2])
# Rotor accelerates, but less than in vacuum
else:
assert(res_[i].toScalar().isEqual(0))
if __name__ == '__main__':
unittests()
Raw
solution_tracking.py
from casadi import *
from quadcopter import *
from casadi.tools import *
from pylab import *
show_3d = False
# Construct the quadcopter model
model = Quadcopter()
f = model.f
# Construct the explicit ODE
R = jacobian(model.r,model.dx)
rhs = - casadi.solve(R, substitute(model.r, model.dx, 0) )
f = SXFunction( [ model.x, model.u ], [ rhs ] )
f.init()
T = 3.0
N = 200
DT = T/N
# This constructs an object that behaves like an MX,
# but has convenient accessors
# e.g W["X",0] returns the state at the first control interval
#
# For more information about this CasADi feature,
# you could consult an online tutorial
# docs.casadi.org -> scroll to bottom
# -> click tutorials
# -> tools
# -> structure.pdf
W = struct_symMX([
(
entry("X",struct=model.x,repeat=N+1),
entry("Y",struct=model.x,repeat=N),
entry("U",struct=model.u,repeat=N)
)
])
ts = [0]
t = 0
# Build up the list of constraints
g = []
for i in range(N):
# Collocation equations
C = [-2,2]
xp_j = C[0]*W["X",i] + C[1]*W["Y",i]
[f_j] = f([ W["Y",i], W["U",i] ])
g.append(DT*f_j - xp_j)
# Continuity equations
D = [-1,2]
xf = D[0]*W["X",i] + D[1]*W["Y",i]
g.append(xf - W["X",i+1])
t = t + DT
ts.append(t)
g = vertcat(g)
# Construct the reference to track
traj = [ array([sin(i*DT*2*pi/3),i*DT*2*pi/3,0]) for i in range(N) ]
# Construct the objective
R1 = vertcat([ p-t for p,t in zip(W["X",:,"p"],traj) ])
R2 = vertcat(W["U",:])
alpha = 0.05
f = (T/N) * ( mul(R1.T, R1) + alpha*mul(R2.T, R2) )
# Create the NLP
nlp = MXFunction( nlpIn(x=W), nlpOut(f=f, g=g) )
nlp.init()
# Create an IPOPT NLP solver
solver = NlpSolver("ipopt",nlp)
solver.setOption("linear_solver","mumps") # todo: remove
# If we need more than 100 iterations, something is wrong
solver.setOption("max_iter",100)
solver.init()
# All constraints are equality constraints in this case
solver.setInput(0, "lbg")
solver.setInput(0, "ubg")
# Construct and populate the vectors with
# upper and lower simple bounds
#
# lbx is an array in disguise.
# You can view the underlying array as lbx.cat
lbx = W(-inf)
ubx = W(inf)
# 0 <= u_k <= 0.5
lbx["U",:] = 0
ubx["U",:] = 0.5
# p_0 = [0,0,0]^T
lbx["X",0,"p"] = 0.0
ubx["X",0,"p"] = 0.0
# v_0 = [0,0,0]^T
lbx["X",0,"v"] = 0.0
ubx["X",0,"v"] = 0.0
# Construct a vector with the initial guess
x0 = W(0)
x0["X",:] = model.x0
x0["Y",:] = model.x0
x0["U",:] = model.u0
solver.setInput(x0, "x0")
solver.setInput(lbx, "lbx")
solver.setInput(ubx, "ubx")
# Solve the NLP
solver.evaluate()
# Cast the result vector in a form
# that we can easily access
sol = W(solver.getOutput("x"))
# Save solution to a file
import pickle
pickle.dump(sol,file('tracking.pkl','w'))
X = sol["X",:,"p","x"]
Y = sol["X",:,"p","y"]
Z = sol["X",:,"p","z"]
# 2D plots
figure()
plot(ts,X,label="p_x")
plot(ts,Y,label="p_y")
plot(ts,Z,label="p_z")
plot(ts[:-1],horzcat(traj)[0,:].T,label="p_ref_x")
plot(ts[:-1],horzcat(traj)[1,:].T,label="p_ref_y")
xlabel("Time [s]")
legend(loc="upper left")
title("State trajectories")
figure()
plot(X,Y,'.',label="optimized")
plot(horzcat(traj)[0,:].T,horzcat(traj)[1,:].T,'.',label="reference")
legend()
title("Top down trajectory view")
xlabel("x [m]")
xlabel("y [m]")
axis('equal')
figure()
step(ts, horzcat(sol["U",:]+[ sol["U",-1] ]).T,where='post')
xlabel("Time [s]")
title("Control trajectories")
ylim([-0.1,0.6])
if show_3d:
# 3D plots
from mpl_toolkits.mplot3d import Axes3D
figure()
ax = gca(projection='3d')
ax.plot(array(X),array(Y),array(Z),"b.",label="optimized")
Traj = array(horzcat(traj))
ax.plot(Traj[:,0],Traj[:,1],Traj[:,2],'g.',label="reference")
# Plot the rotors
circle = array([ [cos(t),sin(t),0] for t in linspace(0,2*pi) ]).T*0.1
for p, q in zip(sol["X",::20,"p"],sol["X",::20,"q"]):
for offset in [ array([[1,0,0]]),array([[0,1,0]]),array([[-1,0,0]]),array([[0,-1,0]]) ]:
circle_offset = circle + 0.1*offset.T
circle_3D = mul(quat(*q), circle_offset)
ax.plot(
array(p[0]+circle_3D[0,:]).squeeze(),
array(p[1]+circle_3D[1,:]).squeeze(),
array(p[2]+circle_3D[2,:]).squeeze(),
'k'
)
ax.set_xlim([-pi,pi])
ax.set_ylim([0,2*pi])
ax.set_zlim([-pi,pi])
legend()
show()
Raw
tempate_timeopt.py
from quadcopter import *
from casadi.tools import *
from pylab import *
show_3d = False
""""
This file solves a time-optimal problem
min T + alpha*int || u(t) ||_2^2 + || w(t) ||_2^2 + || q(t) -q^ref ||_2^2 dt
x(t), u(t)
subject to || p(t)-pA ||^2 >= 0.8 obstacle avoidance
|| p(t)-pB ||^2 >= 0.8 obstacle avoidance
0 <= u <= 0.5
p_0 = [0,0,0]^T
v_0 = [0,0,0]^T
p_N = [0,2*pi,0]^T
v_N_y = 0
v_N_z = 0
alpha = 0.025
"""
# Initialize with the solution of the tracking problem
pA = np.array([0, pi/2, 0])
pB = np.array([0, 3*pi/2, 0])
ps = [pA, pB]
rs = [0.8, 0.8]
# Import the tracking solution (for initialisation)
import pickle
sol_tracking = pickle.load(file('tracking.pkl','r'))
# Construct the quadcopter model
model = Quadcopter()
f = model.f
N = 200
# This constructs an object that behaves like an MX,
# but has convenient accessors
W = struct_symMX([
(
entry("X",struct=model.x,repeat=N+1),
entry("Y",struct=model.x,repeat=N),
entry("U",struct=model.u,repeat=N)
),
entry("T")
])
T = W["T"]
DT = T/N
ts = [0]
t = 0
# Build up the list of constraints
g = []
for i in range(N):
[xdot] = f([ W["Y",i], W["U",i] ])
g.append(...)
g.append(...)
t = t + DT
ts.append(t)
def norm22(x):
return mul(x.T,x)
# This time, we have both equalities and inequalities.
# To avoid messy indexing of lbg and ubg,
# we use a structure for conveniece
g = struct_MX([
entry("collocation",expr=g),
entry("obstacleA",expr=[ norm22(p[:2]-pA[:2]) for p in W["X",:,"p"] ]),
entry("obstacleB",expr=[ norm22(p[:2]-pB[:2]) for p in W["X",:,"p"] ])
])
# Construct the objective
# Reference quaternion orientation: upright position
qref = array([0,0,0,1])
# To avoid the quadcopter getting lost in spinning or upside down trajectories,
# we add some extra regularisation
R2 = vertcat(W["U",:]+W["X",:,"w"]+[ q-qref for q in W["X",:,"q"] ])
alpha = 0.025
# Time optimality
f = T + (T/N) * alpha*mul(R2.T, R2)
# Create the NLP
nlp = MXFunction( nlpIn(x=W), nlpOut(f=f, g=g) )
nlp.init()
# Create an IPOPT NLP solver
solver = NlpSolver("ipopt",nlp)
solver.setOption("linear_solver","mumps") # todo: remove
# If we need more than 100 iterations, something is wrong
solver.setOption("max_iter",200)
solver.init()
lbg = g(0)
ubg = g(0)
# Set the bounds on the obstacle constraints
lbg["obstacleA"] = rs[0]**2
lbg["obstacleB"] = rs[1]**2
ubg["obstacleA"] = inf
ubg["obstacleB"] = inf
solver.setInput(lbg, "lbg")
solver.setInput(ubg, "ubg")
# Construct and populate the vectors with
# upper and lower bounds simple bounds
lbx = W(-inf)
ubx = W(inf)
...
x0 = W(0)
# Initialize with the solution of tracking
...
solver.setInput(x0, "x0")
solver.setInput(lbx, "lbx")
solver.setInput(ubx, "ubx")
# Solve the NLP
solver.evaluate()
# Cast the result vector in a form
# that we can easily access
sol = W(solver.getOutput("x"))
X = sol["X",:,"p","x"]
Y = sol["X",:,"p","y"]
Z = sol["X",:,"p","z"]
# 2D plots
ts = linspace(0,sol["T"],N+1)
figure()
plot(ts,X,label="p_x")
plot(ts,Y,label="p_y")
plot(ts,Z,label="p_z")
xlabel("Time [s]")
legend(loc="upper left")
title("State trajectories")
figure()
plot(X,Y,'.',label="optimized")
for p, r in zip(ps,rs):
gca().add_patch(Circle(p[:2],r,color='red'))
gca().add_patch(Circle(p[:2],r,color='red'))
legend()
title("Top down trajectory view")
xlabel("x [m]")
xlabel("y [m]")
axis('equal')
figure()
step(ts,horzcat(sol["U",:]+[ sol["U",-1] ]).T,where='post')
xlabel("Time [s]")
title("Control trajectories")
ylim([-0.1,0.6])
if show_3d:
# 3D plots
from mpl_toolkits.mplot3d import Axes3D
figure()
ax = gca(projection='3d')
ax.plot(array(X),array(Y),array(Z),"b.",label="optimized")
# Plot the rotors
circle = array([ [cos(t),sin(t),0] for t in linspace(0,2*pi) ]).T*0.05
for p, q in zip(sol["X",::30,"p"],sol["X",::30,"q"]):
for offset in [ array([[1,0,0]]),array([[0,1,0]]),array([[-1,0,0]]),array([[0,-1,0]]) ]:
circle_offset = circle + 0.1*offset.T
circle_3D = mul(quat(*q), circle_offset)
ax.plot(
array(p[0]+circle_3D[0,:]).squeeze(),
array(p[1]+circle_3D[1,:]).squeeze(),
array(p[2]+circle_3D[2,:]).squeeze(),
'k'
)
# plot the obstacles
for p, r in zip(ps,rs):
x=linspace(-1,1,40)
z=linspace(-3,3,40)
Xc, Zc= meshgrid(x,z)
Yc=sqrt(1-Xc**2)*r
ax.plot_wireframe(p[0]+Xc*r,p[1]+Yc,Zc,color='red')
ax.plot_wireframe(p[0]+Xc*r,p[1]-Yc,Zc,color='red')
ax.set_xlim([-pi,pi])
ax.set_ylim([0,2*pi])
ax.set_zlim([-pi,pi])
show()
Raw
template_tracking.py
from casadi import *
from quadcopter import *
from casadi.tools import *
from pylab import *
show_3d = False
# Construct the quadcopter model
model = Quadcopter()
# Get the ode model function
f = model.f
T = 3.0
N = 200
DT = T/N
# This constructs an object that behaves like an MX,
# but has convenient accessors
# e.g W["X",0] returns x_0 (the state at the first control interval)
#
# For more information about this CasADi feature,
# you could consult an online tutorial
# docs.casadi.org -> scroll to bottom
# -> click tutorials
# -> tools
# -> structure.pdf
#
# At this point, do not attempt to fully
# understand this, but just proceed and learn by example
W = struct_symMX([
(
entry("X",struct=model.x,repeat=N+1),
entry("Y",struct=model.x,repeat=N),
entry("U",struct=model.u,repeat=N)
)
])
ts = [0]
t = 0
# Build up the list of constraints
g = []
for i in range(N):
[xdot] = f([ W["Y",i], W["U",i] ])
g.append(...) # TODO: fill in
g.append(...)
t = t + DT
ts.append(t)
g = vertcat(g)
# Construct the reference to track
traj = [ array([sin(i*DT*2*pi/3),i*DT*2*pi/3,0]) for i in range(N) ]
# Construct the objective
# Fill in
# Hint: build up a list of expressions first, and then vertcat
R1 = ...
R2 = ...
alpha = 0.05
obj = (T/N) * ( mul(R1.T, R1) + alpha*mul(R2.T, R2) )
# Create the NLP
nlp = MXFunction( nlpIn(x=W), nlpOut(f=obj, g=g) )
nlp.init()
# Create an IPOPT NLP solver
solver = NlpSolver("ipopt",nlp)
solver.setOption("max_iter",100)
solver.init()
# All constraints are equality constraints in this case
solver.setInput(0, "lbg")
solver.setInput(0, "ubg")
# Construct and populate the vectors with
# upper and lower simple bounds
#
# lbx is an array in disguise.
# You can view the underlying array as lbx.cat
lbx = W(-inf)
ubx = W(inf)
# 0 <= u_k <= 0.5
lbx["U",:] = 0
ubx["U",:] = 0.5
# p_0 = [0,0,0]^T
lbx["X",0,"p"] = 0.0
ubx["X",0,"p"] = 0.0
# v_0 = [0,0,0]^T
...
...
# Construct a vector with the initial guess
x0 = W(0)
x0["X",:] = model.x0
...
...
# Extra info: model.x0 is just an array in disguise,
# you can look through the disguise by doing:
# print model.x0.cat
solver.setInput(x0, "x0")
solver.setInput(lbx, "lbx")
solver.setInput(ubx, "ubx")
# Solve the NLP
solver.evaluate()
# Cast the result vector in a form
# that we can easily access
sol = W(solver.getOutput("x"))
# Save solution to a file
import pickle
pickle.dump(sol,file('tracking.pkl','w'))
X = sol["X",:,"p","x"]
Y = sol["X",:,"p","y"]
Z = sol["X",:,"p","z"]
# 2D plots
figure()
plot(ts,X,label="p_x")
plot(ts,Y,label="p_y")
plot(ts,Z,label="p_z")
plot(ts[:-1],horzcat(traj)[0,:].T,label="p_ref_x")
plot(ts[:-1],horzcat(traj)[1,:].T,label="p_ref_y")
xlabel("Time [s]")
legend(loc="upper left")
title("State trajectories")
figure()
plot(X,Y,'.',label="optimized")
plot(horzcat(traj)[0,:].T,horzcat(traj)[1,:].T,'.',label="reference")
legend()
title("Top down trajectory view")
xlabel("x [m]")
xlabel("y [m]")
axis('equal')
figure()
step(ts, horzcat(sol["U",:]+[ sol["U",-1] ]).T,where='post')
xlabel("Time [s]")
title("Control trajectories")
ylim([-0.1,0.6])
if show_3d:
# 3D plots
from mpl_toolkits.mplot3d import Axes3D
figure()
ax = gca(projection='3d')
ax.plot(array(X),array(Y),array(Z),"b.",label="optimized")
Traj = array(horzcat(traj))
ax.plot(Traj[:,0],Traj[:,1],Traj[:,2],'g.',label="reference")
# Plot the rotors
circle = array([ [cos(t),sin(t),0] for t in linspace(0,2*pi) ]).T*0.1
for p, q in zip(sol["X",::20,"p"],sol["X",::20,"q"]):
for offset in [ array([[1,0,0]]),array([[0,1,0]]),array([[-1,0,0]]),array([[0,-1,0]]) ]:
circle_offset = circle + 0.1*offset.T
circle_3D = mul(quat(*q), circle_offset)
ax.plot(
array(p[0]+circle_3D[0,:]).squeeze(),
array(p[1]+circle_3D[1,:]).squeeze(),
array(p[2]+circle_3D[2,:]).squeeze(),
'k'
)
ax.set_xlim([-pi,pi])
ax.set_ylim([0,2*pi])
ax.set_zlim([-pi,pi])
legend()
show()
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment