def export_external_ode_model():
model_name = 'external_ode'
# Declare model variables
x = MX.sym('x', 2)
u = MX.sym('u', 1)
xDot = MX.sym('xDot', 2)
cdll.LoadLibrary('./test_external_lib/build/libexternal_ode_casadi.so')
f_ext = external('libexternal_ode_casadi', 'libexternal_ode_casadi.so',
{'enable_fd': True})
f_expl = f_ext(x, u)
f_impl = xDot - f_expl
model = AcadosModel()
model.f_impl_expr = f_impl
model.f_expl_expr = f_expl
model.x = x
model.xdot = xDot
model.u = u
model.p = []
model.name = model_name
return model
def export_pendulum_ode_model():
model_name = 'pendulum_ode'
# constants
M = 1. # mass of the cart [kg] -> now estimated
m = 0.1 # mass of the ball [kg]
g = 9.81 # length of the rod [m]
l = 0.8 # gravity constant [m/s^2]
# set up states & controls
x1 = SX.sym('x1')
theta = SX.sym('theta')
v1 = SX.sym('v1')
dtheta = SX.sym('dtheta')
x = vertcat(x1, theta, v1, dtheta)
# controls
F = SX.sym('F')
u = vertcat(F)
# xdot
x1_dot = SX.sym('x1_dot')
theta_dot = SX.sym('theta_dot')
v1_dot = SX.sym('v1_dot')
dtheta_dot = SX.sym('dtheta_dot')
xdot = vertcat(x1_dot, theta_dot, v1_dot, dtheta_dot)
# algebraic variables
# z = None
# parameters
p = []
# dynamics
denominator = M + m - m * cos(theta) * cos(theta)
f_expl = vertcat(
v1, dtheta, (-m * l * sin(theta) * dtheta * dtheta +
m * g * cos(theta) * sin(theta) + F) / denominator,
(-m * l * cos(theta) * sin(theta) * dtheta * dtheta + F * cos(theta) +
(M + m) * g * sin(theta)) / (l * denominator))
f_impl = xdot - f_expl
model = AcadosModel()
model.f_impl_expr = f_impl
model.f_expl_expr = f_expl
model.x = x
model.xdot = xdot
model.u = u
# model.z = z
model.p = p
model.name = model_name
return model
def export_chain_mass_model(n_mass, m, D, L):
model_name = 'chain_mass_' + str(n_mass)
x0 = np.array([0, 0, 0]) # fix mass (at wall)
M = n_mass - 2 # number of intermediate masses
nx = (2 * M + 1) * 3 # differential states
nu = 3 # control inputs
xpos = SX.sym('xpos', (M + 1) * 3, 1) # position of fix mass eliminated
xvel = SX.sym('xvel', M * 3, 1)
u = SX.sym('u', nu, 1)
xdot = SX.sym('xdot', nx, 1)
f = SX.zeros(3 * M, 1) # force on intermediate masses
for i in range(M):
f[3 * i + 2] = -9.81
for i in range(M + 1):
if i == 0:
dist = xpos[i * 3:(i + 1) * 3] - x0
else:
dist = xpos[i * 3:(i + 1) * 3] - xpos[(i - 1) * 3:i * 3]
scale = D / m * (1 - L / norm_2(dist))
F = scale * dist
# mass on the right
if i < M:
f[i * 3:(i + 1) * 3] -= F
# mass on the left
if i > 0:
f[(i - 1) * 3:i * 3] += F
# parameters
p = []
x = vertcat(xpos, xvel)
# dynamics
f_expl = vertcat(xvel, u, f)
f_impl = xdot - f_expl
model = AcadosModel()
model.f_impl_expr = f_impl
model.f_expl_expr = f_expl
model.x = x
model.xdot = xdot
model.u = u
model.p = p
model.name = model_name
return model
def export_sine_wave_mhe_ode_model():
model_name = 'sine_wave_mhe_ode'
# set up states & controls
s = SX.sym('s')
amp = SX.sym('amp')
freq = SX.sym('freq')
phase = SX.sym('phase')
trend = SX.sym('trend')
x = vertcat(s, amp, freq, phase, trend)
# (controls) state noise
w_s = SX.sym('w_s')
w = vertcat(w_s)
# xdot
s_dot = SX.sym('s_dot')
amp_dot = SX.sym('amp_dot')
freq_dot = SX.sym('freq_dot')
phase_dot = SX.sym('phase_dot')
trend_dot = SX.sym('trend_dot')
xdot = vertcat(s_dot, amp_dot, freq_dot, phase_dot, trend_dot)
# algebraic variables
# z = None
# parameters
# set up parameters
t = SX.sym('t') # time
p = vertcat(t)
# dynamics
f_expl = vertcat(
trend + amp * cos(2 * pi * freq * t + phase) * 2 * pi * freq, 0, 0, 0,
0)
f_expl[0] = f_expl[0] + w
f_impl = xdot - f_expl
model = AcadosModel()
model.f_impl_expr = f_impl
model.f_expl_expr = f_expl
model.x = x
model.xdot = xdot
model.u = w
# model.z = []
model.p = p
model.name = model_name
return model
def fill_in_acados_model(x, u, p, dynamics, name):
x_dot = cs.MX.sym('x_dot', dynamics.shape)
f_impl = x_dot - dynamics
# Dynamics model
model = AcadosModel()
model.f_expl_expr = dynamics
model.f_impl_expr = f_impl
model.x = x
model.xdot = x_dot
model.u = u
model.p = p
model.name = name
return model
def export_so_ode_ct():
model_name = 'second_order_ode_ct'
# set up states & controls
z1 = SX.sym('z1')
z2 = SX.sym('z2')
x = vertcat(z1, z2)
# control
u = SX.sym('u')
# xdot
z1_dot = SX.sym('z1_dot')
z2_dot = SX.sym('z2_dot')
xdot = vertcat(z1_dot, z2_dot)
# algebraic variables
# z = None
# parameters
# set up parameters
zeta = SX.sym('zeta') # zeta
ts = SX.sym('ts') # ts
Kp = SX.sym('Kp') # Kp
p = vertcat(zeta, ts, Kp)
# dynamics
f_expl = vertcat(
z2, -1 / (ts * ts) * z1 - 2 * zeta / ts * z2 + Kp / (ts * ts) * u)
f_impl = xdot - f_expl
model = AcadosModel()
model.f_impl_expr = f_impl
model.f_expl_expr = f_expl
model.x = x
model.xdot = xdot
model.u = u
# model.z = []
model.p = p
model.name = model_name
return model
def acados_linear_quad_model(num_states, g=9.81):
model_name = 'linear_quad'
A, B = linear_quad_model(num_states)
tc = 0.59375 # mapping control to throttle
# tc = 0.75
scaling_control_mat = np.array([1, 1, 1, (g / tc)])
f_const = 20
# scaling_control_mat = np.array([1, 1, 1, ((g + f_const)/tc)])
# add_mat = np.array([0,0,0,-f_const])
# scaling_control_mat = np.array([1,1,1,1])
# set up states & controls
x = SX.sym('x', 9)
u = SX.sym('u', 4)
# xdot
xdot = SX.sym('xdot', 9)
#parameters
p = []
f_expl = mtimes(A, x) + mtimes(B, u * scaling_control_mat)
# f_expl = mtimes(A,x) + mtimes(B,u * scaling_control_mat + add_mat)
f_impl = xdot - f_expl
model = AcadosModel()
model.f_impl_expr = f_impl
model.f_expl_expr = f_expl
model.x = x
model.xdot = xdot
model.u = u
# model.z = z
model.p = p
model.name = model_name
return model
def acados_linear_quad_model(num_states, g=9.81):
'''
A simple linear model linearized about x = 0, u = [0,0,0,g] but with a
format compatible with acados.
'''
model_name = 'linear_quad'
A, B = linear_quad_model(num_states)
tc = 0.59375 # mapping control to throttle
# tc = 0.75
scaling_control_mat = np.array([1, 1, 1, (g / tc)])
# scaling_control_mat = np.array([1,1,1,1])
# set up states & controls
x = SX.sym('x', 9)
u = SX.sym('u', 4)
# xdot
xdot = SX.sym('xdot', 9)
#parameters
p = []
f_expl = mtimes(A, x) + mtimes(B, u * scaling_control_mat)
f_impl = xdot - f_expl
model = AcadosModel()
model.f_impl_expr = f_impl
model.f_expl_expr = f_expl
model.x = x
model.xdot = xdot
model.u = u
# model.z = z
model.p = p
model.name = model_name
return model
def export_pend_ode_model():
model_name = 'pend_ode'
# set up states & controls
q1 = SX.sym('q1')
q2 = SX.sym('q2')
dq1 = SX.sym('dq1')
dq2 = SX.sym('dq2')
b1 = SX.sym('b1')
b2 = SX.sym('b2')
x = vertcat(q1, q2, dq1, dq2, b1, b2)
# controls
tau1 = SX.sym('tau1')
tau = vertcat(tau1, 0)
u = vertcat(tau1)
# xdot
q1_dot = SX.sym('q1_dot')
q2_dot = SX.sym('q2_dot')
dq1_dot = SX.sym('dq1_dot')
dq2_dot = SX.sym('dq2_dot')
b1_dot = SX.sym('b1_dot')
b2_dot = SX.sym('b2_dot')
xdot = vertcat(q1_dot, q2_dot, dq1_dot, dq2_dot, b1_dot, b2_dot)
# algebraic variables
# z = None
# parameters
# set up parameters
m1 = SX.sym('m1') # mass link 1
m2 = SX.sym('m2') # mass link 2
l1 = SX.sym('l1') # length link 1
l2 = SX.sym('l2') # length link 2
lc1 = SX.sym('lc1') # cm link 1
lc2 = SX.sym('lc2') # cm link 2
g = SX.sym('g')
I1 = (1 / 3) * m1 * l1 * l1
I2 = (1 / 3) * m2 * l2 * l2
p = vertcat(m1, m2, l1, l2, lc1, lc2, g)
# system dynamics.
theta1 = m1 * lc1 * lc1 + m2 * l1 * l1 + I1
theta2 = m2 * lc2 * lc2 + I2
theta3 = m2 * l1 * lc2
theta4 = m1 * lc1 + m2 * l1
theta5 = m2 * lc2
# In matrix form, D(q) -> Inertia Matrix, C(dq,q) ->Coriolis Matrix, F(dq) -> Dissipation matrix
# D(q) ddq + C(q,dq) dq + F(dq) + g(q) = tau
D = SX.zeros(2, 2)
C = SX.zeros(2, 2)
F = SX.zeros(2, 1)
gVec = SX.zeros(2, 1)
D[0, 0] = theta1 + theta2 + 2 * theta3 * cos(q2)
D[1, 0] = theta2 + theta3 * cos(q2)
D[0, 1] = D[1, 0]
D[1, 1] = theta2
C[0, 0] = -theta3 * sin(q2) * dq2
C[1, 0] = -theta3 * sin(q2) * dq2 - theta3 * sin(q2) * dq1
C[0, 1] = theta3 * sin(q2) * dq1
C[1, 1] = 0
F[0, 0] = b1 * dq1
F[1, 0] = b2 * dq2
gVec[0, 0] = theta4 * g * cos(q1) + theta5 * g * cos(q1 + q2)
gVec[1, 0] = theta5 * g * cos(q1 + q2)
# dynamics
f_expl = vertcat(
dq1, dq2, mtimes(inv(D),
tau - mtimes(C, vertcat(dq1, dq2)) - F - gVec), 0, 0)
f_impl = xdot - f_expl
model = AcadosModel()
model.f_impl_expr = f_impl
model.f_expl_expr = f_expl
model.x = x
model.xdot = xdot
model.u = u
# model.z = []
model.p = p
model.name = model_name
return model
def export_quad_ode_model():
model_name = 'quad_ode'
# set up states & controls
q0 = SX.sym('q0')
q1 = SX.sym('q1')
q2 = SX.sym('q2')
q3 = SX.sym('q3')
omegax = SX.sym('omegax')
omegay = SX.sym('omegay')
omegaz = SX.sym('omegaz')
x = vertcat(q0, q1, q2, q3, omegax, omegay, omegaz)
# controls
w1 = SX.sym('w1')
w2 = SX.sym('w2')
w3 = SX.sym('w3')
w4 = SX.sym('w4')
u = vertcat(w1, w2, w3, w4)
# xdot
q0_dot = SX.sym('q0_dot')
q1_dot = SX.sym('q1_dot')
q2_dot = SX.sym('q2_dot')
q3_dot = SX.sym('q3_dot')
omegax_dot = SX.sym('omegax_dot')
omegay_dot = SX.sym('omegay_dot')
omegaz_dot = SX.sym('omegaz_dot')
xdot = vertcat(q0_dot, q1_dot, q2_dot, q3_dot, omegax_dot, omegay_dot,
omegaz_dot)
# algebraic variables
# z = None
# parameters
# set up parameters
rho = SX.sym('rho') # air density
A = SX.sym('A') # propeller area
Cl = SX.sym('Cl') # lift coefficient
Cd = SX.sym('Cd') # drag coefficient
m = SX.sym('m') # mass of quad
g = SX.sym('g') # gravity
J1 = SX.sym('J1') # mom inertia
J2 = SX.sym('J2') # mom inertia
J3 = SX.sym('J3') # mom inertia
J = diag(vertcat(J1, J2, J3))
# This comes from ref[6] pg 449.
# dq = 1/2 * G(q)' * Ω
S = SX.zeros(3, 4)
S[:, 0] = vertcat(-q1, -q2, -q3)
S[:, 1:4] = q0 * np.eye(3) - skew(vertcat(q1, q2, q3))
print("S:", S, S.shape)
#Define angular velocity vector
OMG = vertcat(omegax, omegay, omegaz)
p = vertcat(rho, A, Cl, Cd, m, g, J1, J2, J3)
#Define torque applied to the system
T = SX(3, 1)
T[0, 0] = 0.5 * A * Cl * rho * (w2 * w2 - w4 * w4)
T[1, 0] = 0.5 * A * Cl * rho * (w1 * w1 - w3 * w3)
T[2, 0] = 0.5 * A * Cd * rho * (w1 * w1 - w2 * w2 + w3 * w3 - w4 * w4)
# Reference generation (quaternion to eul) used in cost function.
Eul = SX(3, 1)
Eul[0, 0] = arctan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1**2 + q2**2))
Eul[1, 0] = arcsin(2 * (q0 * q2 - q3 * q1))
Eul[2, 0] = arctan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2**2 + q3**2))
# dynamics
f_expl = vertcat(0.5 * mtimes(transpose(S), OMG),
mtimes(inv(J), (T - cross(OMG, mtimes(J, OMG)))))
f_impl = xdot - f_expl
print("dynamics shape: ", f_expl.shape)
model = AcadosModel()
model.f_impl_expr = f_impl
model.f_expl_expr = f_expl
model.x = x
model.xdot = xdot
model.u = u
# model.z = []
model.p = p
model.name = model_name
# add the nonlinear cost.
# Cost defined in paper
# --- ---
# | roll(x) - roll_ref |
# | pitch(x) - pitch_ref |
# l(x,u,z) = | yaw(x) - yaw_ref |
# | x - x_ref |
# | u - u_ref |
# --- --- 14x1
# --- ---
# | roll(x) - roll_ref |
# | pitch(x) - pitch_ref |
# m(x) = | yaw(x) - yaw_ref |
# | x - x_ref |
# --- --- 10x1
model.cost_y_expr = vertcat(
Eul, x, u) #: CasADi expression for nonlinear least squares
model.cost_y_expr_e = vertcat(
Eul, x) #: CasADi expression for nonlinear least squares, terminal
model.con_h_expr = q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3
# # yReference checking.
# refCheck = Function('y_ref', [vertcat(x, u)], [model.cost_y_expr])
# print(refCheck)
# y_ref = refCheck(np.array([0.566, 0.571, 0.168, 0.571, 0.1, 0.2, 0.3, 1, 2, 3, 4]))
# print(y_ref)
#
# dynCheck = Function('sys_dyn', [vertcat(x, u),p], [f_expl])
# print(dynCheck)
# dx_ref = dynCheck(np.array([0.566, 0.571, 0.168, 0.571, 0.1, 0.2, 0.3, 1, 2, 3, 4]),np.array([1, 1, 1, 1, 1, 1, 1, 1, 1]))
# print(dx_ref)
return model
def acados_settings_dyn(Tf, N, modelparams = "modelparams.yaml"):
#create render arguments
ocp = AcadosOcp()
#load model
model, constraints = dynamic_model(modelparams)
# define acados ODE
model_ac = AcadosModel()
model_ac.f_impl_expr = model.f_impl_expr
model_ac.f_expl_expr = model.f_expl_expr
model_ac.x = model.x
model_ac.xdot = model.xdot
#inputvector
model_ac.u = model.u
#parameter vector
model_ac.p = model.p
#parameter vector
model_ac.z = model.z
#external cost function
model_ac.cost_expr_ext_cost = model.stage_cost
#model_ac.cost_expr_ext_cost_e = 0
model_ac.con_h_expr = model.con_h_expr
model_ac.name = model.name
ocp.model = model_ac
# set dimensions
nx = model.x.size()[0]
nu = model.u.size()[0]
nz = model.z.size()[0]
np = model.p.size()[0]
#ny = nu + nx
#ny_e = nx
ocp.dims.nx = nx
ocp.dims.nz = nz
#ocp.dims.ny = ny
#ocp.dims.ny_e = ny_e
ocp.dims.nu = nu
ocp.dims.np = np
ocp.dims.nh = 1
#ocp.dims.ny = ny
#ocp.dims.ny_e = ny_e
ocp.dims.nbx = 3
ocp.dims.nsbx = 0
ocp.dims.nbu = nu
#number of soft on h constraints
ocp.dims.nsh = 1
ocp.dims.ns = 1
ocp.dims.N = N
# set cost to casadi expression defined above
ocp.cost.cost_type = "EXTERNAL"
#ocp.cost.cost_type_e = "EXTERNAL"
ocp.cost.zu = 1000 * npy.ones((ocp.dims.ns,))
ocp.cost.Zu = 1000 * npy.ones((ocp.dims.ns,))
ocp.cost.zl = 1000 * npy.ones((ocp.dims.ns,))
ocp.cost.Zl = 1000 * npy.ones((ocp.dims.ns,))
#not sure if needed
#unscale = N / Tf
#constraints
#stagewise constraints for tracks with slack
ocp.constraints.uh = npy.array([0.00])
ocp.constraints.lh = npy.array([-10])
ocp.constraints.lsh = 0.1*npy.ones(ocp.dims.nsh)
ocp.constraints.ush = -0.1*npy.ones(ocp.dims.nsh)
ocp.constraints.idxsh = npy.array([0])
#ocp.constraints.Jsh = 1
# boxconstraints
# xvars = ['posx', 'posy', 'phi', 'vx', 'vy', 'omega', 'd', 'delta', 'theta']
ocp.constraints.lbx = npy.array([10, 10, 100, model.vx_min, model.vy_min, model.omega_min, model.d_min, model.delta_min, model.theta_min])
ocp.constraints.ubx = npy.array([10, 10, 100, model.vx_max, model.vy_max, model.omega_max, model.d_max, model.delta_max, model.theta_max])
ocp.constraints.idxbx = npy.arange(nx)
#ocp.constraints.lsbx= -0.1 * npy.ones(ocp.dims.nbx)
#ocp.constraints.usbx= 0.1 * npy.ones(ocp.dims.nbx)
#ocp.constraints.idxsbx= npy.array([6,7,8])
#print("ocp.constraints.idxbx: ",ocp.constraints.idxbx)
ocp.constraints.lbu = npy.array([model.ddot_min, model.deltadot_min, model.thetadot_min])
ocp.constraints.ubu = npy.array([model.ddot_max, model.deltadot_max, model.thetadot_max])
ocp.constraints.idxbu = npy.array([0, 1, 2])
# set intial condition
ocp.constraints.x0 = model.x0
# set QP solver and integration
ocp.solver_options.tf = Tf
# ocp.solver_options.qp_solver = 'FULL_CONDENSING_QPOASES'
ocp.solver_options.qp_solver = "PARTIAL_CONDENSING_HPIPM"
ocp.solver_options.nlp_solver_type = "SQP"#"SQP_RTI" #
ocp.solver_options.hessian_approx = "GAUSS_NEWTON"
ocp.solver_options.integrator_type = "ERK"
ocp.parameter_values = npy.zeros(np)
#ocp.solver_options.sim_method_num_stages = 4
#ocp.solver_options.sim_method_num_steps = 3
ocp.solver_options.nlp_solver_step_length = 0.01
ocp.solver_options.nlp_solver_max_iter = 50
ocp.solver_options.tol = 1e-4
#ocp.solver_options.print_level = 1
# ocp.solver_options.nlp_solver_tol_comp = 1e-1
# create solver
acados_solver = AcadosOcpSolver(ocp, json_file="acados_ocp_dynamic.json")
return constraints, model, acados_solver, ocp
def acados_linear_quad_model_moving_eq(num_states, g=9.81):
model_name = 'linear_quad_v2'
tc = 0.59375
# scaling_control_mat = np.array([1, 1, 1, (g/tc)])
gravity = SX.zeros(3)
gravity[2] = g
x = SX.sym('x', 9)
u = SX.sym('u', 4)
xdot = SX.sym('xdot', 9)
r1 = SX(3, 3)
r1[0, 0] = 1
r1[1, 1] = cos(x[3])
r1[1, 2] = -sin(x[3])
r1[2, 1] = sin(x[3])
r1[2, 2] = cos(x[3])
r2 = SX(3, 3)
r2[0, 0] = cos(x[4])
r2[0, 2] = sin(x[4])
r2[1, 1] = 1
r2[2, 0] = -sin(x[4])
r2[2, 2] = cos(x[4])
r3 = SX(3, 3)
r3[0, 0] = cos(x[5])
r3[0, 1] = -sin(x[5])
r3[1, 0] = sin(x[5])
r3[1, 1] = cos(x[5])
r3[2, 2] = 1
tau = SX(3, 3)
tau[:, 0] = (inv(r2))[:, 0]
tau[:, 1] = (inv(r2))[:, 1]
tau[:, 2] = (inv(r2 @ r1))[:, 2]
f_nl = vertcat(x[6:9],
inv(tau) @ u[0:3],
-((r3 @ r1 @ r2)[:, 2]) * u[3] * (g / tc) + gravity)
# f_const = 20
# f_nl = vertcat(
# x[6:9],
# inv(tau) @ u[0:3],
# - ((r3 @ r1 @ r2)[:,2]) * (u[3] * ((g + f_const)/tc) - f_const) + gravity
# )
A = jacobian(f_nl, x)
B = jacobian(f_nl, u)
p = []
f_expl = mtimes(A, x) + mtimes(B, u)
f_impl = xdot - f_expl
model = AcadosModel()
model.f_impl_expr = f_impl
model.f_expl_expr = f_expl
model.x = x
model.xdot = xdot
model.u = u
# model.z = z
model.p = p
model.name = model_name
return model
def acados_settings(Tf, N):
# create render arguments
ocp = AcadosOcp()
# export model
model, constraint = usv_model()
# define acados ODE
model_ac = AcadosModel()
model_ac.f_impl_expr = model.f_impl_expr
model_ac.f_expl_expr = model.f_expl_expr
model_ac.x = model.x
model_ac.xdot = model.xdot
model_ac.u = model.U
model_ac.z = model.z
model_ac.p = model.p
model_ac.name = model.name
ocp.model = model_ac
# define constraint
model_ac.con_h_expr = constraint.expr
# set dimensions
nx = model.x.size()[0]
nu = model.U.size()[0]
ny = nx + nu
ny_e = nx
ocp.dims.N = N
ns = 8
nsh = 8
# set cost
Q = np.diag([0, 0, 0.05, 0.01, 0, 0, 0, 0])
R = np.eye(nu)
R[0, 0] = 0.2
Qe = np.diag([0, 0, 0.1, 0.05, 0, 0, 0, 0])
ocp.cost.cost_type = "LINEAR_LS"
ocp.cost.cost_type_e = "LINEAR_LS"
#unscale = N / Tf
#ocp.cost.W = unscale * scipy.linalg.block_diag(Q, R)
#ocp.cost.W_e = Qe / unscale
ocp.cost.W = scipy.linalg.block_diag(Q, R)
ocp.cost.W_e = Qe
Vx = np.zeros((ny, nx))
Vx[:nx, :nx] = np.eye(nx)
ocp.cost.Vx = Vx
Vu = np.zeros((ny, nu))
Vu[8, 0] = 1.0
ocp.cost.Vu = Vu
Vx_e = np.zeros((ny_e, nx))
Vx_e[:nx, :nx] = np.eye(nx)
ocp.cost.Vx_e = Vx_e
ocp.cost.zl = 10 * np.ones((ns, )) #previously 100
ocp.cost.Zl = 0 * np.ones((ns, ))
ocp.cost.zu = 10 * np.ones((ns, )) #previously 100
ocp.cost.Zu = 0 * np.ones((ns, ))
# set intial references
ocp.cost.yref = np.array([0, 0, 0, 0, 0, 0, 0, 0, 0])
ocp.cost.yref_e = np.array([0, 0, 0, 0, 0, 0, 0, 0])
# setting constraints
#ocp.constraints.lbx = np.array([model.psieddot_min])
#ocp.constraints.ubx = np.array([model.psieddot_max])
#ocp.constraints.idxbx = np.array([8])
ocp.constraints.lbu = np.array([model.Upsieddot_min])
ocp.constraints.ubu = np.array([model.Upsieddot_max])
ocp.constraints.idxbu = np.array([0])
# ocp.constraints.lsbx=np.zero s([1])
# ocp.constraints.usbx=np.zeros([1])
# ocp.constraints.idxsbx=np.array([1])
ocp.constraints.lh = np.array([
constraint.distance_min, constraint.distance_min,
constraint.distance_min, constraint.distance_min,
constraint.distance_min, constraint.distance_min,
constraint.distance_min, constraint.distance_min
])
ocp.constraints.uh = np.array([
1000000, 1000000, 1000000, 1000000, 1000000, 1000000, 1000000, 1000000
])
'''ocp.constraints.lsh = np.zeros(nsh)
ocp.constraints.ush = np.zeros(nsh)
ocp.constraints.idxsh = np.array([0, 2])'''
ocp.constraints.lsh = np.array(
[-0.2, -0.2, -0.2, -0.2, -0.2, -0.2, -0.2, -0.2])
ocp.constraints.ush = np.array([0, 0, 0, 0, 0, 0, 0, 0])
ocp.constraints.idxsh = np.array([0, 1, 2, 3, 4, 5, 6, 7])
# set intial condition
ocp.constraints.x0 = model.x0
#ocp.parameter_values = np.array([4,4,4,8,4,12,4,20,100,100,100,100,100,100,100,100])
ocp.parameter_values = np.array([
100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100,
100, 100
])
# set QP solver and integration
ocp.solver_options.tf = Tf
#ocp.solver_options.qp_solver = 'FULL_CONDENSING_QPOASES'
ocp.solver_options.qp_solver = "PARTIAL_CONDENSING_HPIPM"
ocp.solver_options.nlp_solver_type = "SQP_RTI"
#ocp.solver_options.nlp_solver_type = "SQP"
ocp.solver_options.hessian_approx = "GAUSS_NEWTON"
ocp.solver_options.integrator_type = "ERK"
#ocp.solver_options.sim_method_num_stages = 4
#ocp.solver_options.sim_method_num_steps = 3
#ocp.solver_options.nlp_solver_max_iter = 200
#ocp.solver_options.tol = 1e-4
#ocp.solver_options.qp_solver_tol_stat = 1e-2
#ocp.solver_options.qp_solver_tol_eq = 1e-2
#ocp.solver_options.qp_solver_tol_ineq = 1e-2
#ocp.solver_options.qp_solver_tol_comp = 1e-2
# create solver
acados_solver = AcadosOcpSolver(ocp, json_file="acados_ocp.json")
return constraint, model, acados_solver
def __init__(self, ):
#constants
g = 9.8066
l = 0.1
# control inputs
roll_ref = ca.SX.sym('roll_ref_')
pitch_ref = ca.SX.sym('pitch_ref_')
yaw_ref = ca.SX.sym('yaw_ref_')
thrust_ref = ca.SX.sym('thrust_ref_')
controls = ca.vcat([roll_ref, pitch_ref, yaw_ref, thrust_ref])
# model constants after SI
roll_gain = 2.477
roll_tau = 0.477
pitch_gain = 2.477
pitch_tau = 0.477
# roll_gain = 1.477
# roll_tau = 0.477
# pitch_gain = 1.477
# pitch_tau = 0.477
# model states
x = ca.SX.sym('x')
y = ca.SX.sym('y')
z = ca.SX.sym('z')
vx = ca.SX.sym('vx')
vy = ca.SX.sym('vy')
vz = ca.SX.sym('vz')
s = ca.SX.sym('s') # pendulum x coordinate
r = ca.SX.sym('r') # pendulum y coordinate
ds = ca.SX.sym('ds')
dr = ca.SX.sym('dr')
roll = ca.SX.sym('roll')
pitch = ca.SX.sym('pitch')
yaw = ca.SX.sym('yaw')
states = ca.vcat([x, y, z, vx, vy, vz, s, r, ds, dr, roll, pitch, yaw])
### DYNAMICS
dragacc1, dragacc2 = 0.0, 0.0
h = ca.sqrt(l * l - s * s - r * r)
ddx = thrust_ref * (ca.sin(roll) * ca.sin(yaw) + ca.cos(roll) *
ca.cos(yaw) * ca.sin(pitch)) - dragacc1
ddy = thrust_ref * (ca.cos(roll) * ca.sin(pitch) * ca.sin(yaw) -
ca.sin(roll) * ca.cos(yaw)) - dragacc2
ddz = thrust_ref * ca.cos(roll) * ca.cos(pitch) - g
rhs = [states[3], states[4], states[5]]
rhs.append(ddx)
rhs.append(ddy)
rhs.append(ddz)
rhs.append(states[8])
rhs.append(states[9])
rhs.append( #dds
-1 / (l * l * h * h) *
(l * l * l * l * ddx + s * s * s * (s * ddx - ddy) + l * l * s *
(ds * ds + dr * dr) - l * l * ddx *
(2 * s * s + r * r) - s * r * r * ds * ds + s * s * s * s * s *
(ddz + g) / h + s * r * r * r * ddy + s * s * s * r *
(ddy + r * 2 * ddz / h) + 2 * s * s * r * ds * dr +
l * l * l * l * s * ddz / h - l * l * s * r * ddy + 1 / h *
(2 * g * s * s * s * r * r + g * l * l * l * l * s -
2 * l * l * s * s * s * ddz + s * r * r * r * r * ddz -
2 * g * l * l * s * s * s + g * s * r * r * r * r -
2 * l * l * s * r * r * ddz - 2 * g * l * l * s * r * r)))
rhs.append( #ddr
-1 / (l * l * h * h) *
(l * l * l * l * ddy - r * r * r *
(r * ddy - ds * ds) + s * s * r * r * ddy + l * l * r *
(ds * ds + dr * dr) - l * l * s * s * ddy -
2 * l * l * r * r * ddy - s * s * r * dr * dr + s * r *
(r * r * ddx + s * s * ddx) + 2 * s * r * r * ds * dr -
l * l * s * r * ddx + 1 / h *
(r * r * r * r * r * (ddz + g) + 2 * s * s * r * r * r * ddz +
l * l * l * l * r * ddz + 2 * r * r * r *
(g * s * s - l * l * ddz - 2 * g * l * l) +
g * l * l * l * l * r + s * s * s * s * r *
(g + ddz) - 2 * l * l * s * s * r * (ddz + g))))
rhs.append((roll_gain * roll_ref - roll) / roll_tau) #droll
rhs.append((pitch_gain * pitch_ref - pitch) / pitch_tau) #dpitch
rhs.append(yaw_ref) #dyaw
self.f = ca.Function('f', [states, controls], [ca.vcat(rhs)])
# acados model
x_dot = ca.SX.sym('x_dot', len(rhs))
f_impl = x_dot - self.f(states, controls)
model = AcadosModel()
model.f_expl_expr = self.f(states, controls)
model.f_impl_expr = f_impl
model.x = states
model.xdot = x_dot
model.u = controls
model.p = [] # ca.vcat([])
model.name = 'quadrotor'
### CONSTRAINTS
constraints = ca.types.SimpleNamespace()
constraints.roll_min = np.deg2rad(-45)
constraints.pitch_min = np.deg2rad(-45)
constraints.roll_max = np.deg2rad(45)
constraints.pitch_max = np.deg2rad(45)
constraints.yaw_min = np.deg2rad(-45)
constraints.yaw_max = np.deg2rad(45)
constraints.thrust_min = 0.0 * g # 0.5*g
constraints.thrust_max = 1.9 * g # 1.9*g
# constraints.s_min = -l
# constraints.s_max = l
# constraints.r_min = -l
# constraints.r_max = l
self.model = model
self.constraints = constraints
def export_quaternion_ode_model():
model_name = 'quaternion_ode'
# set up states & controls
q0 = SX.sym('q0')
q1 = SX.sym('q1')
q2 = SX.sym('q2')
q3 = SX.sym('q3')
omegax = SX.sym('omegax')
omegay = SX.sym('omegay')
omegaz = SX.sym('omegaz')
x = vertcat(q0, q1, q2, q3, omegax, omegay, omegaz)
# controls
omegax_cont = SX.sym('omegax_cont')
omegay_cont = SX.sym('omegay_cont')
omegaz_cont = SX.sym('omegaz_cont')
u = vertcat(omegax_cont, omegay_cont, omegaz_cont)
# xdot
q0_dot = SX.sym('q0_dot')
q1_dot = SX.sym('q1_dot')
q2_dot = SX.sym('q2_dot')
q3_dot = SX.sym('q3_dot')
omegax_dot = SX.sym('omegax_dot')
omegay_dot = SX.sym('omegay_dot')
omegaz_dot = SX.sym('omegaz_dot')
xdot = vertcat(q0_dot, q1_dot, q2_dot, q3_dot, omegax_dot, omegay_dot,
omegaz_dot)
# algebraic variables
# z = None
# parameters
p = []
# This comes from ref[6] pg 449.
# dq = 1/2 * G(q)' * Ω
S = SX.zeros(3, 4)
S[:, 0] = vertcat(-q1, -q2, -q3)
S[:, 1:4] = q0 * np.eye(3) - skew(vertcat(q1, q2, q3))
print("S:", S, S.shape)
OMG = vertcat(omegax, omegay, omegaz)
# dynamics
f_expl = vertcat(0.5 * mtimes(S.T, OMG), u)
f_impl = xdot - f_expl
model = AcadosModel()
model.f_impl_expr = f_impl
model.f_expl_expr = f_expl
model.x = x
model.xdot = xdot
model.u = u
# model.z = []
model.p = p
model.name = model_name
# Reference generation (quaternion to eul) used in cost function.
Eul = SX(3, 1)
Eul[0, 0] = arctan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1**2 + q2**2))
Eul[1, 0] = arcsin(2 * (q0 * q2 - q3 * q1))
Eul[2, 0] = arctan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2**2 + q3**2))
quat_to_eul = Function("quat_to_eul", [x, u], [Eul])
model.cost_y_expr = vertcat(
Eul, x[4:, :], u) #: CasADi expression for nonlinear least squares
model.cost_y_expr_e = vertcat(
Eul,
x[4:, :]) #: CasADi expression for nonlinear least squares, terminal
model.con_h_expr = q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3
return model
def generate(self, dae, name='tunempc', opts={}):
""" Create embeddable NLP solver
"""
from acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver, AcadosSimSolver
# extract dimensions
nx = self.__nx
nu = self.__nu + self.__ns # treat slacks as pseudo-controls
# extract reference
ref = self.__ref
xref = np.squeeze(self.__ref[0][:nx], axis=1)
uref = np.squeeze(self.__ref[0][nx:nx + nu], axis=1)
# create acados model
model = AcadosModel()
model.x = ca.MX.sym('x', nx)
model.u = ca.MX.sym('u', nu)
model.p = []
model.name = name
# detect input type
n_in = dae.n_in()
if n_in == 2:
# xdot = f(x, u)
if 'integrator_type' in opts:
if opts['integrator_type'] == 'IRK':
xdot = ca.MX.sym('xdot', nx)
model.xdot = xdot
model.f_impl_expr = xdot - dae(model.x,
model.u[:self.__nu])
model.f_expl_expr = xdot
elif opts['integrator_type'] == 'ERK':
model.f_expl_expr = dae(model.x, model.u[:self.__nu])
else:
raise ValueError('Provide numerical integrator type!')
else:
xdot = ca.MX.sym('xdot', nx)
model.xdot = xdot
model.f_expl_expr = xdot
if n_in == 3:
# f(xdot, x, u) = 0
model.f_impl_expr = dae(xdot, model.x, model.u[:self.__nu])
elif n_in == 4:
# f(xdot, x, u, z) = 0
nz = dae.size1_in(3)
z = ca.MX.sym('z', nz)
model.z = z
model.f_impl_expr = dae(xdot, model.x, model.u[:self.__nu], z)
else:
raise ValueError(
'Invalid number of inputs for system dynamics function.')
if self.__gnl is not None:
model.con_h_expr = self.__gnl(model.x, model.u[:self.__nu],
model.u[self.__nu:])
if self.__type == 'economic':
model.cost_expr_ext_cost = self.__cost(model.x,
model.u[:self.__nu])
# create acados ocp
ocp = AcadosOcp()
ocp.model = model
ny = nx + nu
ny_e = nx
# set horizon length
ocp.dims.N = self.__N
# set cost module
if self.__type == 'economic':
# set cost function type to external (provided in model)
ocp.cost.cost_type = 'EXTERNAL'
else:
# set weighting matrices
if self.__type == 'tracking':
ocp.cost.W = self.__Href[0][0]
# set-up linear least squares cost
ocp.cost.cost_type = 'LINEAR_LS'
ocp.cost.W_e = np.zeros((nx, nx))
ocp.cost.Vx = np.zeros((ny, nx))
ocp.cost.Vx[:nx, :nx] = np.eye(nx)
Vu = np.zeros((ny, nu))
Vu[nx:, :] = np.eye(nu)
ocp.cost.Vu = Vu
ocp.cost.Vx_e = np.eye(nx)
ocp.cost.yref = np.squeeze(
ca.vertcat(xref,uref).full() - \
ct.mtimes(np.linalg.inv(ocp.cost.W),self.__qref[0][0].T).full(), # gradient term
axis = 1
)
ocp.cost.yref_e = np.zeros((ny_e, ))
if n_in == 4: # DAE flag
ocp.cost.Vz = np.zeros((ny, nz))
# initial condition
ocp.constraints.x0 = xref
# set inequality constraints
ocp.constraints.constr_type = 'BGH'
if self.__S['C'] is not None:
C = self.__S['C'][0][:, :nx]
D = self.__S['C'][0][:, nx:]
lg = -self.__S['e'][0] + ct.mtimes(C, xref).full() + ct.mtimes(
D, uref).full()
ug = 1e8 - self.__S['e'][0] + ct.mtimes(
C, xref).full() + ct.mtimes(D, uref).full()
ocp.constraints.lg = np.squeeze(lg, axis=1)
ocp.constraints.ug = np.squeeze(ug, axis=1)
ocp.constraints.C = C
ocp.constraints.D = D
if 'usc' in self.__vars:
if 'us' in self.__vars:
arg = [
self.__vars['x'], self.__vars['u'], self.__vars['us'],
self.__vars['usc']
]
else:
arg = [
self.__vars['x'], self.__vars['u'], self.__vars['usc']
]
Jsg = ca.Function(
'Jsg', [self.__vars['usc']],
[ca.jacobian(self.__h(*arg), self.__vars['usc'])])(0.0)
self.__Jsg = Jsg.full()[:-self.__nsc, :]
ocp.constraints.Jsg = self.__Jsg
ocp.cost.Zl = np.zeros((self.__nsc, ))
ocp.cost.Zu = np.zeros((self.__nsc, ))
ocp.cost.zl = np.squeeze(self.__scost.full(), axis=1)
ocp.cost.zu = np.squeeze(self.__scost.full(), axis=1)
# set nonlinear equality constraints
if self.__gnl is not None:
ocp.constraints.lh = np.zeros(self.__ns, )
ocp.constraints.uh = np.zeros(self.__ns, )
# terminal constraint:
x_term = self.__p_operator(model.x)
Jbx = ca.Function('Jbx', [model.x],
[ca.jacobian(x_term, model.x)])(0.0)
ocp.constraints.Jbx_e = Jbx.full()
ocp.constraints.lbx_e = np.squeeze(self.__p_operator(xref).full(),
axis=1)
ocp.constraints.ubx_e = np.squeeze(self.__p_operator(xref).full(),
axis=1)
for option in list(opts.keys()):
setattr(ocp.solver_options, option, opts[option])
self.__acados_ocp_solver = AcadosOcpSolver(ocp,
json_file='acados_ocp_' +
model.name + '.json')
self.__acados_integrator = AcadosSimSolver(ocp,
json_file='acados_ocp_' +
model.name + '.json')
# set initial guess
self.__set_acados_initial_guess()
return self.__acados_ocp_solver, self.__acados_integrator
def export_car_ode_model(mb, mw, Iw, Rw):
model_name = 'car_ode'
#system parameters
mt = mb + mw
g = 9.81 # [m/s^2]
# set up states & controls
x1 = MX.sym('x1')
z = MX.sym('z')
phi = MX.sym('phi')
l = MX.sym('l')
theta = MX.sym('theta')
x1_dot = MX.sym('x1_dot')
z_dot = MX.sym('z_dot')
phi_dot = MX.sym('phi_dot')
l_dot = MX.sym('l_dot')
theta_dot = MX.sym('theta_dot')
x = vertcat(x1, z, phi, l, theta, x1_dot, z_dot, phi_dot, l_dot, theta_dot)
# controls
tau = MX.sym('tau')
f = MX.sym('f')
lambda_x = MX.sym('lambda_x')
lambda_z = MX.sym('lambda_z')
u = vertcat(tau, f, lambda_x, lambda_z)
# xdot
x1_ddot = MX.sym('x1_ddot')
z_ddot = MX.sym('z_ddot')
phi_ddot = MX.sym('phi_ddot')
l_ddot = MX.sym('l_ddot')
theta_ddot = MX.sym('theta_ddot')
xdot = vertcat(x1_dot, z_dot, phi_dot, l_dot, theta_dot, x1_ddot, z_ddot,
phi_ddot, l_ddot, theta_ddot)
# inertia matrix
M = MX(5, 5)
M[0, 0] = mt
M[0, 1] = 0
M[0, 2] = 0
M[0, 3] = mb * sin(theta)
M[0, 4] = mb * l * cos(theta)
M[1, 0] = 0
M[1, 1] = mt
M[1, 2] = 0
M[1, 3] = mb * cos(theta)
M[1, 4] = -mb * l * sin(theta)
M[2, 0] = 0
M[2, 1] = 0
M[2, 2] = Iw
M[2, 3] = 0
M[2, 4] = 0
M[3, 0] = mb * sin(theta)
M[3, 1] = mb * cos(theta)
M[3, 2] = 0
M[3, 3] = mb
M[3, 4] = 0
M[4, 0] = mb * l * cos(theta)
M[4, 1] = -mb * l * sin(theta)
M[4, 2] = 0
M[4, 3] = 0
M[4, 4] = mb * (l**2)
# coriolis and centrifugal terms
C = MX(5, 5)
C[0, 0] = 0
C[0, 1] = 0
C[0, 2] = 0
C[0, 3] = 2 * mb * cos(theta) * theta_dot
C[0, 4] = -mb * l * sin(theta) * theta_dot
C[1, 0] = 0
C[1, 1] = 0
C[1, 2] = 0
C[1, 3] = -2 * mb * sin(theta) * theta_dot
C[1, 4] = -mb * l * cos(theta) * theta_dot
C[2, 0] = 0
C[2, 1] = 0
C[2, 2] = 0
C[2, 3] = 0
C[2, 4] = 0
C[3, 0] = 0
C[3, 1] = 0
C[3, 2] = 0
C[3, 3] = 0
C[3, 4] = -mb * l * theta_dot
C[4, 0] = 0
C[4, 1] = 0
C[4, 2] = 0
C[4, 3] = 2 * mb * l * theta_dot
C[4, 4] = 0
# gravity term
G = MX(5, 1)
G[0, 0] = 0
G[1, 0] = g * mt
G[2, 0] = 0
G[3, 0] = g * mb * cos(theta)
G[4, 0] = -g * mb * l * sin(theta)
# define S
S = MX(2, 5)
S[0, 0] = 0
S[0, 1] = 0
S[0, 2] = 1
S[0, 3] = 0
S[0, 4] = -1
S[1, 0] = 0
S[1, 1] = 0
S[1, 2] = 0
S[1, 3] = 1
S[1, 4] = 0
# define Jc
Jc = MX(2, 5)
Jc[0, 0] = 1
Jc[0, 1] = 0
Jc[0, 2] = -Rw
Jc[0, 3] = 0
Jc[0, 4] = 0
Jc[1, 0] = 0
Jc[1, 1] = 1
Jc[1, 2] = 0
Jc[1, 3] = 0
Jc[1, 4] = 0
# compute acceleration
ddq = mtimes(
inv(M),
(mtimes(transpose(S), vertcat(tau, f)) +
mtimes(transpose(Jc), vertcat(lambda_x, lambda_z)) -
mtimes(C, vertcat(x1_dot, z_dot, phi_dot, l_dot, theta_dot)) - G))
print(ddq.shape)
# dynamics
f_expl = vertcat(x1_dot, z_dot, phi_dot, l_dot, theta_dot, ddq)
print(f_expl.shape)
f_impl = xdot - f_expl
model = AcadosModel()
model.f_impl_expr = f_impl
model.f_expl_expr = f_expl
model.x = x
model.xdot = xdot
model.u = u
# algebraic variables
model.z = vertcat([])
# parameters
p = vertcat([])
model.p = p
model.name = model_name
# activate1 = MX.sym('activate1')
# activate2 = MX.sym('activate2')
# activate3 = MX.sym('activate3')
# activate4 = MX.sym('activate4')
# activate5 = MX.sym('activate5')
# activate6 = MX.sym('activate6')
# activate7 = MX.sym('activate7')
# model.p = vertcat(activate1, activate2, activate3, activate4, activate5, activate6, activate7)
model.con_h_expr = vertcat((lambda_z - fabs(lambda_x)),
(x1_dot - Rw * phi_dot))
print(f"SHAPE: {model.con_h_expr}")
return model
def export_car_ode_model(mb, mw, Iw, Rw, l):
model_name = 'car_phase0_ode'
#system parameters
mt = mb + mw
g = 9.81 # [m/s^2]
# set up states & controls
phi = MX.sym('phi')
theta = MX.sym('theta')
phi_dot = MX.sym('phi_dot')
theta_dot = MX.sym('theta_dot')
x = vertcat(phi, theta,
phi_dot, theta_dot)
# controls
tau = MX.sym('tau')
u = vertcat(tau)
# xdot
phi_ddot = MX.sym('phi_ddot')
theta_ddot = MX.sym('theta_ddot')
xdot = vertcat(phi_dot, theta_dot,
phi_ddot, theta_ddot)
phidd = tau/(Iw+mt*Rw*Rw)
# dynamics
f_expl = vertcat(phi_dot,
theta_dot,
phidd,
(1/mb*l*l)*(-mb*Rw*phidd*l*sin(theta) + mb*g*l*cos(theta) - tau)
)
print(f_expl.shape)
f_impl = xdot - f_expl
model = AcadosModel()
model.f_impl_expr = f_impl
model.f_expl_expr = f_expl
model.x = x
model.xdot = xdot
model.u = u
# algebraic variables
model.z = vertcat([])
# parameters
p = vertcat([])
model.p = p
model.name = model_name
# activate1 = MX.sym('activate1')
# activate2 = MX.sym('activate2')
# activate3 = MX.sym('activate3')
# activate4 = MX.sym('activate4')
# activate5 = MX.sym('activate5')
# activate6 = MX.sym('activate6')
# activate7 = MX.sym('activate7')
# model.p = vertcat(activate1, activate2, activate3, activate4, activate5, activate6, activate7)
#model.con_h_expr = vertcat( (lambda_z - fabs(lambda_x)),
# (x1_dot-Rw*phi_dot))
print(f"SHAPE: {model.con_h_expr}")
return model
def acados_linear_quad_model_moving_eq(num_states, g=9.81):
'''
A linear model linearized about the moving point. (I am not certain what I
am linearizing about here actually). I compute the Jacobian at every
execution basically.
'''
model_name = 'linear_quad_v2'
tc = 0.59375
# scaling_control_mat = np.array([1, 1, 1, (g/tc)])
gravity = SX.zeros(3)
gravity[2] = g
x = SX.sym('x', 9)
u = SX.sym('u', 4)
xdot = SX.sym('xdot', 9)
r1 = SX(3, 3)
r1[0, 0] = 1
r1[1, 1] = cos(x[3])
r1[1, 2] = -sin(x[3])
r1[2, 1] = sin(x[3])
r1[2, 2] = cos(x[3])
r2 = SX(3, 3)
r2[0, 0] = cos(x[4])
r2[0, 2] = sin(x[4])
r2[1, 1] = 1
r2[2, 0] = -sin(x[4])
r2[2, 2] = cos(x[4])
r3 = SX(3, 3)
r3[0, 0] = cos(x[5])
r3[0, 1] = -sin(x[5])
r3[1, 0] = sin(x[5])
r3[1, 1] = cos(x[5])
r3[2, 2] = 1
tau = SX(3, 3)
tau[:, 0] = (inv(r2))[:, 0]
tau[:, 1] = (inv(r2))[:, 1]
tau[:, 2] = (inv(r2 @ r1))[:, 2]
f_nl = vertcat(x[6:9],
inv(tau) @ u[0:3],
-((r3 @ r1 @ r2)[:, 2]) * u[3] * (g / tc) + gravity)
A = jacobian(f_nl, x)
B = jacobian(f_nl, u)
p = []
f_expl = mtimes(A, x) + mtimes(B, u)
f_impl = xdot - f_expl
model = AcadosModel()
model.f_impl_expr = f_impl
model.f_expl_expr = f_expl
model.x = x
model.xdot = xdot
model.u = u
# model.z = z
model.p = p
model.name = model_name
return model
def acados_nonlinear_quad_model(num_states, g=9.81):
'''
A non-linear model of the quadrotor. We assume the input
u = [phi_dot,theta_dot,psi_dot,throttle]. Therefore, the system has only 9
states.
'''
model_name = 'nonlinear_quad'
position = SX.sym('o', 3)
velocity = SX.sym('v', 3)
phi = SX.sym('phi')
theta = SX.sym('theta')
psi = SX.sym('psi')
gravity = SX.zeros(3)
gravity[2] = g
x = vertcat(position, phi, theta, psi, velocity)
p = SX.sym('p')
q = SX.sym('q')
r = SX.sym('r')
F = SX.sym('F')
u = vertcat(p, q, r, F)
r1 = SX(3, 3)
r1[0, 0] = 1
r1[1, 1] = cos(phi)
r1[1, 2] = -sin(phi)
r1[2, 1] = sin(phi)
r1[2, 2] = cos(phi)
r2 = SX(3, 3)
r2[0, 0] = cos(theta)
r2[0, 2] = sin(theta)
r2[1, 1] = 1
r2[2, 0] = -sin(theta)
r2[2, 2] = cos(theta)
r3 = SX(3, 3)
r3[0, 0] = cos(psi)
r3[0, 1] = -sin(psi)
r3[1, 0] = sin(psi)
r3[1, 1] = cos(psi)
r3[2, 2] = 1
tau = SX(3, 3)
tau[:, 0] = (inv(r2))[:, 0]
tau[:, 1] = (inv(r2))[:, 1]
tau[:, 2] = (inv(r2 @ r1))[:, 2]
# tau[:,0] = (inv(r1))[:,0]
# tau[:,1] = (inv(r1))[:,1]
# tau[:,2] = (inv(r1 @ r2))[:,2]
# tau[0,0] = cos(theta)
# tau[0,2] = -sin(theta)
# tau[1,1] = 1
# tau[1,2] = sin(phi) * cos(theta)
# tau[2,0] = sin(theta)
# tau[2,2] = cos(phi) * cos(theta)
tc = 0.59375 # mapping control to throttle
# tc = 0.75
# scaling_control_mat = np.array([1, 1, 1, (g/tc)])
f_expl = vertcat(velocity,
inv(tau) @ u[0:3],
-((r3 @ r1 @ r2)[:, 2]) * u[3] * (g / tc) + gravity)
# xdot
xdot = SX.sym('xdot', 9)
#parameters
p = []
f_impl = xdot - f_expl
model = AcadosModel()
model.f_impl_expr = f_impl
model.f_expl_expr = f_expl
model.x = x
model.xdot = xdot
model.u = u
# model.z = z
model.p = p
model.name = model_name
return model
def __init__(self,):
g_ = 9.8066
# control input
roll_rate_ref_ = ca.SX.sym('roll_rate_ref')
pitch_rate_ref_ = ca.SX.sym('pitch_rate_ref')
yaw_rate_ref_ = ca.SX.sym('yaw_rate_ref')
thrust_ref_ = ca.SX.sym('thrust_ref')
controls = ca.vcat([roll_rate_ref_, pitch_rate_ref_, yaw_rate_ref_, thrust_ref_])
# model state
x_ = ca.SX.sym('x')
y_ = ca.SX.sym('y')
z_ = ca.SX.sym('z')
vx_ = ca.SX.sym('vx')
vy_ = ca.SX.sym('vy')
vz_ = ca.SX.sym('vz')
qw_ = ca.SX.sym('qw')
qx_ = ca.SX.sym('qx')
qy_ = ca.SX.sym('qy')
qz_ = ca.SX.sym('qz')
# states [p, q, v]
states = ca.vcat([x_, y_, z_, qw_, qx_, qy_, qz_, vx_, vy_, vz_])
rhs = [
vx_,
vy_,
vz_,
0.5*(-roll_rate_ref_*qx_- pitch_rate_ref_*qy_ - yaw_rate_ref_*qz_),
0.5*(roll_rate_ref_*qw_ + yaw_rate_ref_*qy_ - pitch_rate_ref_*qz_),
0.5*(pitch_rate_ref_*qw_ - yaw_rate_ref_*qx_ + roll_rate_ref_*qz_),
0.5*(yaw_rate_ref_*qw_ + pitch_rate_ref_*qx_ - roll_rate_ref_*qy_),
2*(qw_*qy_ + qx_*qz_) * thrust_ref_,
2*(qy_*qz_ - qw_*qx_) * thrust_ref_,
(qw_*qw_ - qx_*qx_ - qy_*qy_ + qz_*qz_) * thrust_ref_ - g_,
]
self.f = ca.Function('f', [states, controls], [ca.vcat(rhs)])
x_dot = ca.SX.sym('x_dot', len(rhs))
f_impl = x_dot - self.f(states, controls)
model = AcadosModel()
model.f_expl_expr = self.f(states, controls)
model.f_impl_expr = f_impl
model.x = states
model.xdot = x_dot
model.u = controls
model.p = []
model.name = 'quadrotor_q'
constraints = ca.types.SimpleNamespace()
constraints.roll_rate_min = -6.0
constraints.roll_rate_max = 6.0
constraints.pitch_rate_min = -6.0
constraints.pitch_rate_max = 6.0
constraints.yaw_rate_min = -3.14
constraints.yaw_rate_max = 3.14
constraints.thrust_min = 2.0
constraints.thrust_max = g_*1.5
self.model = model
self.constraints = constraints
def acados_settings(Tf, N, track_file):
# create render arguments
ocp = AcadosOcp()
# export model
model, constraint = bicycle_model(track_file)
# define acados ODE
model_ac = AcadosModel()
model_ac.f_impl_expr = model.f_impl_expr
model_ac.f_expl_expr = model.f_expl_expr
model_ac.x = model.x
model_ac.xdot = model.xdot
model_ac.u = model.u
model_ac.z = model.z
model_ac.p = model.p
model_ac.name = model.name
ocp.model = model_ac
# define constraint
model_ac.con_h_expr = constraint.expr
# set dimensions
nx = model.x.size()[0]
nu = model.u.size()[0]
ny = nx + nu
ny_e = nx
ocp.dims.N = N
ns = 2
nsh = 2
# set cost
Q = np.diag([1e-1, 1e-8, 1e-8, 1e-8, 1e-3, 5e-3])
R = np.eye(nu)
R[0, 0] = 1e-3
R[1, 1] = 5e-3
Qe = np.diag([5e0, 1e1, 1e-8, 1e-8, 5e-3, 2e-3])
ocp.cost.cost_type = "LINEAR_LS"
ocp.cost.cost_type_e = "LINEAR_LS"
unscale = N / Tf
ocp.cost.W = unscale * scipy.linalg.block_diag(Q, R)
ocp.cost.W_e = Qe / unscale
Vx = np.zeros((ny, nx))
Vx[:nx, :nx] = np.eye(nx)
ocp.cost.Vx = Vx
Vu = np.zeros((ny, nu))
Vu[6, 0] = 1.0
Vu[7, 1] = 1.0
ocp.cost.Vu = Vu
Vx_e = np.zeros((ny_e, nx))
Vx_e[:nx, :nx] = np.eye(nx)
ocp.cost.Vx_e = Vx_e
ocp.cost.zl = 100 * np.ones((ns, ))
ocp.cost.Zl = 0 * np.ones((ns, ))
ocp.cost.zu = 100 * np.ones((ns, ))
ocp.cost.Zu = 0 * np.ones((ns, ))
# set intial references
ocp.cost.yref = np.array([1, 0, 0, 0, 0, 0, 0, 0])
ocp.cost.yref_e = np.array([0, 0, 0, 0, 0, 0])
# setting constraints
ocp.constraints.lbx = np.array([-12])
ocp.constraints.ubx = np.array([12])
ocp.constraints.idxbx = np.array([1])
ocp.constraints.lbu = np.array([model.dthrottle_min, model.ddelta_min])
ocp.constraints.ubu = np.array([model.dthrottle_max, model.ddelta_max])
ocp.constraints.idxbu = np.array([0, 1])
# ocp.constraints.lsbx=np.zero s([1])
# ocp.constraints.usbx=np.zeros([1])
# ocp.constraints.idxsbx=np.array([1])
ocp.constraints.lh = np.array([
constraint.along_min,
constraint.alat_min,
model.n_min,
model.throttle_min,
model.delta_min,
])
ocp.constraints.uh = np.array([
constraint.along_max,
constraint.alat_max,
model.n_max,
model.throttle_max,
model.delta_max,
])
ocp.constraints.lsh = np.zeros(nsh)
ocp.constraints.ush = np.zeros(nsh)
ocp.constraints.idxsh = np.array([0, 2])
# set intial condition
ocp.constraints.x0 = model.x0
# set QP solver and integration
ocp.solver_options.tf = Tf
# ocp.solver_options.qp_solver = 'FULL_CONDENSING_QPOASES'
ocp.solver_options.qp_solver = "PARTIAL_CONDENSING_HPIPM"
ocp.solver_options.nlp_solver_type = "SQP_RTI"
ocp.solver_options.hessian_approx = "GAUSS_NEWTON"
ocp.solver_options.integrator_type = "ERK"
ocp.solver_options.sim_method_num_stages = 4
ocp.solver_options.sim_method_num_steps = 3
# ocp.solver_options.qp_solver_tol_stat = 1e-2
# ocp.solver_options.qp_solver_tol_eq = 1e-2
# ocp.solver_options.qp_solver_tol_ineq = 1e-2
# ocp.solver_options.qp_solver_tol_comp = 1e-2
# create solver
acados_solver = AcadosOcpSolver(ocp, json_file="acados_ocp.json")
return constraint, model, acados_solver
def export_car_ode_model(mb, mw, Iw, Rw):
model_name = 'car_ode'
#system parameters
mt = mb + mw
g = 9.81 # [m/s^2]
# set up states & controls
phi = MX.sym('phi')
l = MX.sym('l')
theta = MX.sym('theta')
phi_dot = MX.sym('phi_dot')
l_dot = MX.sym('l_dot')
theta_dot = MX.sym('theta_dot')
x = vertcat(phi, l, theta,
phi_dot, l_dot, theta_dot)
# controls
tau = MX.sym('tau')
f = MX.sym('f')
u = vertcat(tau, f)
# xdot
phi_ddot = MX.sym('phi_ddot')
l_ddot = MX.sym('l_ddot')
theta_ddot = MX.sym('theta_ddot')
xdot = vertcat(phi_dot, l_dot, theta_dot,
phi_ddot, l_ddot, theta_ddot)
# inertia matrix
M = MX(3, 3)
M[0,0]=Iw + (Rw**2)*mb + (Rw**2)*mw
M[0,1]=Rw*mb*sin(theta)
M[0,2]=Rw*l*mb*cos(theta)
M[1,0]=Rw*mb*sin(theta)
M[1,1]=mb
M[1,2]=0
M[2,0]=Rw*l*mb*cos(theta)
M[2,1]=0
M[2,2]=(l**2)*mb
# coriolis and centrifugal terms
C = MX(3,1)
C[0, 0]=Rw*mb*theta_dot*(2*l_dot*cos(theta) - l*theta_dot*sin(theta))
C[1, 0]=-l*mb*(theta_dot**2)
C[2, 0]=2*l*l_dot*mb*theta_dot
# gravity term
G = MX(3,1)
G[0, 0] = 0
G[1, 0] = g*mb*cos(theta)
G[2, 0] = -g*l*mb*sin(theta)
# define S
S = MX(2,3)
S[0, 0] = 1
S[0, 1] = 0
S[0, 2] = -1
S[1, 0] = 0
S[1, 1] = 1
S[1, 2] = 0
p = vertcat([])
# compute acceleration
ddq = mtimes(inv(M), (mtimes(transpose(S),vertcat(tau, f)) - C - G))
print(ddq.shape)
# dynamics
f_expl = vertcat(phi_dot,
l_dot,
theta_dot,
ddq
)
print(f_expl.shape)
f_impl = xdot - f_expl
model = AcadosModel()
model.f_impl_expr = f_impl
model.f_expl_expr = f_expl
model.x = x
model.xdot = xdot
model.u = u
# algebraic variables
model.z = vertcat([])
# parameters
model.p = p
model.name = model_name
#model.con_h_expr = vertcat( (lambda_z - fabs(lambda_x)),
# (x1_dot-Rw*phi_dot))
#print(f"SHAPE: {model.con_h_expr}")
return model
def acados_nonlinear_quad_model(num_states, g=9.81):
model_name = 'nonlinear_quad'
position = SX.sym('o', 3)
velocity = SX.sym('v', 3)
phi = SX.sym('phi')
theta = SX.sym('theta')
psi = SX.sym('psi')
gravity = SX.zeros(3)
gravity[2] = g
x = vertcat(position, phi, theta, psi, velocity)
p = SX.sym('p')
q = SX.sym('q')
r = SX.sym('r')
F = SX.sym('F')
u = vertcat(p, q, r, F)
r1 = SX(3, 3)
r1[0, 0] = 1
r1[1, 1] = cos(phi)
r1[1, 2] = -sin(phi)
r1[2, 1] = sin(phi)
r1[2, 2] = cos(phi)
r2 = SX(3, 3)
r2[0, 0] = cos(theta)
r2[0, 2] = sin(theta)
r2[1, 1] = 1
r2[2, 0] = -sin(theta)
r2[2, 2] = cos(theta)
r3 = SX(3, 3)
r3[0, 0] = cos(psi)
r3[0, 1] = -sin(psi)
r3[1, 0] = sin(psi)
r3[1, 1] = cos(psi)
r3[2, 2] = 1
tau = SX(3, 3)
tau[:, 0] = (inv(r2))[:, 0]
tau[:, 1] = (inv(r2))[:, 1]
tau[:, 2] = (inv(r2 @ r1))[:, 2]
# tau[:,0] = (inv(r1))[:,0]
# tau[:,1] = (inv(r1))[:,1]
# tau[:,2] = (inv(r1 @ r2))[:,2]
# tau[0,0] = cos(theta)
# tau[0,2] = -sin(theta)
# tau[1,1] = 1
# tau[1,2] = sin(phi) * cos(theta)
# tau[2,0] = sin(theta)
# tau[2,2] = cos(phi) * cos(theta)
tc = 0.59375 # mapping control to throttle
# f_const = 100
f_const = 20
# tc = 0.75
#
# scaling_control_mat = np.array([1, 1, 1, (g/tc)])
# f_expl = vertcat(
# velocity,
# inv(tau) @ u[0:3],
# - ((r3 @ r1 @ r2)[:,2]) * (u[3] * ((g)/tc)) + gravity
# )
f_expl = vertcat(
velocity,
inv(tau) @ u[0:3], -((r3 @ r1 @ r2)[:, 2]) *
(u[3] * ((g + f_const) / tc) - f_const) + gravity)
# f_expl = vertcat(
# velocity,
# inv(tau) @ u[0:3],
# - ((r3 @ r1 @ r2)[:,2]) * ((g - f_const)*(1 - tc) / (1 - u[3]) + f_const) + gravity
# )
# xdot
xdot = SX.sym('xdot', 9)
#parameters
p = []
f_impl = xdot - f_expl
model = AcadosModel()
model.f_impl_expr = f_impl
model.f_expl_expr = f_expl
model.x = x
model.xdot = xdot
model.u = u
# model.z = z
model.p = p
model.name = model_name
return model
def generate(self, dae=None, quad=None, name='tunempc', opts={}):
""" Create embeddable NLP solver
"""
from acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver, AcadosSimSolver
# extract dimensions
nx = self.__nx
nu = self.__nu + self.__ns # treat slacks as pseudo-controls
# extract reference
ref = self.__ref
xref = np.squeeze(self.__ref[0][:nx], axis=1)
uref = np.squeeze(self.__ref[0][nx:nx + nu], axis=1)
# sampling time
self.__ts = opts['tf'] / self.__N
# create acados model
model = AcadosModel()
model.x = ca.MX.sym('x', nx)
model.u = ca.MX.sym('u', nu)
model.p = []
model.name = name
# detect input type
if dae is None:
model.f_expl_expr = self.__F(x0=model.x,
p=model.u)['xf'] / self.__ts
opts['integrator_type'] = 'ERK'
opts['sim_method_num_stages'] = 1
opts['sim_method_num_steps'] = 1
else:
n_in = dae.n_in()
if n_in == 2:
# xdot = f(x, u)
if 'integrator_type' in opts:
if opts['integrator_type'] in ['IRK', 'GNSF']:
xdot = ca.MX.sym('xdot', nx)
model.xdot = xdot
model.f_impl_expr = xdot - dae(model.x,
model.u[:self.__nu])
model.f_expl_expr = xdot
elif opts['integrator_type'] == 'ERK':
model.f_expl_expr = dae(model.x, model.u[:self.__nu])
else:
raise ValueError('Provide numerical integrator type!')
else:
xdot = ca.MX.sym('xdot', nx)
model.xdot = xdot
model.f_expl_expr = xdot
if n_in == 3:
# f(xdot, x, u) = 0
model.f_impl_expr = dae(xdot, model.x, model.u[:self.__nu])
elif n_in == 4:
# f(xdot, x, u, z) = 0
nz = dae.size1_in(3)
z = ca.MX.sym('z', nz)
model.z = z
model.f_impl_expr = dae(xdot, model.x, model.u[:self.__nu],
z)
else:
raise ValueError(
'Invalid number of inputs for system dynamics function.'
)
if self.__gnl is not None:
model.con_h_expr = self.__gnl(model.x, model.u[:self.__nu],
model.u[self.__nu:])
if self.__type == 'economic':
if quad is None:
model.cost_expr_ext_cost = self.__cost(
model.x, model.u[:self.__nu]) / self.__ts
else:
model.cost_expr_ext_cost = self.__cost(model.x,
model.u[:self.__nu])
# create acados ocp
ocp = AcadosOcp()
ocp.model = model
ny = nx + nu
ny_e = nx
if 'integrator_type' in opts and opts['integrator_type'] == 'GNSF':
from acados_template import acados_dae_model_json_dump
import os
acados_dae_model_json_dump(model)
# Set up Octave to be able to run the following:
## if using a virtual python env, the following lines can be added to the env/bin/activate script:
# export OCTAVE_PATH=$OCTAVE_PATH:$ACADOS_INSTALL_DIR/external/casadi-octave
# export OCTAVE_PATH=$OCTAVE_PATH:$ACADOS_INSTALL_DIR/interfaces/acados_matlab_octave/
# export OCTAVE_PATH=$OCTAVE_PATH:$ACADOS_INSTALL_DIR/interfaces/acados_matlab_octave/acados_template_mex/
# echo
# echo "OCTAVE_PATH=$OCTAVE_PATH"
status = os.system(
"octave --eval \"convert({})\"".format("\'" + model.name +
"_acados_dae.json\'"))
# load gnsf from json
with open(model.name + '_gnsf_functions.json', 'r') as f:
import json
gnsf_dict = json.load(f)
ocp.gnsf_model = gnsf_dict
# set horizon length
ocp.dims.N = self.__N
# set cost module
if self.__type == 'economic':
# set cost function type to external (provided in model)
ocp.cost.cost_type = 'EXTERNAL'
if quad is not None:
ocp.solver_options.cost_discretization = 'INTEGRATOR'
elif self.__type == 'tracking':
# set weighting matrices
ocp.cost.W = self.__Href[0][0]
# set-up linear least squares cost
ocp.cost.cost_type = 'LINEAR_LS'
ocp.cost.W_e = np.zeros((nx, nx))
ocp.cost.Vx = np.zeros((ny, nx))
ocp.cost.Vx[:nx, :nx] = np.eye(nx)
Vu = np.zeros((ny, nu))
Vu[nx:, :] = np.eye(nu)
ocp.cost.Vu = Vu
ocp.cost.Vx_e = np.eye(nx)
ocp.cost.yref = np.squeeze(
ca.vertcat(xref,uref).full() - \
ct.mtimes(np.linalg.inv(ocp.cost.W),self.__qref[0][0].T).full(), # gradient term
axis = 1
)
ocp.cost.yref_e = np.zeros((ny_e, ))
if n_in == 4: # DAE flag
ocp.cost.Vz = np.zeros((ny, nz))
# if 'custom_hessian' in opts:
# self.__custom_hessian = opts['custom_hessian']
# initial condition
ocp.constraints.x0 = xref
# set inequality constraints
ocp.constraints.constr_type = 'BGH'
if self.__S['C'] is not None:
C = self.__S['C'][0][:, :nx]
D = self.__S['C'][0][:, nx:]
lg = -self.__S['e'][0] + ct.mtimes(C, xref).full() + ct.mtimes(
D, uref).full()
ug = 1e8 - self.__S['e'][0] + ct.mtimes(
C, xref).full() + ct.mtimes(D, uref).full()
ocp.constraints.lg = np.squeeze(lg, axis=1)
ocp.constraints.ug = np.squeeze(ug, axis=1)
ocp.constraints.C = C
ocp.constraints.D = D
if 'usc' in self.__vars:
if 'us' in self.__vars:
arg = [
self.__vars['x'], self.__vars['u'], self.__vars['us'],
self.__vars['usc']
]
else:
arg = [
self.__vars['x'], self.__vars['u'], self.__vars['usc']
]
Jsg = ca.Function(
'Jsg', [self.__vars['usc']],
[ca.jacobian(self.__h(*arg), self.__vars['usc'])])(0.0)
self.__Jsg = Jsg.full()[:-self.__nsc, :]
ocp.constraints.Jsg = self.__Jsg
ocp.cost.Zl = np.zeros((self.__nsc, ))
ocp.cost.Zu = np.zeros((self.__nsc, ))
ocp.cost.zl = np.squeeze(self.__scost.full(),
axis=1) / self.__ts
ocp.cost.zu = np.squeeze(self.__scost.full(),
axis=1) / self.__ts
# set nonlinear equality constraints
if self.__gnl is not None:
ocp.constraints.lh = np.zeros(self.__ns, )
ocp.constraints.uh = np.zeros(self.__ns, )
# terminal constraint:
x_term = self.__p_operator(model.x)
Jbx = ca.Function('Jbx', [model.x],
[ca.jacobian(x_term, model.x)])(0.0)
ocp.constraints.Jbx_e = Jbx.full()
ocp.constraints.lbx_e = np.squeeze(self.__p_operator(xref).full(),
axis=1)
ocp.constraints.ubx_e = np.squeeze(self.__p_operator(xref).full(),
axis=1)
for option in list(opts.keys()):
if hasattr(ocp.solver_options, option):
setattr(ocp.solver_options, option, opts[option])
self.__acados_ocp_solver = AcadosOcpSolver(ocp,
json_file='acados_ocp_' +
model.name + '.json')
self.__acados_integrator = AcadosSimSolver(ocp,
json_file='acados_ocp_' +
model.name + '.json')
# set initial guess
self.__set_acados_initial_guess()
return self.__acados_ocp_solver, self.__acados_integrator