Hi acados team!
I am working on a nonlinear MPC for a quadrotor with collision avoidance constraints. I am using the Python interface to define the nonlinear quadrotor dynamics and control barrier function (CBF) constraint. For now, I have two static obstacles to avoid.
My CBF constraint h(x,u) is nonlinear and depends on the state and control inputs (or xdot variables). When I export the solver, casadi throws the following error, saying that xdot is a free variable:
Initialization failed since variables [xdot] are free. These symbols occur in the output expressions but you forgot to declare these as inputs. Set option 'allow_free' to allow free variables.
Here is my model:
def exportModel(mpc_params):
mass = mpc_params.mass
model_name = mpc_params.mav_name
nx = mpc_params.nx
ixx = mpc_params.ixx
iyy = mpc_params.iyy
izz = mpc_params.izz
kf = mpc_params.kf
km = mpc_params.km
# set up states
x1 = MX.sym('x1') # xpos
x2 = MX.sym('x2') # ypos
x3 = MX.sym('x3') # zpos
x4 = MX.sym('x4') # xVel
x5 = MX.sym('x5') # yVel
x6 = MX.sym('x6') # zVel
x7 = MX.sym('x7') # Qw
x8 = MX.sym('x8') # Qx
x9 = MX.sym('x9') # Qy
x10 = MX.sym('x10') # Qz
x11 = MX.sym('x11') # omegax
x12 = MX.sym('x12') # omegay
x13 = MX.sym('x13') # omegaz
x = vertcat(x1, x2, x3, x4, x5, x6, x7, x8, x9, x10, x11, x12, x13)
xdot = MX.sym('xdot', nx, 1)
# set up controls
u1 = MX.sym('u1') # motor thrust_0
u2 = MX.sym('u2') # motor thrust_1
u3 = MX.sym('u3') # motor thrust_2
u4 = MX.sym('u4') # motor thrust_3
u = vertcat(u1, u2, u3, u4)
km_kf = km / kf
moment_x = mpc_params.arm_length * (-u1 + u2 + u3 - u4)
moment_y = mpc_params.arm_length * (-u1 + u2 - u3 + u4)
f_expl_rpm = vertcat(x4, x5, x6, \
(1/mass)*(u1+u2+u3+u4)*(2*x7*x9 + 2*x8*x10)/(x7**2 + x8**2 + x9**2 + x10**2),
-(1/mass)*(u1+u2+u3+u4)*(2*x7*x8 - 2*x9*x10)/(x7**2 + x8**2 + x9**2 + x10**2),
-(1/mass)*(u1+u2+u3+u4)*(2*x8*x8 + 2*x9*x9 - 1)/(x7**2 + x8**2 + x9**2 + x10**2) - 9.81,
-(x11*x8)/2 - (x12*x9)/2 - (x13*x10)/2,
(x11*x7)/2 + (x13*x9)/2 - (x12*x10)/2,
(x12*x7)/2 - (x13*x8)/2 + (x11*x10)/2,
(x13*x7)/2 + (x12*x8)/2 - (x11*x9)/2,
(moment_x + iyy*x12*x13 - izz*x12*x13)/ixx,
(moment_y - ixx*x11*x13 + izz*x11*x13)/iyy,
(km_kf*(-u1-u2+u3+u4) + ixx*x11*x12 - iyy*x11*x12)/izz)
f_expl = f_expl_rpm
# parameters
ext_param = MX.sym('references', nx + 13, 1) # ext_param = MX.sym('references', nx + 4, 1)
p = ext_param
z = []
model = AcadosModel()
model.f_impl_expr = xdot - f_expl
model.f_expl_expr = f_expl
model.x = x
model.u = u
model.xdot = xdot
model.z = z
model.p = p
model.name = "quadrotor"
return model
And here is my solver:
def solver(mpc_params):
N = mpc_params.N
Tf = mpc_params.Tf
Ts = Tf / N
mass = mpc_params.mass
ocp = AcadosOcp()
model = export_model.exportModel(mpc_params)
ocp.model = model
nx = model.x.size()[0]
nu = model.u.size()[0]
ocp.dims.N = N
ocp.dims.nx = nx
ocp.dims.nbx = nx
ocp.dims.nbu = nu
ocp.dims.nbx_e = nx
ocp.dims.nu = model.u.size()[0]
ocp.dims.np = model.p.size()[0]
ocp.dims.nbxe_0 = nx
x = ocp.model.x
xdot = ocp.model.xdot
u = ocp.model.u
param = ocp.model.p
ocp.cost.cost_type = 'EXTERNAL'
ocp.cost.cost_type_e = 'EXTERNAL'
Q = mpc_params.Q
Qt = mpc_params.Qt
R = np.array(mpc_params.R)
Q_quat = np.eye(3)
Q_quat[0, 0] = Q[6]
Q_quat[1, 1] = Q[7]
Q_quat[2, 2] = Q[8]
Q_quat_e = np.eye(3)
Q_quat_e[0, 0] = Qt[6]
Q_quat_e[1, 1] = Qt[7]
Q_quat_e[2, 2] = Qt[8]
quat = vertcat(x[6], x[7], x[8], x[9])
params_0 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -10.0, 0.0, 0.0, -10.0, 0.0, 0.0, 0.0]
pos_vel = vertcat(x[0], x[1], x[2], x[3], x[4], x[5], x[10], x[11], x[12])
Q_pos_vel = np.array(Q[0:6] + Q[10:13])
states = pos_vel
state_costs = Q_pos_vel
state_costs_terminal = Qt
state_references = vertcat(param[0:6], param[10:13])
input_references = vertcat(param[13:17])
obstacle1_pos = vertcat(param[17:20])
obstacle2_pos = vertcat(param[20:23])
init_pos = vertcat(param[23:])
bx = np.array([40, 40, 20, 10, 10, 10, 5, 5, 5, 5, 3, 3, 3])
ubu = np.array(mpc_params.ubu)
lbu = np.array(mpc_params.lbu)
ocp.parameter_values = np.array(params_0)
ocp.model.cost_expr_ext_cost_0 = utils.quatError(param[6:10], quat, Q_quat) + \
utils.calcSquareCost(state_references, states, state_costs) + \
utils.calcSquareCost(input_references, u, R)
ocp.model.cost_expr_ext_cost_e = utils.quatError(param[6:10], quat, Q_quat_e) + \
utils.calcSquareCost(state_references, states, state_costs_terminal)
ocp.model.cost_expr_ext_cost = utils.quatError(param[6:10], quat, Q_quat) + \
utils.calcSquareCost(state_references, states, state_costs) + \
utils.calcSquareCost(input_references, u, R)
ocp.constraints.ubx = bx
ocp.constraints.lbx = -bx
ocp.constraints.ubx_0 = bx
ocp.constraints.lbx_0 = -bx
ocp.constraints.ubx_e = bx
ocp.constraints.lbx_e = -bx
ocp.constraints.ubu = ubu
ocp.constraints.lbu = lbu
ocp.constraints.idxbx = np.array([i for i in range(nx)])
ocp.constraints.idxbx_0 = np.array([i for i in range(nx)])
ocp.constraints.idxbx_e = np.array([i for i in range(nx)])
ocp.constraints.idxbu = np.array([0, 1, 2, 3])
ocp.constraints.x0 = np.array(params_0[0:nx])
## Collision avoidance constraint
### DISTANCE CBF CONSTRAINT
dsafe = 0.5
k1 = 0.5
k0 = 0.5
ocp.constraints.lh = np.array([0.0])
ocp.constraints.uh = np.array([300.0])
dx = states[0] - obstacle1_pos[0]
dy = states[1] - obstacle1_pos[1]
dz = states[2] - obstacle1_pos[2]
h = dx**2+dy**2+dz**2-dsafe**2
hdot = 2*dx*states[3]+2*dy*states[4]+2*dz*states[5]
hdotdot = 2*(states[3]*xdot[0]+states[4]*xdot[1]+states[5]*xdot[2])
ocp.model.con_h_expr = vertcat(hdotdot+k1*hdot+k0*h)
### TERMINAL CONSTRAINTS
ocp.constraints.uh_e = ocp.constraints.uh
ocp.constraints.lh_e = ocp.constraints.lh
ocp.model.con_h_expr_e = ocp.model.con_h_expr
ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # "PARTIAL_CONDENSING_HPIPM", "FULL_CONDENSING_HPIPM"
ocp.solver_options.nlp_solver_type = 'SQP_RTI' # "SQP", "SQP_RTI"
ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' # "GAUSS_NEWTON", "EXACT"
ocp.solver_options.integrator_type = 'IRK' # "ERK", "IRK", "GNSF"
ocp.solver_options.regularize_method = 'CONVEXIFY' # ‘NO_REGULARIZE’, ‘MIRROR’, ‘PROJECT’, ‘PROJECT_REDUC_HESS’, ‘CONVEXIFY’
ocp.solver_options.tf = Tf
ocp.solver_options.levenberg_marquardt = 10.0
ocp.solver_options.nlp_solver_tol_stat = 1e-7
acados_solver = AcadosOcpSolver(ocp, json_file='acados_ocp_mpc.json')
print('MPC built')
return acados_solver, ocp
I am not sure I understand what a free variable means in my case, because xdot is directly defined in model.py. I also tried expressing my constraint using directly the control inputs ocp.model.u instead of xdot but I obtain the same error.
According to acados problem formulation using this kind of constraints should be possible. Also, in one of your acados python example, test_parametric_nonlinear_constraint_h.py, a nonlinear constraint is directlly defined using ocp.model.u. I tested this example on my computer and it builds correctly. I do not understand what I do differently.
Would you have any idea what I am doing wrong? Or how to allow free variables in casadi as suggested in the error?
Thank you very much in advance for your help!
Best,
mgo12
