Vx_e = np.zeros((ny_e, nx))
Vx_e[:nx, :nx] = np.eye(nx)
ocp.cost.Vx_e = Vx_e
# set intial references
#target_position = np.array([0., 0.8, 0, 0, 0, 0]) # target standing position
target_position = np.array([-2. / Rw, 0.8, 0, 0, 0, 0, 0,
mb * g]) # target standing position
yref = target_position
yref_e = target_position[:6]
ocp.cost.yref = yref
ocp.cost.yref_e = yref_e
# parameters
ocp.parameter_values = np.array([])
# setting constraints
# setting constraints
lower_X = np.array([2 * Rw, -np.pi / 2])
upper_X = np.array([1 + 2 * Rw, np.pi / 2])
ocp.constraints.lbx = lower_X
ocp.constraints.ubx = upper_X
ocp.constraints.idxbx = np.array([1, 2])
lower_U = np.array([-10, -200])
upper_U = np.array([10, 200])
ocp.constraints.lbu = lower_U
ocp.constraints.ubu = upper_U
ocp.cost.yref_e = nmp.zeros((10, ))
ocp.cost.yref_e[3] = 1
# init conditions, start at yaw of +90deg
x0 = nmp.array([0.285, -0.959, 0.0, 0.0, 0.0, 0.0, 0.0])
# set constraints
uDel = 8
uSS = 39.99
ocp.constraints.constr_type = 'BGH'
ocp.constraints.lbu = nmp.array(
[uSS - uDel, uSS - uDel, uSS - uDel, uSS - uDel])
ocp.constraints.ubu = nmp.array(
[uSS + uDel, uSS + uDel, uSS + uDel, uSS + uDel])
ocp.constraints.x0 = x0
ocp.parameter_values = p
ocp.constraints.idxbu = nmp.array([0, 1, 2, 3])
if USE_QUAT_SLACK == 1:
# nonlinear quaternion constraint
ocp.constraints.lh = nmp.array([1.0])
ocp.constraints.uh = nmp.array([1.0])
# #slack for nonlinear quat
ocp.constraints.lsh = nmp.array([-0.2])
ocp.constraints.ush = nmp.array([0.2])
ocp.constraints.idxsh = nmp.array([0])
ocp.solver_options.qp_solver = 'FULL_CONDENSING_HPIPM' #'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES
ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
ocp.solver_options.integrator_type = 'IRK'
ocp.solver_options.nlp_solver_type = 'SQP_RTI' # SQP_RTI
def quadrotor_optimizer_setup(self, ):
Q_m_ = np.diag([
10,
10,
10,
0.3,
0.3,
0.3,
0.3,
0.05,
0.05,
0.05,
]) # position, q, v
P_m_ = np.diag([10, 10, 10, 0.05, 0.05, 0.05]) # only p and v
R_m_ = np.diag([5.0, 5.0, 5.0, 0.6])
nx = self.model.x.size()[0]
self.nx = nx
nu = self.model.u.size()[0]
self.nu = nu
ny = nx + nu
n_params = self.model.p.size()[0] if isinstance(self.model.p,
ca.SX) else 0
acados_source_path = os.environ['ACADOS_SOURCE_DIR']
sys.path.insert(0, acados_source_path)
# create OCP
ocp = AcadosOcp()
ocp.acados_include_path = acados_source_path + '/include'
ocp.acados_lib_path = acados_source_path + '/lib'
ocp.model = self.model
ocp.dims.N = self.N
ocp.solver_options.tf = self.T
# initialize parameters
ocp.dims.np = n_params
ocp.parameter_values = np.zeros(n_params)
# cost type
ocp.cost.cost_type = 'LINEAR_LS'
ocp.cost.cost_type_e = 'LINEAR_LS'
ocp.cost.W = scipy.linalg.block_diag(Q_m_, R_m_)
ocp.cost.W_e = P_m_
ocp.cost.Vx = np.zeros((ny, nx))
ocp.cost.Vx[:nx, :nx] = np.eye(nx)
ocp.cost.Vu = np.zeros((ny, nu))
ocp.cost.Vu[-nu:, -nu:] = np.eye(nu)
ocp.cost.Vx_e = np.zeros((nx - 4, nx))
# ocp.cost.Vx_e[:6, :6] = np.eye(6)
ocp.cost.Vx_e[:3, :3] = np.eye(3)
ocp.cost.Vx_e[-3:, -3:] = np.eye(3)
# initial reference trajectory_ref
x_ref = np.zeros(nx)
x_ref[3] = 1.0
x_ref_e = np.zeros(nx - 4)
u_ref = np.zeros(nu)
u_ref[-1] = self.g_
ocp.cost.yref = np.concatenate((x_ref, u_ref))
ocp.cost.yref_e = x_ref_e
# Set constraints
ocp.constraints.lbu = np.array([
self.constraints.roll_rate_min, self.constraints.pitch_rate_min,
self.constraints.yaw_rate_min, self.constraints.thrust_min
])
ocp.constraints.ubu = np.array([
self.constraints.roll_rate_max, self.constraints.pitch_rate_max,
self.constraints.yaw_rate_max, self.constraints.thrust_max
])
ocp.constraints.idxbu = np.array([0, 1, 2, 3])
# initial state
ocp.constraints.x0 = x_ref
# solver options
ocp.solver_options.qp_solver = 'FULL_CONDENSING_HPIPM'
ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
# explicit Runge-Kutta integrator
ocp.solver_options.integrator_type = 'ERK'
ocp.solver_options.print_level = 0
ocp.solver_options.nlp_solver_type = 'SQP' # 'SQP_RTI'
ocp.solver_options.nlp_solver_max_iter = 400
# compile acados ocp
## files are stored in .ros/
json_file = os.path.join('./' + self.model.name + '_acados_ocp.json')
self.solver = AcadosOcpSolver(ocp, json_file=json_file)
if self.simulation_required:
self.integrator = AcadosSimSolver(ocp, json_file=json_file)
# set constraints
Fmax = 80
# use equivalent formulation with h constraint
# ocp.constraints.lbu = np.array([-Fmax])
# ocp.constraints.ubu = np.array([+Fmax])
# ocp.constraints.idxbu = np.array([0])
p = SX.sym('p')
ocp.model.p = p
ocp.constraints.lh = np.array([-Fmax])
ocp.constraints.uh = np.array([+Fmax])
ocp.model.con_h_expr = model.u / p
ocp.parameter_values = np.array([0])
ocp.constraints.x0 = np.array([0.0, np.pi, 0.0, 0.0])
ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES
ocp.solver_options.hessian_approx = 'EXACT' # GAUSS_NEWTON, EXACT
ocp.solver_options.regularize_method = 'CONVEXIFY' # GAUSS_NEWTON, EXACT
ocp.solver_options.integrator_type = 'ERK'
ocp.solver_options.print_level = 0
ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI, SQP
# set prediction horizon
ocp.solver_options.tf = Tf
ocp_solver = AcadosOcpSolver(ocp, json_file='acados_ocp.json')
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 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 quadrotor_optimizer_setup(self, ):
# Q_m_ = np.diag([80.0, 80.0, 120.0, 20.0, 20.0,
# 30.0, 10.0, 10.0, 0.0]) # position, velocity, roll, pitch, (not yaw)
# P_m_ = np.diag([86.21, 86.21, 120.95,
# 6.94, 6.94, 11.04]) # only p and v
# P_m_[0, 3] = 6.45
# P_m_[3, 0] = 6.45
# P_m_[1, 4] = 6.45
# P_m_[4, 1] = 6.45
# P_m_[2, 5] = 10.95
# P_m_[5, 2] = 10.95
# R_m_ = np.diag([50.0, 60.0, 1.0])
Q_m_ = np.diag(
[
10.0,
10.0,
10.0,
3e-1,
3e-1,
3e-1,
#3e-1, 3e-1, 3e-2, 3e-2,
100.0,
100.0,
1e-3,
1e-3,
10.5,
10.5,
10.5
]
) # position, velocity, load_position, load_velocity, [roll, pitch, yaw]
P_m_ = np.diag([
10.0, 10.0, 10.0, 0.05, 0.05, 0.05
# 10.0, 10.0, 10.0,
# 0.05, 0.05, 0.05
]) # only p and v
# P_m_[0, 8] = 6.45
# P_m_[8, 0] = 6.45
# P_m_[1, 9] = 6.45
# P_m_[9, 1] = 6.45
# P_m_[2, 10] = 10.95
# P_m_[10, 2] = 10.95
R_m_ = np.diag([3.0, 3.0, 3.0, 1.0])
nx = self.model.x.size()[0]
self.nx = nx
nu = self.model.u.size()[0]
self.nu = nu
ny = nx + nu
n_params = self.model.p.size()[0] if isinstance(self.model.p,
ca.SX) else 0
acados_source_path = os.environ['ACADOS_SOURCE_DIR']
sys.path.insert(0, acados_source_path)
# create OCP
ocp = AcadosOcp()
ocp.acados_include_path = acados_source_path + '/include'
ocp.acados_lib_path = acados_source_path + '/lib'
ocp.model = self.model
ocp.dims.N = self.N
ocp.solver_options.tf = self.T
# initialize parameters
ocp.dims.np = n_params
ocp.parameter_values = np.zeros(n_params)
# cost type
ocp.cost.cost_type = 'LINEAR_LS'
ocp.cost.cost_type_e = 'LINEAR_LS'
ocp.cost.W = scipy.linalg.block_diag(Q_m_, R_m_)
ocp.cost.W_e = P_m_ # np.zeros((nx-3, nx-3))
ocp.cost.Vx = np.zeros((ny, nx))
ocp.cost.Vx[:nx, :nx] = np.eye(nx)
ocp.cost.Vu = np.zeros((ny, nu))
ocp.cost.Vu[-nu:, -nu:] = np.eye(nu)
ocp.cost.Vx_e = np.zeros((nx - 7, nx)) # only consider p and v
ocp.cost.Vx_e[:nx - 7, :nx - 7] = np.eye(nx - 7)
# initial reference trajectory_ref
x_ref = np.zeros(nx)
x_ref_e = np.zeros(nx - 7)
u_ref = np.zeros(nu)
u_ref[-1] = self.g
ocp.cost.yref = np.concatenate((x_ref, u_ref))
ocp.cost.yref_e = x_ref_e
# Set constraints
ocp.constraints.lbu = np.array([
self.constraints.roll_min, self.constraints.pitch_min,
self.constraints.yaw_min, self.constraints.thrust_min
])
ocp.constraints.ubu = np.array([
self.constraints.roll_max, self.constraints.pitch_max,
self.constraints.yaw_max, self.constraints.thrust_max
])
ocp.constraints.idxbu = np.array([0, 1, 2, 3])
# initial state
ocp.constraints.x0 = x_ref
# solver options
ocp.solver_options.qp_solver = 'FULL_CONDENSING_HPIPM'
ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
# explicit Runge-Kutta integrator
ocp.solver_options.integrator_type = 'ERK'
ocp.solver_options.print_level = 0
ocp.solver_options.nlp_solver_type = 'SQP' # 'SQP_RTI'
ocp.solver_options.levenberg_marquardt = 0.12 # 0.0
# compile acados ocp
json_file = os.path.join('./' + self.model.name + '_acados_ocp.json')
self.solver = AcadosOcpSolver(ocp, json_file=json_file)
if self.simulation_required:
self.integrator = AcadosSimSolver(ocp, json_file=json_file)
def run_nominal_control(chain_params):
# create ocp object to formulate the OCP
ocp = AcadosOcp()
# chain parameters
n_mass = chain_params["n_mass"]
M = chain_params["n_mass"] - 2 # number of intermediate masses
Ts = chain_params["Ts"]
Tsim = chain_params["Tsim"]
N = chain_params["N"]
u_init = chain_params["u_init"]
with_wall = chain_params["with_wall"]
yPosWall = chain_params["yPosWall"]
m = chain_params["m"]
D = chain_params["D"]
L = chain_params["L"]
perturb_scale = chain_params["perturb_scale"]
nlp_iter = chain_params["nlp_iter"]
nlp_tol = chain_params["nlp_tol"]
save_results = chain_params["save_results"]
show_plots = chain_params["show_plots"]
seed = chain_params["seed"]
np.random.seed(seed)
nparam = 3*M
W = perturb_scale * np.eye(nparam)
# export model
model = export_disturbed_chain_mass_model(n_mass, m, D, L)
# set model
ocp.model = model
nx = model.x.size()[0]
nu = model.u.size()[0]
ny = nx + nu
ny_e = nx
Tf = N * Ts
# initial state
xPosFirstMass = np.zeros((3,1))
xEndRef = np.zeros((3,1))
xEndRef[0] = L * (M+1) * 6
pos0_x = np.linspace(xPosFirstMass[0], xEndRef[0], n_mass)
xrest = compute_steady_state(n_mass, m, D, L, xPosFirstMass, xEndRef)
x0 = xrest
# set dimensions
ocp.dims.N = N
# set cost module
ocp.cost.cost_type = 'LINEAR_LS'
ocp.cost.cost_type_e = 'LINEAR_LS'
Q = 2*np.diagflat( np.ones((nx, 1)) )
q_diag = np.ones((nx,1))
strong_penalty = M+1
q_diag[3*M] = strong_penalty
q_diag[3*M+1] = strong_penalty
q_diag[3*M+2] = strong_penalty
Q = 2*np.diagflat( q_diag )
R = 2*np.diagflat( 1e-2 * np.ones((nu, 1)) )
ocp.cost.W = scipy.linalg.block_diag(Q, R)
ocp.cost.W_e = Q
ocp.cost.Vx = np.zeros((ny, nx))
ocp.cost.Vx[:nx,:nx] = np.eye(nx)
Vu = np.zeros((ny, nu))
Vu[nx:nx+nu, :] = np.eye(nu)
ocp.cost.Vu = Vu
ocp.cost.Vx_e = np.eye(nx)
# import pdb; pdb.set_trace()
yref = np.vstack((xrest, np.zeros((nu,1)))).flatten()
ocp.cost.yref = yref
ocp.cost.yref_e = xrest.flatten()
# set constraints
umax = 1*np.ones((nu,))
ocp.constraints.constr_type = 'BGH'
ocp.constraints.lbu = -umax
ocp.constraints.ubu = umax
ocp.constraints.x0 = x0.reshape((nx,))
ocp.constraints.idxbu = np.array(range(nu))
# disturbances
nparam = 3*M
ocp.parameter_values = np.zeros((nparam,))
# wall constraint
if with_wall:
nbx = M + 1
Jbx = np.zeros((nbx,nx))
for i in range(nbx):
Jbx[i, 3*i+1] = 1.0
ocp.constraints.Jbx = Jbx
ocp.constraints.lbx = yPosWall * np.ones((nbx,))
ocp.constraints.ubx = 1e9 * np.ones((nbx,))
# slacks
ocp.constraints.Jsbx = np.eye(nbx)
L2_pen = 1e3
L1_pen = 1
ocp.cost.Zl = L2_pen * np.ones((nbx,))
ocp.cost.Zu = L2_pen * np.ones((nbx,))
ocp.cost.zl = L1_pen * np.ones((nbx,))
ocp.cost.zu = L1_pen * np.ones((nbx,))
# solver options
ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES
ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
ocp.solver_options.integrator_type = 'IRK'
ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI
ocp.solver_options.nlp_solver_max_iter = nlp_iter
ocp.solver_options.sim_method_num_stages = 2
ocp.solver_options.sim_method_num_steps = 2
ocp.solver_options.qp_solver_cond_N = N
ocp.solver_options.qp_tol = nlp_tol
ocp.solver_options.tol = nlp_tol
# ocp.solver_options.nlp_solver_tol_eq = 1e-9
# set prediction horizon
ocp.solver_options.tf = Tf
acados_ocp_solver = AcadosOcpSolver(ocp, json_file = 'acados_ocp_' + model.name + '.json')
# acados_integrator = AcadosSimSolver(ocp, json_file = 'acados_ocp_' + model.name + '.json')
acados_integrator = export_chain_mass_integrator(n_mass, m, D, L)
#%% get initial state from xrest
xcurrent = x0.reshape((nx,))
for i in range(5):
acados_integrator.set("x", xcurrent)
acados_integrator.set("u", u_init)
status = acados_integrator.solve()
if status != 0:
raise Exception('acados integrator returned status {}. Exiting.'.format(status))
# update state
xcurrent = acados_integrator.get("x")
#%% actual simulation
N_sim = int(np.floor(Tsim/Ts))
simX = np.ndarray((N_sim+1, nx))
simU = np.ndarray((N_sim, nu))
wall_dist = np.zeros((N_sim,))
timings = np.zeros((N_sim,))
simX[0,:] = xcurrent
# closed loop
for i in range(N_sim):
# solve ocp
acados_ocp_solver.set(0, "lbx", xcurrent)
acados_ocp_solver.set(0, "ubx", xcurrent)
status = acados_ocp_solver.solve()
timings[i] = acados_ocp_solver.get_stats("time_tot")[0]
if status != 0:
raise Exception('acados acados_ocp_solver returned status {} in time step {}. Exiting.'.format(status, i))
simU[i,:] = acados_ocp_solver.get(0, "u")
print("control at time", i, ":", simU[i,:])
# simulate system
acados_integrator.set("x", xcurrent)
acados_integrator.set("u", simU[i,:])
pertubation = sampleFromEllipsoid(np.zeros((nparam,)), W)
acados_integrator.set("p", pertubation)
status = acados_integrator.solve()
if status != 0:
raise Exception('acados integrator returned status {}. Exiting.'.format(status))
# update state
xcurrent = acados_integrator.get("x")
simX[i+1,:] = xcurrent
# xOcpPredict = acados_ocp_solver.get(1, "x")
# print("model mismatch = ", str(np.max(xOcpPredict - xcurrent)))
yPos = xcurrent[range(1,3*M+1,3)]
wall_dist[i] = np.min(yPos - yPosWall)
print("time i = ", str(i), " dist2wall ", str(wall_dist[i]))
print("dist2wall (minimum over simulation) ", str(np.min(wall_dist)))
#%% plot results
if os.environ.get('ACADOS_ON_TRAVIS') is None and show_plots:
plot_chain_control_traj(simU)
plot_chain_position_traj(simX, yPosWall=yPosWall)
plot_chain_velocity_traj(simX)
animate_chain_position(simX, xPosFirstMass, yPosWall=yPosWall)
# animate_chain_position_3D(simX, xPosFirstMass)
plt.show()
def __init__(self, quad, t_horizon=1, n_nodes=20,
q_cost=None, r_cost=None, q_mask=None,
B_x=None, gp_regressors=None, rdrv_d_mat=None,
model_name="quad_3d_acados_mpc", solver_options=None):
"""
:param quad: quadrotor object
:type quad: Quadrotor3D
:param t_horizon: time horizon for MPC optimization
:param n_nodes: number of optimization nodes until time horizon
:param q_cost: diagonal of Q matrix for LQR cost of MPC cost function. Must be a numpy array of length 12.
:param r_cost: diagonal of R matrix for LQR cost of MPC cost function. Must be a numpy array of length 4.
:param B_x: dictionary of matrices that maps the outputs of the gp regressors to the state space.
:param gp_regressors: Gaussian Process ensemble for correcting the nominal model
:type gp_regressors: GPEnsemble
:param q_mask: Optional boolean mask that determines which variables from the state compute towards the cost
function. In case no argument is passed, all variables are weighted.
:param solver_options: Optional set of extra options dictionary for solvers.
:param rdrv_d_mat: 3x3 matrix that corrects the drag with a linear model according to Faessler et al. 2018. None
if not used
"""
# Weighted squared error loss function q = (p_xyz, a_xyz, v_xyz, r_xyz), r = (u1, u2, u3, u4)
if q_cost is None:
q_cost = np.array([10, 10, 10, 0.1, 0.1, 0.1, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05])
if r_cost is None:
r_cost = np.array([0.1, 0.1, 0.1, 0.1])
self.T = t_horizon # Time horizon
self.N = n_nodes # number of control nodes within horizon
self.quad = quad
self.max_u = quad.max_input_value
self.min_u = quad.min_input_value
# Declare model variables
self.p = cs.MX.sym('p', 3) # position
self.q = cs.MX.sym('a', 4) # angle quaternion (wxyz)
self.v = cs.MX.sym('v', 3) # velocity
self.r = cs.MX.sym('r', 3) # angle rate
# Full state vector (13-dimensional)
self.x = cs.vertcat(self.p, self.q, self.v, self.r)
self.state_dim = 13
# Control input vector
u1 = cs.MX.sym('u1')
u2 = cs.MX.sym('u2')
u3 = cs.MX.sym('u3')
u4 = cs.MX.sym('u4')
self.u = cs.vertcat(u1, u2, u3, u4)
# Nominal model equations symbolic function (no GP)
self.quad_xdot_nominal = self.quad_dynamics(rdrv_d_mat)
# Linearized model dynamics symbolic function
self.quad_xdot_jac = self.linearized_quad_dynamics()
# Initialize objective function, 0 target state and integration equations
self.L = None
self.target = None
# Check if GP ensemble has an homogeneous feature space (if actual Ensemble)
if gp_regressors is not None and gp_regressors.homogeneous:
self.gp_reg_ensemble = gp_regressors
self.B_x = [B_x[dim] for dim in gp_regressors.dim_idx]
self.B_x = np.squeeze(np.stack(self.B_x, axis=1))
self.B_x = self.B_x[:, np.newaxis] if len(self.B_x.shape) == 1 else self.B_x
self.B_z = gp_regressors.B_z
elif gp_regressors and gp_regressors.no_ensemble:
# If not homogeneous, we have to treat each z feature vector independently
self.gp_reg_ensemble = gp_regressors
self.B_x = [B_x[dim] for dim in gp_regressors.dim_idx]
self.B_x = np.squeeze(np.stack(self.B_x, axis=1))
self.B_x = self.B_x[:, np.newaxis] if len(self.B_x.shape) == 1 else self.B_x
self.B_z = gp_regressors.B_z
else:
self.gp_reg_ensemble = None
# Declare model variables for GP prediction (only used in real quadrotor flight with EKF estimator).
# Will be used as initial state for GP prediction as Acados parameters.
self.gp_p = cs.MX.sym('gp_p', 3)
self.gp_q = cs.MX.sym('gp_a', 4)
self.gp_v = cs.MX.sym('gp_v', 3)
self.gp_r = cs.MX.sym('gp_r', 3)
self.gp_x = cs.vertcat(self.gp_p, self.gp_q, self.gp_v, self.gp_r)
# The trigger variable is used to tell ACADOS to use the additional GP state estimate in the first optimization
# node and the regular integrated state in the rest
self.trigger_var = cs.MX.sym('trigger', 1)
# Build full model. Will have 13 variables. self.dyn_x contains the symbolic variable that
# should be used to evaluate the dynamics function. It corresponds to self.x if there are no GP's, or
# self.x_with_gp otherwise
acados_models, nominal_with_gp = self.acados_setup_model(
self.quad_xdot_nominal(x=self.x, u=self.u)['x_dot'], model_name)
# Convert dynamics variables to functions of the state and input vectors
self.quad_xdot = {}
for dyn_model_idx in nominal_with_gp.keys():
dyn = nominal_with_gp[dyn_model_idx]
self.quad_xdot[dyn_model_idx] = cs.Function('x_dot', [self.x, self.u], [dyn], ['x', 'u'], ['x_dot'])
# ### Setup and compile Acados OCP solvers ### #
self.acados_ocp_solver = {}
# Check if GP's have been loaded
self.with_gp = self.gp_reg_ensemble is not None
# Add one more weight to the rotation (use quaternion norm weighting in acados)
q_diagonal = np.concatenate((q_cost[:3], np.mean(q_cost[3:6])[np.newaxis], q_cost[3:]))
if q_mask is not None:
q_mask = np.concatenate((q_mask[:3], np.zeros(1), q_mask[3:]))
q_diagonal *= q_mask
# Ensure current working directory is current folder
os.chdir(os.path.dirname(os.path.realpath(__file__)))
self.acados_models_dir = '../../acados_models'
safe_mkdir_recursive(os.path.join(os.getcwd(), self.acados_models_dir))
for key, key_model in zip(acados_models.keys(), acados_models.values()):
nx = key_model.x.size()[0]
nu = key_model.u.size()[0]
ny = nx + nu
n_param = key_model.p.size()[0] if isinstance(key_model.p, cs.MX) else 0
acados_source_path = os.environ['ACADOS_SOURCE_DIR']
sys.path.insert(0, '../common')
# Create OCP object to formulate the optimization
ocp = AcadosOcp()
ocp.acados_include_path = acados_source_path + '/include'
ocp.acados_lib_path = acados_source_path + '/lib'
ocp.model = key_model
ocp.dims.N = self.N
ocp.solver_options.tf = t_horizon
# Initialize parameters
ocp.dims.np = n_param
ocp.parameter_values = np.zeros(n_param)
ocp.cost.cost_type = 'LINEAR_LS'
ocp.cost.cost_type_e = 'LINEAR_LS'
ocp.cost.W = np.diag(np.concatenate((q_diagonal, r_cost)))
ocp.cost.W_e = np.diag(q_diagonal)
terminal_cost = 0 if solver_options is None or not solver_options["terminal_cost"] else 1
ocp.cost.W_e *= terminal_cost
ocp.cost.Vx = np.zeros((ny, nx))
ocp.cost.Vx[:nx, :nx] = np.eye(nx)
ocp.cost.Vu = np.zeros((ny, nu))
ocp.cost.Vu[-4:, -4:] = np.eye(nu)
ocp.cost.Vx_e = np.eye(nx)
# Initial reference trajectory (will be overwritten)
x_ref = np.zeros(nx)
ocp.cost.yref = np.concatenate((x_ref, np.array([0.0, 0.0, 0.0, 0.0])))
ocp.cost.yref_e = x_ref
# Initial state (will be overwritten)
ocp.constraints.x0 = x_ref
# Set constraints
ocp.constraints.lbu = np.array([self.min_u] * 4)
ocp.constraints.ubu = np.array([self.max_u] * 4)
ocp.constraints.idxbu = np.array([0, 1, 2, 3])
# Solver options
ocp.solver_options.qp_solver = 'FULL_CONDENSING_HPIPM'
ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
ocp.solver_options.integrator_type = 'ERK'
ocp.solver_options.print_level = 0
ocp.solver_options.nlp_solver_type = 'SQP_RTI' if solver_options is None else solver_options["solver_type"]
# Compile acados OCP solver if necessary
json_file = os.path.join(self.acados_models_dir, key_model.name + '_acados_ocp.json')
self.acados_ocp_solver[key] = AcadosOcpSolver(ocp, json_file=json_file)
