ocp.constraints.ubu = np.array([+Fmax])
ocp.constraints.idxbu = np.array([0])
ocp.constraints.x0 = np.array([0.0, np.pi, 0.0, 0.0])
# set options
ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES
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' # SQP_RTI, SQP
# set prediction horizon
ocp.solver_options.tf = Tf
ocp_solver = AcadosOcpSolver(ocp, json_file='acados_ocp.json')
simX = np.ndarray((N + 1, nx))
simU = np.ndarray((N, nu))
# call SQP_RTI solver in the loop:
tol = 1e-6
for i in range(20):
status = ocp_solver.solve()
ocp_solver.print_statistics(
) # encapsulates: stat = ocp_solver.get_stats("statistics")
residuals = ocp_solver.get_residuals()
print("residuals after ", i, "SQP_RTI iterations:\n", residuals)
if max(residuals) < tol:
break
ocp.constraints.ubu = np.array([+Fmax])
ocp.constraints.x0 = x0
ocp.constraints.idxbu = np.array([0])
ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES
ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
ocp.solver_options.integrator_type = 'ERK'
ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI
ocp.solver_options.qp_solver_cond_N = N
# 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')
simX = np.ndarray((N + 1, nx))
simU = np.ndarray((N, nu))
xcurrent = x0
simX[0, :] = xcurrent
# closed loop
for i in range(N):
# solve 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)
class QuadOptimizer:
def __init__(self,
quad_model,
quad_constraints,
t_horizon,
n_nodes,
sim_required=False,
max_hight=1.0):
self.model = quad_model
self.constraints = quad_constraints
self.g = 9.8066
self.T = t_horizon
self.N = n_nodes
self.simulation_required = sim_required
robot_name_ = rospy.get_param("~robot_name", "bebop1_r")
self.current_pose = None
self.current_state = np.zeros((13, 1))
self.dt = 0.02
self.rate = rospy.Rate(1 / self.dt)
self.time_stamp = None
self.trajectory_path = None
self.current_twist = np.zeros(3)
self.att_command = AttitudeTarget()
self.att_command.type_mask = 3
self.pendulum_state = np.zeros(4)
# subscribers
# the robot state
robot_state_sub_ = rospy.Subscriber('/robot_pose', Odometry,
self.robot_state_callback)
# pendulum state
pendulum_state_sub_ = rospy.Subscriber('/pendulum_pose', Odometry,
self.pendulum_state_callback)
# trajectory
robot_trajectory_sub_ = rospy.Subscriber(
'/robot_trajectory', itm_trajectory_msg,
self.trajectory_command_callback)
# publisher
self.att_setpoint_pub = rospy.Publisher(
'/mavros/setpoint_raw/attitude', AttitudeTarget, queue_size=1)
# create a server
server_ = rospy.Service('uav_mpc_server', SetBool, self.state_server)
# setup optimizer
self.quadrotor_optimizer_setup()
# # It seems that thread cannot ensure the performance of the time
self.att_thread = Thread(target=self.send_command, args=())
self.att_thread.daemon = True
self.att_thread.start()
def robot_state_callback(self, data):
# robot state as [x, y, z, vx, vy, vz, roll, pitch, yaw]
roll, pitch, yaw = self.quaternion_to_rpy(data.pose.pose.orientation)
# self.current_state[:6] = np.array([data.pose.pose.position.x, data.pose.pose.position.y, data.pose.pose.position.z,
# data.twist.twist.linear.x, data.twist.twist.linear.y, data.twist.twist.linear.z]).reshape(6,1)
# self.current_state[-3:] = np.array([roll, pitch, yaw]).reshape(3,1)
self.current_state = np.array([
data.pose.pose.position.x, data.pose.pose.position.y,
data.pose.pose.position.z, data.twist.twist.linear.x,
data.twist.twist.linear.y, data.twist.twist.linear.z,
data.pose.pose.position.x, data.pose.pose.position.y, 0., 0., roll,
pitch, yaw
],
dtype=np.float64)
def pendulum_state_callback(self, data):
# pendulum state as [x, y, z, vx, vy, vz, s, r, ds, dr, roll, pitch, yaw]
s, r = data.pose.pose.position.x, data.pose.pose.position.y
ds, dr = data.twist.twist.linear.x, data.twist.twist.linear.y
# self.current_state[7:10] = np.array([s, r, ds, dr]).reshape(4, 1)
self.pendulum_state = np.array([s, r, ds, dr]) # .reshape(4, 1)
# self.current_state_pendulum = np.array([self.current_state[0], self.current_state[1], self.current_state[2],
# self.current_state[3], self.current_state[4], self.current_state[5],
# s, r, ds, dr,
# self.current_state[10], self.current_state[11], self.current_state[12]], dtype=np.float64)
def trajectory_command_callback(self, data):
temp_traj = data.traj
if data.size != len(temp_traj):
rospy.logerr('Some data are lost')
else:
self.trajectory_path = np.zeros((data.size, 13))
for i in range(data.size):
self.trajectory_path[i] = np.array([
temp_traj[i].x,
temp_traj[i].y,
temp_traj[i].z,
temp_traj[i].vx,
temp_traj[i].vy,
temp_traj[i].vz,
temp_traj[i].load_x,
temp_traj[i].load_y,
temp_traj[i].load_vx,
temp_traj[i].load_vy,
temp_traj[i].roll,
temp_traj[i].pitch,
temp_traj[i].yaw,
])
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 mpc_estimation_loop(self, mpc_iter):
if self.trajectory_path is not None and self.current_state is not None:
t1 = time.time()
# dt = 0.1
current_trajectory = self.trajectory_path
u_des = np.array([0.0, 0.0, 0.0, self.g])
new_state = self.current_state
# new_state[6:10] = self.pendulum_state
new_state[6] = self.pendulum_state[0] - self.current_state[0]
new_state[7] = self.pendulum_state[1] - self.current_state[1]
new_state[8] = self.pendulum_state[2] - self.current_state[3]
new_state[9] = self.pendulum_state[3] - self.current_state[4]
simX[mpc_iter] = new_state # mpc_iter+1 ?
simD[mpc_iter] = current_trajectory[0]
l = 0.1
#print(np.rad2deg(np.arcsin(new_state[6]/l)))
# print()
# print("current state: ")
# np.set_printoptions(suppress=True)
# print(new_state)
# print()
# print("current trajectory: ")
# print(current_trajectory[-1])
tra = current_trajectory[0]
# error_xyz = np.linalg.norm(np.array([new_state[0] - tra[0], new_state[1] - tra[1], new_state[2] - tra[2]]))
# print(error_xyz)
self.solver.set(self.N, 'yref', current_trajectory[-1, :6])
for i in range(self.N):
self.solver.set(
i, 'yref',
np.concatenate([current_trajectory[i].flatten(), u_des]))
# self.solver.set(0, 'lbx', self.current_state)
# self.solver.set(0, 'ubx', self.current_state)
self.solver.set(0, 'lbx', new_state)
self.solver.set(0, 'ubx', new_state)
status = self.solver.solve()
tarr[mpc_iter] = time.time() - t1
if status != 0:
rospy.logerr("MPC cannot find a proper solution.")
# self.solver.print_statistics()
print()
print("current state: ")
np.set_printoptions(suppress=True)
print(new_state)
print()
print("current trajectory: ")
print(current_trajectory[0])
self.att_command.orientation = Quaternion(
*self.rpy_to_quaternion(0.0, 0.0, 0.0, w_first=False))
#self.att_command.thrust = 0.5
self.att_command.thrust = 0.62
self.att_command.body_rate.z = 0.0
else:
# print()
# print("predicted trajectory:")
# for i in range(self.N):
# print(self.solver.get(i, 'x'))
mpc_u_ = self.solver.get(0, 'u')
simU[mpc_iter] = mpc_u_
quat_local_ = self.rpy_to_quaternion(mpc_u_[0],
mpc_u_[1],
mpc_u_[2],
w_first=False)
self.att_command.orientation.x = quat_local_[0]
self.att_command.orientation.y = quat_local_[1]
self.att_command.orientation.z = quat_local_[2]
self.att_command.orientation.w = quat_local_[3]
# print(self.att_command.orientation)
#self.att_command.thrust = mpc_u_[3]/9.8066 - 0.5
# self.att_command.thrust = mpc_u_[3]/9.8066 - 0.38
self.att_command.thrust = mpc_u_[3] / 9.8066 * 0.6175
# print()
# print("mpc_u: ")
# print(mpc_u_)
# print()
# print("---------------------------------------")
# print(self.att_command.thrust)
# yaw_command_ = self.yaw_command(current_yaw_, trajectory_path_[1, -1], 0.0)
# yaw_command_ = self.yaw_controller(trajectory_path_[1, -1]-current_yaw_)
# self.att_command.angular.z = yaw_command_
#self.att_command.body_rate.z = 0.0
# self.att_setpoint_pub.publish(self.att_command)
# print("time: ")
# print(time.time()-time_1)
else:
if self.trajectory_path is None:
rospy.loginfo("waiting trajectory")
elif self.current_state is None:
rospy.loginfo("waiting current state")
else:
rospy.loginfo("Unknown error")
self.rate.sleep()
return True
@staticmethod
def quaternion_to_rpy(quaternion):
q0, q1, q2, q3 = quaternion.w, quaternion.x, quaternion.y, quaternion.z
roll_ = np.arctan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1**2 + q2**2))
pitch_ = np.arcsin(2 * (q0 * q2 - q3 * q1))
yaw_ = np.arctan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2**2 + q3**2))
return roll_, pitch_, yaw_
@staticmethod
def state_server(req):
return SetBoolResponse(True, 'MPC is ready')
@staticmethod
def rpy_to_quaternion(r, p, y, w_first=True):
cy = np.cos(y * 0.5)
sy = np.sin(y * 0.5)
cp = np.cos(p * 0.5)
sp = np.sin(p * 0.5)
cr = np.cos(r * 0.5)
sr = np.sin(r * 0.5)
qw = cr * cp * cy + sr * sp * sy
qx = sr * cp * cy - cr * sp * sy
qy = cr * sp * cy + sr * cp * sy
qz = cr * cp * sy - sr * sp * cy
if w_first:
return np.array([qw, qx, qy, qz])
else:
return np.array([qx, qy, qz, qw])
def send_command(self, ):
rate = rospy.Rate(100) # Hz
self.att_command.header = Header()
while not rospy.is_shutdown():
# t2 = time.time()
command_ = self.att_command
# self.att_command.header.stamp = rospy.Time.now()
self.att_setpoint_pub.publish(command_)
try: # prevent garbage in console output when thread is killed
rate.sleep()
except rospy.ROSInterruptException:
pass
ocp.constraints.uh = nmp.array([1.0])
# slack for nonlinear quaternion
ocp.constraints.lsh = nmp.array([1e-5])
ocp.constraints.ush = nmp.array([1e-5])
ocp.constraints.idxsh = nmp.array([0])
ocp.solver_options.qp_solver = 'FULL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES
ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
ocp.solver_options.integrator_type = 'IRK'
# set prediction horizon
ocp.solver_options.tf = Tf
ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI
ocp.solver_options.print_level = 0
ocp.solver_options.qp_solver_iter_max = 200
ocp_solver = AcadosOcpSolver(ocp, json_file='acados_ocp.json')
simX = nmp.ndarray((N + 1, nx))
simU = nmp.ndarray((N, nu))
status = ocp_solver.solve()
# if status != 0:
# raise Exception('acados returned status {}. Exiting.'.format(status))
# get solution
for i in range(N):
simX[i, :] = ocp_solver.get(i, "x")
simU[i, :] = ocp_solver.get(i, "u")
simX[N, :] = ocp_solver.get(N, "x")
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()
# 'FULL_CONDENSING_QPOASES', 'FULL_CONDENSING_HPIPM', \
# 'PARTIAL_CONDENSING_QPDUNES', 'PARTIAL_CONDENSING_OSQP')
ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
ocp.solver_options.integrator_type = 'ERK'
# ocp.solver_options.print_level = 1
ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI, SQP
ocp.solver_options.nlp_solver_max_iter = 20 # SQP_RTI, SQP
ocp.solver_options.qp_solver_iter_max = 2000
# ocp.solver_options.qp_solver_warm_start = 1
# set prediction horizon
ocp.solver_options.tf = Tf
ocp_solver = AcadosOcpSolver(ocp, json_file = 'acados_ocp.json')
# ocp_solver.options_set("qp_solver_warm_start", 1)
simX = np.ndarray((N+1, nx))
simU = np.ndarray((N, nu))
# test setter
ocp_solver.set(0, "u", 0.0)
ocp_solver.set(0, "u", 0)
ocp_solver.set(0, "u", np.array([0]))
status = ocp_solver.solve()
if status != 0:
ocp_solver.print_statistics() # encapsulates: stat = ocp_solver.get_stats("statistics")
def run_closed_loop_experiment(FORMULATION):
# create ocp object to formulate the OCP
ocp = AcadosOcp()
# set model
model = export_pendulum_ode_model()
ocp.model = model
Tf = 1.0
nx = model.x.size()[0]
nu = model.u.size()[0]
ny = nx + nu
ny_e = nx
N = 20
# set dimensions
# NOTE: all dimensions but N ar detected
ocp.dims.N = N
# set cost module
ocp.cost.cost_type = 'LINEAR_LS'
ocp.cost.cost_type_e = 'LINEAR_LS'
Q = 2 * np.diag([1e3, 1e3, 1e-2, 1e-2])
R = 2 * np.diag([1e-2])
ocp.cost.W = scipy.linalg.block_diag(Q, R)
ocp.cost.Vx = np.zeros((ny, nx))
ocp.cost.Vx[:nx, :nx] = np.eye(nx)
Vu = np.zeros((ny, nu))
Vu[4, 0] = 1.0
ocp.cost.Vu = Vu
ocp.cost.Vx_e = np.eye(nx)
ocp.cost.W_e = Q
ocp.cost.yref = np.zeros((ny, ))
ocp.cost.yref_e = np.zeros((ny_e, ))
ocp.cost.zl = 2000 * np.ones((1, ))
ocp.cost.Zl = 1 * np.ones((1, ))
ocp.cost.zu = 2000 * np.ones((1, ))
ocp.cost.Zu = 1 * np.ones((1, ))
# set constraints
Fmax = 80
vmax = 5
x0 = np.array([0.0, np.pi, 0.0, 0.0])
ocp.constraints.x0 = x0
# bound on u
ocp.constraints.lbu = np.array([-Fmax])
ocp.constraints.ubu = np.array([+Fmax])
ocp.constraints.idxbu = np.array([0])
if FORMULATION == 0:
# soft bound on x
ocp.constraints.lbx = np.array([-vmax])
ocp.constraints.ubx = np.array([+vmax])
ocp.constraints.idxbx = np.array([2]) # v is x[2]
# indices of slacked constraints within bx
ocp.constraints.idxsbx = np.array([0])
elif FORMULATION == 1:
# soft bound on x, using constraint h
v1 = ocp.model.x[2]
ocp.model.con_h_expr = v1
ocp.constraints.lh = np.array([-vmax])
ocp.constraints.uh = np.array([+vmax])
# indices of slacked constraints within h
ocp.constraints.idxsh = np.array([0])
# set options
ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'
ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
ocp.solver_options.integrator_type = 'ERK'
ocp.solver_options.tf = Tf
ocp.solver_options.nlp_solver_type = 'SQP'
json_filename = 'pendulum_soft_constraints.json'
acados_ocp_solver = AcadosOcpSolver(ocp, json_file=json_filename)
acados_integrator = AcadosSimSolver(ocp, json_file=json_filename)
# closed loop
Nsim = 20
simX = np.ndarray((Nsim + 1, nx))
simU = np.ndarray((Nsim, nu))
xcurrent = x0
for i in range(Nsim):
simX[i, :] = xcurrent
# solve ocp
acados_ocp_solver.set(0, "lbx", xcurrent)
acados_ocp_solver.set(0, "ubx", xcurrent)
status = acados_ocp_solver.solve()
if status != 0:
raise Exception(
'acados acados_ocp_solver returned status {}. Exiting.'.format(
status))
simU[i, :] = acados_ocp_solver.get(0, "u")
# simulate system
acados_integrator.set("x", xcurrent)
acados_integrator.set("u", simU[i, :])
status = acados_integrator.solve()
if status != 0:
raise Exception(
'acados integrator returned status {}. Exiting.'.format(
status))
# update state
xcurrent = acados_integrator.get("x")
simX[Nsim, :] = xcurrent
# get slack values at stage 1
sl = acados_ocp_solver.get(1, "sl")
su = acados_ocp_solver.get(1, "su")
print("sl", sl, "su", su)
# plot results
plot_pendulum(Tf / N, Fmax, simU, simX, latexify=False)
# store results
np.savetxt('test_results/simX_soft_formulation_' + str(FORMULATION), simX)
np.savetxt('test_results/simU_soft_formulation_' + str(FORMULATION), simU)
print("soft constraint example: ran formulation", FORMULATION,
"successfully.")
def main(use_cython=True):
# (very) simple crane model
beta = 0.001
k = 0.9
a_max = 10
dt_max = 2.0
# states
p1 = SX.sym('p1')
v1 = SX.sym('v1')
p2 = SX.sym('p2')
v2 = SX.sym('v2')
x = vertcat(p1, v1, p2, v2)
# controls
a = SX.sym('a')
dt = SX.sym('dt')
u = vertcat(a, dt)
f_expl = dt*vertcat(v1, a, v2, -beta*v2-k*(p2 - p1))
model = AcadosModel()
model.f_expl_expr = f_expl
model.x = x
model.u = u
model.name = 'crane_time_opt'
# create ocp object to formulate the OCP
x0 = np.array([2.0, 0.0, 2.0, 0.0])
xf = np.array([0.0, 0.0, 0.0, 0.0])
ocp = AcadosOcp()
ocp.model = model
# N - maximum number of bangs
N = 7
Tf = N
nx = model.x.size()[0]
nu = model.u.size()[0]
# set dimensions
ocp.dims.N = N
# set cost
ocp.cost.cost_type = 'EXTERNAL'
ocp.cost.cost_type_e = 'EXTERNAL'
ocp.model.cost_expr_ext_cost = dt
ocp.model.cost_expr_ext_cost_e = 0
ocp.constraints.lbu = np.array([-a_max, 0.0])
ocp.constraints.ubu = np.array([+a_max, dt_max])
ocp.constraints.idxbu = np.array([0, 1])
ocp.constraints.x0 = x0
ocp.constraints.lbx_e = xf
ocp.constraints.ubx_e = xf
ocp.constraints.idxbx_e = np.array([0, 1, 2, 3])
# set prediction horizon
ocp.solver_options.tf = Tf
# set options
ocp.solver_options.qp_solver = 'FULL_CONDENSING_QPOASES'#'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES
ocp.solver_options.integrator_type = 'ERK'
ocp.solver_options.print_level = 3
ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI, SQP
ocp.solver_options.globalization = 'MERIT_BACKTRACKING'
ocp.solver_options.nlp_solver_max_iter = 5000
ocp.solver_options.nlp_solver_tol_stat = 1e-6
ocp.solver_options.levenberg_marquardt = 0.1
ocp.solver_options.sim_method_num_steps = 15
ocp.solver_options.qp_solver_iter_max = 100
ocp.code_export_directory = 'c_generated_code'
if use_cython:
AcadosOcpSolver.generate(ocp, json_file='acados_ocp.json')
AcadosOcpSolver.build(ocp.code_export_directory, with_cython=True)
ocp_solver = AcadosOcpSolver.create_cython_solver('acados_ocp.json')
else: # ctypes
## Note: skip generate and build assuming this is done before (in cython run)
ocp_solver = AcadosOcpSolver(ocp, json_file='acados_ocp.json', build=False, generate=False)
for i, tau in enumerate(np.linspace(0, 1, N)):
ocp_solver.set(i, 'x', (1-tau)*x0 + tau*xf)
ocp_solver.set(i, 'u', np.array([0.1, 0.5]))
simX = np.zeros((N+1, nx))
simU = np.zeros((N, nu))
status = ocp_solver.solve()
if status != 0:
ocp_solver.print_statistics()
raise Exception('acados returned status {}. Exiting.'.format(status))
# get solution
for i in range(N):
simX[i,:] = ocp_solver.get(i, "x")
simU[i,:] = ocp_solver.get(i, "u")
simX[N,:] = ocp_solver.get(N, "x")
dts = simU[:, 1]
print("acados solved OCP successfully, creating integrator to simulate the solution")
# simulate on finer grid
sim = AcadosSim()
# set model
sim.model = model
# set options
sim.solver_options.integrator_type = 'ERK'
sim.solver_options.num_stages = 4
sim.solver_options.num_steps = 3
sim.solver_options.T = 1.0 # dummy value
dt_approx = 0.0005
dts_fine = np.zeros((N,))
Ns_fine = np.zeros((N,), dtype='int16')
# compute number of simulation steps for bang interval + dt_fine
for i in range(N):
N_approx = max(int(dts[i]/dt_approx), 1)
dts_fine[i] = dts[i]/N_approx
Ns_fine[i] = int(round(dts[i]/dts_fine[i]))
N_fine = int(np.sum(Ns_fine))
simU_fine = np.zeros((N_fine, nu))
ts_fine = np.zeros((N_fine+1, ))
simX_fine = np.zeros((N_fine+1, nx))
simX_fine[0, :] = x0
acados_integrator = AcadosSimSolver(sim)
k = 0
for i in range(N):
u = simU[i, 0]
acados_integrator.set("u", np.hstack((u, np.ones(1, ))))
# set simulation time
acados_integrator.set("T", dts_fine[i])
for j in range(Ns_fine[i]):
acados_integrator.set("x", simX_fine[k,:])
status = acados_integrator.solve()
if status != 0:
raise Exception('acados returned status {}. Exiting.'.format(status))
simX_fine[k+1,:] = acados_integrator.get("x")
simU_fine[k, :] = u
ts_fine[k+1] = ts_fine[k] + dts_fine[i]
k += 1
# visualize
if os.environ.get('ACADOS_ON_TRAVIS'):
plt.figure()
state_labels = ['p1', 'v1', 'p2', 'v2']
for i,l in enumerate(state_labels):
plt.subplot(5, 1, i+1)
plt.plot(ts_fine, simX_fine[:, i], label='time optimal solution')
plt.grid(True)
plt.ylabel(l)
if i ==0:
plt.legend(loc=1)
plt.subplot(5, 1, 5)
plt.step(ts_fine, np.hstack((simU_fine[:, 0], simU_fine[-1, 0])), '-', where='post')
plt.grid(True)
plt.ylabel('a')
plt.xlabel('t')
plt.show()
ocp.constraints.x0 = np.array([0.0, np.pi, 0.0, 0.0])
ocp.constraints.idxbu = np.array([0])
ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES
ocp.solver_options.hessian_approx = HESSIAN_APPROXIMATION
ocp.solver_options.regularize_method = 'CONVEXIFY'
ocp.solver_options.integrator_type = 'ERK'
ocp.solver_options.qp_solver_cond_N = 5
# set prediction horizon
ocp.solver_options.tf = Tf
ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI
ocp.solver_options.ext_cost_num_hess = EXTERNAL_COST_USE_NUM_HESS
ocp_solver = AcadosOcpSolver(ocp, json_file='acados_ocp.json')
# from casadi import jacobian
# ux = vertcat(ocp.model.u, ocp.model.x)
# jacobian(jacobian(ocp.model.cost_expr_ext_cost, ux), ux)
# SX(@1=0.04, @2=4000,
# [[@1, 00, 00, 00, 00],
# [00, @2, 00, 00, 00],
# [00, 00, @2, 00, 00],
# [00, 00, 00, @1, 00],
# [00, 00, 00, 00, @1]])
# NOTE: hessian is wrt [u,x]
if EXTERNAL_COST_USE_NUM_HESS:
for i in range(N):
ocp_solver.cost_set(i, "ext_cost_num_hess",
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
class Pmpc(object):
def __init__(self,
N,
sys,
cost,
wref=None,
tuning=None,
lam_g_ref=None,
options={}):
""" Constructor
"""
# store construction data
self.__N = N
self.__vars = sys['vars']
self.__nx = sys['vars']['x'].shape[0]
self.__nu = sys['vars']['u'].shape[0]
# nonlinear inequalities slacks
if 'us' in sys['vars']:
self.__ns = sys['vars']['us'].shape[0]
else:
self.__ns = 0
# mpc slacks
if 'usc' in sys['vars']:
self.__nsc = sys['vars']['usc'].shape[0]
self.__scost = sys['scost']
else:
self.__nsc = 0
# store system dynamics
self.__F = sys['f']
# store path constraints
if 'h' in sys:
self.__h = sys['h']
else:
self.__h = None
# store slacked nonlinear inequality constraints
if 'g' in sys:
self.__gnl = sys['g']
else:
self.__gnl = None
# store system sensitivities around steady state
self.__S = sys['S']
self.__cost = cost
# set options
self.__options = self.__default_options()
for option in options:
if option in self.__options:
self.__options[option] = options[option]
else:
raise ValueError(
'Unknown option for Pmpc class instance: "{}"'.format(
option))
# detect cost-type
if self.__cost.n_in() == 2:
# cost function of the form: l(x,u)
self.__type = 'economic'
# no tuning required
tuning = None
if self.__options['hessian_approximation'] == 'gauss_newton':
self.__options['hessian_approximation'] = 'exact'
Logger.logger.warning(
'Gauss-Newton Hessian approximation cannot be applied for economic MPC problem. Switched to exact Hessian.'
)
else:
# cost function of the form: (w-wref)'*H*(w-wref) + q'w
self.__type = 'tracking'
# check if tuning matrices are provided
assert tuning != None, 'Provide tuning matrices for tracking MPC!'
# periodicity operator
self.__p_operator = self.__options['p_operator']
# construct MPC solver
self.__construct_solver()
# periodic indexing
self.__index = 0
self.__index_acados = 0
# create periodic reference
assert wref != None, 'Provide reference trajectory!'
self.__create_reference(wref, tuning, lam_g_ref)
# initialize log
self.__initialize_log()
# initialize acados solvers
self.__acados_ocp_solver = None
self.__acados_integrator = None
# solver initial guess
self.__set_initial_guess()
return None
def __default_options(self):
# default options
opts = {
'hessian_approximation':
'exact',
'ipopt_presolve':
False,
'max_iter':
2000,
'p_operator':
ca.Function('p_operator', [self.__vars['x']], [self.__vars['x']]),
'slack_flag':
'none'
}
return opts
def __construct_solver(self):
""" Construct periodic MPC solver
"""
# system variables and dimensions
x = self.__vars['x']
u = self.__vars['u']
# NLP parameters
if self.__type == 'economic':
# parameters
self.__p = ct.struct_symMX([
ct.entry('x0', shape=(self.__nx, 1)),
ct.entry('xN', shape=(self.__nx, 1))
])
# reassign for brevity
x0 = self.__p['x0']
xN = self.__p['xN']
if self.__type == 'tracking':
ref_vars = (ct.entry('x', shape=(self.__nx, ),
repeat=self.__N + 1),
ct.entry('u', shape=(self.__nu, ), repeat=self.__N))
if 'us' in self.__vars:
ref_vars += (ct.entry('us',
shape=(self.__ns, ),
repeat=self.__N), )
# reference trajectory
wref = ct.struct_symMX([ref_vars])
nw = self.__nx + self.__nu + self.__ns
tuning = ct.struct_symMX([ # tracking tuning
ct.entry('H', shape=(nw, nw), repeat=self.__N),
ct.entry('q', shape=(nw, 1), repeat=self.__N)
])
# parameters
self.__p = ct.struct_symMX([
ct.entry('x0', shape=(self.__nx, )),
ct.entry('wref', struct=wref),
ct.entry('tuning', struct=tuning)
])
# reassign for brevity
x0 = self.__p['x0']
wref = self.__p.prefix['wref']
tuning = self.__p.prefix['tuning']
xN = wref['x', -1]
# NLP variables
variables_entry = (ct.entry('x',
shape=(self.__nx, ),
repeat=self.__N + 1),
ct.entry('u', shape=(self.__nu, ), repeat=self.__N))
if 'us' in self.__vars:
variables_entry += (ct.entry('us',
shape=(self.__ns, ),
repeat=self.__N), )
self.__wref = ct.struct_symMX([variables_entry
]) # structure of reference
if 'usc' in self.__vars:
variables_entry += (ct.entry('usc',
shape=(self.__nsc, ),
repeat=self.__N), )
# nlp variables + bounds
w = ct.struct_symMX([variables_entry])
# variable bounds are implemented as inequalities
self.__lbw = w(-np.inf)
self.__ubw = w(np.inf)
# prepare dynamics and path constraints entry
constraints_entry = (ct.entry('dyn',
shape=(self.__nx, ),
repeat=self.__N), )
if self.__gnl is not None:
constraints_entry += (ct.entry('g',
shape=self.__gnl.size1_out(0),
repeat=self.__N), )
if self.__h is not None:
constraints_entry += (ct.entry('h',
shape=self.__h.size1_out(0),
repeat=self.__N), )
# terminal constraint
nx_term = self.__p_operator.size1_out(0)
# create general constraints structure
g_struct = ct.struct_symMX([
ct.entry('init', shape=(self.__nx, 1)), constraints_entry,
ct.entry('term', shape=(nx_term, 1))
])
# create symbolic constraint expressions
map_args = collections.OrderedDict()
map_args['x0'] = ct.horzcat(*w['x', :-1])
map_args['p'] = ct.horzcat(*w['u'])
F_constr = ct.horzsplit(self.__F.map(self.__N)(**map_args)['xf'])
# generate constraints
constr = collections.OrderedDict()
constr['dyn'] = [a - b for a, b in zip(F_constr, w['x', 1:])]
if 'us' in self.__vars:
map_args['us'] = ct.horzcat(*w['us'])
if self.__gnl is not None:
constr['g'] = ct.horzsplit(
self.__gnl.map(self.__N)(*map_args.values()))
if 'usc' in self.__vars:
map_args['usc'] = ct.horzcat(*w['usc'])
if self.__h is not None:
constr['h'] = ct.horzsplit(
self.__h.map(self.__N)(*map_args.values()))
repeated_constr = list(
itertools.chain.from_iterable(zip(*constr.values())))
term_constraint = self.__p_operator(w['x', -1] - xN)
self.__g = g_struct(
ca.vertcat(w['x', 0] - x0, *repeated_constr, term_constraint))
self.__lbg = g_struct(np.zeros(self.__g.shape))
self.__ubg = g_struct(np.zeros(self.__g.shape))
if self.__h is not None:
self.__ubg['h', :] = np.inf
# nlp cost
cost_map = self.__cost.map(self.__N)
if self.__type == 'economic':
cost_args = [ct.horzcat(*w['x', :-1]), ct.horzcat(*w['u'])]
elif self.__type == 'tracking':
if self.__ns != 0:
cost_args_w = ct.horzcat(*[
ct.vertcat(w['x', k], w['u', k], w['us', k])
for k in range(self.__N)
])
cost_args_w_ref = ct.horzcat(*[
ct.vertcat(wref['x', k], wref['u', k], wref['us', k])
for k in range(self.__N)
])
else:
cost_args_w = ct.horzcat(*[
ct.vertcat(w['x', k], w['u', k]) for k in range(self.__N)
])
cost_args_w_ref = ct.horzcat(*[
ct.vertcat(wref['x', k], wref['u', k])
for k in range(self.__N)
])
cost_args = [
cost_args_w, cost_args_w_ref,
ct.horzcat(*tuning['H']),
ct.horzcat(*tuning['q'])
]
if self.__options['hessian_approximation'] == 'gauss_newton':
if 'usc' not in self.__vars:
hess_gn = ct.diagcat(*tuning['H'],
ca.DM.zeros(self.__nx, self.__nx))
else:
hess_block = list(
itertools.chain.from_iterable(
zip(tuning['H'],
[ca.DM.zeros(self.__nsc, self.__nsc)] *
self.__N)))
hess_gn = ct.diagcat(*hess_block,
ca.DM.zeros(self.__nx, self.__nx))
J = ca.sum2(cost_map(*cost_args))
# add cost on slacks
if 'usc' in self.__vars:
J += ca.sum2(ct.mtimes(self.__scost.T, ct.horzcat(*w['usc'])))
# create solver
prob = {'f': J, 'g': self.__g, 'x': w, 'p': self.__p}
self.__w = w
self.__g_fun = ca.Function('g_fun', [self.__w, self.__p], [self.__g])
# create IPOPT-solver instance if needed
if self.__options['ipopt_presolve']:
opts = {
'ipopt': {
'linear_solver': 'ma57',
'print_level': 0
},
'expand': False
}
if Logger.logger.getEffectiveLevel() > 10:
opts['ipopt']['print_level'] = 0
opts['print_time'] = 0
opts['ipopt']['sb'] = 'yes'
self.__solver = ca.nlpsol('solver', 'ipopt', prob, opts)
# create hessian approximation function
if self.__options['hessian_approximation'] == 'gauss_newton':
lam_g = ca.MX.sym('lam_g', self.__g.shape) # will not be used
hess_approx = ca.Function('hess_approx',
[self.__w, self.__p, lam_g], [hess_gn])
elif self.__options['hessian_approximation'] == 'exact':
hess_approx = 'exact'
# create sqp solver
prob['lbg'] = self.__lbg
prob['ubg'] = self.__ubg
sqp_opts = {
'hessian_approximation': hess_approx,
'max_iter': self.__options['max_iter']
}
self.__sqp_solver = sqp_method.Sqp(prob, sqp_opts)
def step(self, x0):
""" Compute periodic MPC feedback control for given initial condition.
"""
# reset periodic indexing if necessary
self.__index = self.__index % len(self.__ref)
# update nlp parameters
p0 = self.__p(0.0)
p0['x0'] = x0
if self.__type == 'economic':
p0['xN'] = self.__ref[self.__index][-x0.shape[0]:]
elif self.__type == 'tracking':
p0['wref'] = self.__ref[self.__index]
p0['tuning', 'H'] = self.__Href[self.__index]
p0['tuning', 'q'] = self.__qref[self.__index]
# pre-solve NLP with IPOPT for globalization
if self.__options['ipopt_presolve']:
ipopt_sol = self.__solver(x0=self.__w0,
lbg=self.__lbg,
ubg=self.__ubg,
p=p0)
self.__w0 = self.__w(ipopt_sol['x'])
self.__lam_g0 = self.__g(ipopt_sol['lam_g'])
# solve NLP
sol = self.__sqp_solver.solve(self.__w0.cat, p0.cat, self.__lam_g0.cat)
# store solution
self.__g_sol = self.__g(self.__g_fun(sol['x'], p0))
self.__w_sol = self.__w(sol['x'])
self.__extract_solver_stats()
# shift reference
self.__index += 1
# update initial guess
self.__w0, self.__lam_g0 = self.__shift_initial_guess(
self.__w_sol, self.__g(sol['lam_g']))
return self.__w_sol['u', 0]
def step_acados(self, x0):
# reset periodic indexing if necessary
self.__index_acados = self.__index_acados % self.__Nref
# format x0
x0 = np.squeeze(x0.full())
# update NLP parameters
self.__acados_ocp_solver.set(0, "lbx", x0)
self.__acados_ocp_solver.set(0, "ubx", x0)
# update reference and tuning matrices
self.__set_acados_reference()
# solve
status = self.__acados_ocp_solver.solve()
# if status != 0:
# raise Exception('acados solver returned status {}. Exiting.'.format(status))
# save solution
self.__w_sol_acados = self.__w(0.0)
for i in range(self.__N):
self.__w_sol_acados['x', i] = self.__acados_ocp_solver.get(i, "x")
self.__w_sol_acados['u', i] = self.__acados_ocp_solver.get(
i, "u")[:self.__nu]
if 'us' in self.__vars:
self.__w_sol_acados['us', i] = self.__acados_ocp_solver.get(
i, "u")[self.__nu:]
self.__w_sol_acados['x', self.__N] = self.__acados_ocp_solver.get(
self.__N, "x")
# feedback policy
u0 = self.__acados_ocp_solver.get(0, "u")[:self.__nu]
# update initial guess
self.__shift_initial_guess_acados()
# shift index
self.__index_acados += 1
return u0
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 __create_reference(self, wref, tuning, lam_g_ref):
""" Create periodic reference and tuning data.
"""
# period of reference
self.__Nref = len(wref['u'])
# create reference and tuning sequence
# for each starting point in period
ref_pr = []
ref_du = []
ref_du_struct = []
H = []
q = []
for k in range(self.__Nref):
# reference primal solution
refk = []
for j in range(self.__N):
refk += [
wref['x', (k + j) % self.__Nref],
wref['u', (k + j) % self.__Nref]
]
if 'us' in self.__vars:
refk += [wref['us', (k + j) % self.__Nref]]
refk.append(wref['x', (k + self.__N) % self.__Nref])
# reference dual solution
lamgk = self.__g(0.0)
lamgk['init'] = -lam_g_ref['dyn', (k - 1) % self.__Nref]
for j in range(self.__N):
lamgk['dyn', j] = lam_g_ref['dyn', (k + j) % self.__Nref]
if 'g' in list(lamgk.keys()):
lamgk['g', j] = lam_g_ref['g', (k + j) % self.__Nref]
if 'h' in list(lamgk.keys()):
lam_h = [lam_g_ref['h', (k + j) % self.__Nref]]
if 'usc' in self.__vars:
lam_h += [-self.__scost] # TODO not entirely correct
lamgk['h', j] = ct.vertcat(*lam_h)
lamgk['term'] = self.__p_operator(
lam_g_ref['dyn', (k + self.__N - 1) % self.__Nref])
ref_pr.append(ct.vertcat(*refk))
ref_du.append(lamgk.cat)
ref_du_struct.append(lamgk)
if tuning is not None:
H.append([
tuning['H'][(k + j) % self.__Nref] for j in range(self.__N)
])
q.append([
tuning['q'][(k + j) % self.__Nref] for j in range(self.__N)
])
self.__ref = ref_pr
self.__ref_du = ref_du
self.__ref_du_struct = ref_du_struct
self.__Href = H
self.__qref = q
return None
def __initialize_log(self):
self.__log = {
'cpu': [],
'iter': [],
'f': [],
'status': [],
'sol_x': [],
'lam_x': [],
'lam_g': [],
'u0': [],
'nACtot': [],
'nAC': [],
'idx_AC': [],
'nAS': []
}
return None
def __extract_solver_stats(self):
info = self.__sqp_solver.stats
self.__log['cpu'].append(info['t_wall_total'])
self.__log['iter'].append(info['iter_count'])
self.__log['status'].append(info['return_status'])
self.__log['sol_x'].append(info['x'])
self.__log['lam_g'].append(info['lam_g'])
self.__log['f'].append(info['f'])
self.__log['u0'].append(self.__w(info['x'])['u', 0])
self.__log['nACtot'].append(info['nAC'])
nAC, idx_AC = self.__detect_AC(self.__g(info['lam_g']))
self.__log['nAC'].append(nAC)
self.__log['idx_AC'].append(nAC)
self.__log['nAS'].append(info['nAS'])
return None
def __detect_AC(self, lam_g_opt):
# optimal active set
if 'h' in lam_g_opt.keys():
idx_opt = [
k for k in range(self.__h.size1_out(0) - self.__nsc)
if lam_g_opt['h', 0][k] != 0
]
lam_g_ref = self.__g(self.__ref_du[self.__index])
idx_ref = [
k for k in range(self.__h.size1_out(0) - self.__nsc)
if lam_g_ref['h', 0][k] != 0
]
else:
idx_opt = []
idx_ref = []
# get number of active set changes
nAC = len([k for k in idx_opt if k not in idx_ref])
nAC += len([k for k in idx_ref if k not in idx_opt])
return nAC, idx_opt
def reset(self):
self.__index = 0
self.__index_acados = 0
self.__initialize_log()
self.__set_initial_guess()
return None
def __shift_initial_guess(self, w0, lam_g0):
w_shifted = self.__w(0.0)
lam_g_shifted = self.__g(0.0)
lam_g_shifted['init'] = lam_g0['dyn', 0]
# shift states and controls
for i in range(self.__N):
# shift primal solution
w_shifted['x', i] = w0['x', i + 1]
if i < self.__N - 1:
w_shifted['u', i] = w0['u', i + 1]
if 'us' in self.__vars:
w_shifted['us', i] = w0['us', i + 1]
if 'usc' in self.__vars:
w_shifted['usc', i] = w0['usc', i + 1]
# shift dual solution
lam_g_shifted['dyn', i] = lam_g0['dyn', i + 1]
for constr in ['g', 'h']:
if constr in lam_g0.keys():
lam_g_shifted[constr, i] = lam_g0[constr, i + 1]
# copy final interval
w_shifted['x', self.__N] = w_shifted['x', self.__N - 1]
w_shifted['u', self.__N - 1] = w_shifted['u', self.__N - 2]
if 'us' in self.__vars:
w_shifted['us', self.__N - 1] = w_shifted['us', self.__N - 2]
if 'usc' in self.__vars:
w_shifted['usc', self.__N - 1] = w_shifted['usc', self.__N - 2]
lam_g_shifted['dyn', self.__N - 1] = lam_g_shifted['dyn', self.__N - 2]
for constr in ['g', 'h']:
if constr in lam_g0.keys():
lam_g_shifted[constr,
self.__N - 1] = lam_g_shifted[constr,
self.__N - 2]
lam_g_shifted['term'] = lam_g0['term']
return w_shifted, lam_g_shifted
def __shift_initial_guess_acados(self):
for i in range(self.__N):
x_prev = np.squeeze(self.__w_sol_acados['x', i + 1].full(), axis=1)
self.__acados_ocp_solver.set(i, "x", x_prev)
if i < self.__N - 1:
u_prev = np.squeeze(self.__w_sol_acados['u', i + 1].full(),
axis=1)
if 'us' in self.__vars:
u_prev = np.squeeze(ct.vertcat(
u_prev, self.__w_sol_acados['us', i + 1]).full(),
axis=1)
self.__acados_ocp_solver.set(i, "u", u_prev)
# initial guess in terminal stage on periodic trajectory
idx = (self.__index_acados + self.__N) % self.__Nref
# reference
xref = np.squeeze(self.__ref[(idx + 1) % self.__Nref][:self.__nx],
axis=1)
uref = np.squeeze(self.__ref[idx][self.__nx:self.__nx + self.__nu +
self.__ns],
axis=1)
self.__acados_ocp_solver.set(self.__N, "x", xref)
self.__acados_ocp_solver.set(self.__N - 1, "u", uref)
return None
def __set_initial_guess(self):
# create initial guess at steady state
wref = self.__wref(self.__ref[self.__index])
w0 = self.__w(0.0)
w0['x'] = wref['x']
w0['u'] = wref['u']
if 'us' in self.__vars:
w0['us'] = wref['us']
self.__w0 = w0
# initial guess for multipliers
self.__lam_g0 = self.__g(self.__ref_du[self.__index])
# acados solver initialization at reference
if self.__acados_ocp_solver is not None:
self.__set_acados_initial_guess()
return None
def __set_acados_reference(self):
for i in range(self.__N):
# periodic index
idx = (self.__index_acados + i) % self.__Nref
# reference
xref = np.squeeze(self.__ref[idx][:self.__nx], axis=1)
uref = np.squeeze(self.__ref[idx][self.__nx:self.__nx + self.__nu +
self.__ns],
axis=1)
# construct output reference with gradient term
yref = np.squeeze(
ca.vertcat(xref,uref).full() - \
ct.mtimes(
np.linalg.inv(self.__Href[idx][0]), # inverse of weighting matrix
self.__qref[idx][0].T).full(), # gradient term
axis = 1
)
self.__acados_ocp_solver.set(i, 'yref', yref)
# update tuning matrix
self.__acados_ocp_solver.cost_set(i, 'W', self.__Href[idx][0])
# update constraint bounds
C = self.__S['C'][idx][:, :self.__nx]
D = self.__S['C'][idx][:, self.__nx:]
lg = -self.__S['e'][idx] + ct.mtimes(C, xref).full() + ct.mtimes(
D, uref).full()
ug = 1e8 - self.__S['e'][idx] + ct.mtimes(
C, xref).full() + ct.mtimes(D, uref).full()
self.__acados_ocp_solver.constraints_set(i, 'lg',
np.squeeze(lg, axis=1))
self.__acados_ocp_solver.constraints_set(i, 'ug',
np.squeeze(ug, axis=1))
# update terminal constraint
idx = (self.__index_acados + self.__N) % self.__Nref
x_term = np.squeeze(self.__p_operator(self.__ref[idx][:self.__nx]),
axis=1)
self.__acados_ocp_solver.set(self.__N, 'lbx', x_term)
self.__acados_ocp_solver.set(self.__N, 'ubx', x_term)
return None
def __set_acados_initial_guess(self):
for i in range(self.__N):
# periodic index
idx = (self.__index_acados + i) % self.__Nref
# initialize at reference
xref = np.squeeze(self.__ref[idx][:self.__nx], axis=1)
uref = np.squeeze(self.__ref[idx][self.__nx:self.__nx + self.__nu +
self.__ns],
axis=1)
# set initial guess
self.__acados_ocp_solver.set(i, "x", xref)
self.__acados_ocp_solver.set(i, "u", uref)
# set dual initial guess
ref_dual = self.__ref_du_struct[idx]
self.__acados_ocp_solver.set(i, "pi",
np.squeeze(ref_dual['dyn', i].full()))
# the inequalities are internally organized in the following order:
# [ lbu lbx lg lh ubu ubx ug uh ]
lam_h = []
t = []
if i == 0:
lam_h.append(ref_dual['init']) # lbx_0
t.append(np.zeros((self.__nx, )))
if 'h' in list(ref_dual.keys()):
lam_lh = -ref_dual['h',
0][:ref_dual['h', 0].shape[0] - self.__nsc]
lam_h.append(lam_lh) # lg
t.append(self.__S['e'][idx % self.__Nref])
if 'g' in list(ref_dual.keys()):
lam_h.append(ref_dual['g', 0]) # lh
t.append(np.zeros((ref_dual['g', 0].shape[0], )))
if i == 0:
lam_h.append(np.zeros((self.__nx, ))) # ubx_0
t.append(np.zeros((self.__nx, )))
if 'h' in list(ref_dual.keys()):
lam_h.append(
np.zeros((ref_dual['h', 0].shape[0] - self.__nsc, ))) # ug
t.append(1e8 *
np.ones((ref_dual['h', 0].shape[0] - self.__nsc, 1)) -
self.__S['e'][idx])
if 'g' in list(ref_dual.keys()):
lam_h.append(np.zeros((ref_dual['g', 0].shape[0], ))) # uh
t.append(np.zeros((ref_dual['g', 0].shape[0], )))
if self.__nsc > 0:
lam_sl = self.__scost - ct.mtimes(lam_lh.T, self.__Jsg).T
lam_h.append(lam_sl) # ls
lam_h.append(self.__scost) # us
t.append(np.zeros((self.__nsc, ))) # slg > 0
t.append(np.zeros((self.__nsc, ))) # sug > 0
self.__acados_ocp_solver.set(i, "lam",
np.squeeze(ct.vertcat(*lam_h).full()))
self.__acados_ocp_solver.set(i, "t",
np.squeeze(ct.vertcat(*t).full()))
# terminal state
idx = (self.__index_acados + self.__N) % self.__Nref
xref = np.squeeze(self.__ref[idx][:self.__nx], axis=1)
self.__acados_ocp_solver.set(self.__N, "x", xref)
# terminal multipliers
lam_term = np.squeeze(
ct.vertcat(ref_dual['term'], np.zeros(
(ref_dual['term'].shape[0], ))).full())
self.__acados_ocp_solver.set(self.__N, "lam", lam_term)
return None
@property
def w(self):
return self.__w
@property
def g_sol(self):
return self.__g_sol
@property
def w_sol(self):
return self.__w_sol
@property
def log(self):
return self.__log
@property
def index(self):
return self.__index
@property
def acados_ocp_solver(self):
return self.__acados_ocp_solver
@property
def acados_integrator(self):
return self.__acados_integrator
@property
def w_sol_acados(self):
return self.__w_sol_acados
# #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
ocp.solver_options.print_level = 0
# 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.set("p", p)
Nsim = 200
simX = nmp.zeros((Nsim + 1, nx))
simU = nmp.zeros((Nsim, nu))
simX[0, :] = x0
for i in range(Nsim):
class AcadosInterface(SolverInterface):
"""
The ACADOS solver interface
Attributes
----------
acados_ocp: AcadosOcp
The current AcadosOcp reference
acados_model: AcadosModel
The current AcadosModel reference
lagrange_costs: SX
The lagrange cost function
mayer_costs: SX
The mayer cost function
y_ref = list[np.ndarray]
The lagrange targets
y_ref_end = list[np.ndarray]
The mayer targets
params = dict
All the parameters to optimize
W: np.ndarray
The Lagrange weights
W_e: np.ndarray
The Mayer weights
status: int
The status of the optimization
all_constr: SX
All the Lagrange constraints
end_constr: SX
All the Mayer constraints
all_g_bounds = Bounds
All the Lagrange bounds on the variables
end_g_bounds = Bounds
All the Mayer bounds on the variables
x_bound_max = np.ndarray
All the bounds max
x_bound_min = np.ndarray
All the bounds min
Vu: np.ndarray
The control objective functions
Vx: np.ndarray
The Lagrange state objective functions
Vxe: np.ndarray
The Mayer state objective functions
opts: ACADOS
Options of Acados from ACADOS
Methods
-------
__acados_export_model(self, ocp: OptimalControlProgram)
Creating a generic ACADOS model
__prepare_acados(self, ocp: OptimalControlProgram)
Set some important ACADOS variables
__set_constr_type(self, constr_type: str = "BGH")
Set the type of constraints
__set_constraints(self, ocp: OptimalControlProgram)
Set the constraints from the ocp
__set_cost_type(self, cost_type: str = "NONLINEAR_LS")
Set the type of cost functions
__set_costs(self, ocp: OptimalControlProgram)
Set the cost functions from ocp
__update_solver(self)
Update the ACADOS solver to new values
get_optimized_value(self) -> Union[list[dict], dict]
Get the previously optimized solution
solve(self) -> "AcadosInterface"
Solve the prepared ocp
"""
def __init__(self, ocp, solver_options: Solver.ACADOS = None):
"""
Parameters
----------
ocp: OptimalControlProgram
A reference to the current OptimalControlProgram
solver_options: ACADOS
The options to pass to the solver
"""
if not isinstance(ocp.cx(), SX):
raise RuntimeError("CasADi graph must be SX to be solved with ACADOS. Please set use_sx to True in OCP")
super().__init__(ocp)
# solver_options = solver_options.__dict__
if solver_options is None:
solver_options = Solver.ACADOS()
self.acados_ocp = AcadosOcp(acados_path=solver_options.acados_dir)
self.acados_model = AcadosModel()
self.__set_cost_type(solver_options.cost_type)
self.__set_constr_type(solver_options.constr_type)
self.lagrange_costs = SX()
self.mayer_costs = SX()
self.y_ref = []
self.y_ref_end = []
self.nparams = 0
self.params_initial_guess = None
self.params_bounds = None
self.__acados_export_model(ocp)
self.__prepare_acados(ocp)
self.ocp_solver = None
self.W = np.zeros((0, 0))
self.W_e = np.zeros((0, 0))
self.status = None
self.out = {}
self.real_time_to_optimize = -1
self.all_constr = None
self.end_constr = SX()
self.all_g_bounds = Bounds(interpolation=InterpolationType.CONSTANT)
self.end_g_bounds = Bounds(interpolation=InterpolationType.CONSTANT)
self.x_bound_max = np.ndarray((self.acados_ocp.dims.nx, 3))
self.x_bound_min = np.ndarray((self.acados_ocp.dims.nx, 3))
self.Vu = np.array([], dtype=np.int64).reshape(0, ocp.nlp[0].controls.shape)
self.Vx = np.array([], dtype=np.int64).reshape(0, ocp.nlp[0].states.shape)
self.Vxe = np.array([], dtype=np.int64).reshape(0, ocp.nlp[0].states.shape)
self.opts = Solver.ACADOS() if solver_options is None else solver_options
def __acados_export_model(self, ocp):
"""
Creating a generic ACADOS model
Parameters
----------
ocp: OptimalControlProgram
A reference to the current OptimalControlProgram
"""
if ocp.n_phases > 1:
raise NotImplementedError("More than 1 phase is not implemented yet with ACADOS backend")
# Declare model variables
x = ocp.nlp[0].states.cx
u = ocp.nlp[0].controls.cx
p = ocp.nlp[0].parameters.cx
if ocp.v.parameters_in_list:
for param in ocp.v.parameters_in_list:
if str(param.cx)[:11] == f"time_phase_":
raise RuntimeError("Time constraint not implemented yet with Acados.")
self.nparams = ocp.nlp[0].parameters.shape
self.params_initial_guess = ocp.v.parameters_in_list.initial_guess
self.params_initial_guess.check_and_adjust_dimensions(self.nparams, 1)
self.params_bounds = ocp.v.parameters_in_list.bounds
self.params_bounds.check_and_adjust_dimensions(self.nparams, 1)
x = vertcat(p, x)
x_dot = SX.sym("x_dot", x.shape[0], x.shape[1])
f_expl = vertcat([0] * self.nparams, ocp.nlp[0].dynamics_func(x[self.nparams :, :], u, p))
f_impl = x_dot - f_expl
self.acados_model.f_impl_expr = f_impl
self.acados_model.f_expl_expr = f_expl
self.acados_model.x = x
self.acados_model.xdot = x_dot
self.acados_model.u = u
self.acados_model.con_h_expr = np.zeros((0, 0))
self.acados_model.con_h_expr_e = np.zeros((0, 0))
self.acados_model.p = []
now = datetime.now() # current date and time
self.acados_model.name = f"model_{now.strftime('%Y_%m_%d_%H%M%S%f')[:-4]}"
def __prepare_acados(self, ocp):
"""
Set some important ACADOS variables
Parameters
----------
ocp: OptimalControlProgram
A reference to the current OptimalControlProgram
"""
# set model
self.acados_ocp.model = self.acados_model
# set time
self.acados_ocp.solver_options.tf = ocp.nlp[0].tf
# set dimensions
self.acados_ocp.dims.nx = ocp.nlp[0].states.shape + ocp.nlp[0].parameters.shape
self.acados_ocp.dims.nu = ocp.nlp[0].controls.shape
self.acados_ocp.dims.N = ocp.nlp[0].ns
def __set_constr_type(self, constr_type: str = "BGH"):
"""
Set the type of constraints
Parameters
----------
constr_type: str
The requested type of constraints
"""
self.acados_ocp.constraints.constr_type = constr_type
self.acados_ocp.constraints.constr_type_e = constr_type
def __set_constraints(self, ocp):
"""
Set the constraints from the ocp
Parameters
----------
ocp: OptimalControlProgram
A reference to the current OptimalControlProgram
"""
# constraints handling in self.acados_ocp
if ocp.nlp[0].x_bounds.type != InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT:
raise NotImplementedError(
"ACADOS must declare an InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT " "for the x_bounds"
)
if ocp.nlp[0].u_bounds.type != InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT:
raise NotImplementedError(
"ACADOS must declare an InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT " "for the u_bounds"
)
u_min = np.array(ocp.nlp[0].u_bounds.min)
u_max = np.array(ocp.nlp[0].u_bounds.max)
x_min = np.array(ocp.nlp[0].x_bounds.min)
x_max = np.array(ocp.nlp[0].x_bounds.max)
self.all_constr = SX()
self.end_constr = SX()
# TODO:change for more node flexibility on bounds
self.all_g_bounds = Bounds(interpolation=InterpolationType.CONSTANT)
self.end_g_bounds = Bounds(interpolation=InterpolationType.CONSTANT)
for i, nlp in enumerate(ocp.nlp):
x = nlp.states.cx
u = nlp.controls.cx
p = nlp.parameters.cx
for g, G in enumerate(nlp.g):
if not G:
continue
if G.node[0] == Node.ALL or G.node[0] == Node.ALL_SHOOTING:
self.all_constr = vertcat(self.all_constr, G.function(x, u, p))
self.all_g_bounds.concatenate(G.bounds)
if G.node[0] == Node.ALL:
self.end_constr = vertcat(self.end_constr, G.function(x, u, p))
self.end_g_bounds.concatenate(G.bounds)
elif G.node[0] == Node.END:
self.end_constr = vertcat(self.end_constr, G.function(x, u, p))
self.end_g_bounds.concatenate(G.bounds)
else:
raise RuntimeError(
"Except for states and controls, Acados solver only handles constraints on last or all nodes."
)
self.acados_model.con_h_expr = self.all_constr
self.acados_model.con_h_expr_e = self.end_constr
if not np.all(np.all(u_min.T == u_min.T[0, :], axis=0)):
raise NotImplementedError("u_bounds min must be the same at each shooting point with ACADOS")
if not np.all(np.all(u_max.T == u_max.T[0, :], axis=0)):
raise NotImplementedError("u_bounds max must be the same at each shooting point with ACADOS")
if (
not np.isfinite(u_min).all()
or not np.isfinite(x_min).all()
or not np.isfinite(u_max).all()
or not np.isfinite(x_max).all()
):
raise NotImplementedError(
"u_bounds and x_bounds cannot be set to infinity in ACADOS. Consider changing it "
"to a big value instead."
)
# setup state constraints
# TODO replace all these np.concatenate by proper bound and initial_guess classes
self.x_bound_max = np.ndarray((self.acados_ocp.dims.nx, 3))
self.x_bound_min = np.ndarray((self.acados_ocp.dims.nx, 3))
param_bounds_max = []
param_bounds_min = []
if self.nparams:
param_bounds_max = self.params_bounds.max[:, 0]
param_bounds_min = self.params_bounds.min[:, 0]
for i in range(3):
self.x_bound_max[:, i] = np.concatenate((param_bounds_max, np.array(ocp.nlp[0].x_bounds.max[:, i])))
self.x_bound_min[:, i] = np.concatenate((param_bounds_min, np.array(ocp.nlp[0].x_bounds.min[:, i])))
# setup control constraints
self.acados_ocp.constraints.lbu = np.array(ocp.nlp[0].u_bounds.min[:, 0])
self.acados_ocp.constraints.ubu = np.array(ocp.nlp[0].u_bounds.max[:, 0])
self.acados_ocp.constraints.idxbu = np.array(range(self.acados_ocp.dims.nu))
self.acados_ocp.dims.nbu = self.acados_ocp.dims.nu
# initial state constraints
self.acados_ocp.constraints.lbx_0 = self.x_bound_min[:, 0]
self.acados_ocp.constraints.ubx_0 = self.x_bound_max[:, 0]
self.acados_ocp.constraints.idxbx_0 = np.array(range(self.acados_ocp.dims.nx))
self.acados_ocp.dims.nbx_0 = self.acados_ocp.dims.nx
# setup path state constraints
self.acados_ocp.constraints.Jbx = np.eye(self.acados_ocp.dims.nx)
self.acados_ocp.constraints.lbx = self.x_bound_min[:, 1]
self.acados_ocp.constraints.ubx = self.x_bound_max[:, 1]
self.acados_ocp.constraints.idxbx = np.array(range(self.acados_ocp.dims.nx))
self.acados_ocp.dims.nbx = self.acados_ocp.dims.nx
# setup terminal state constraints
self.acados_ocp.constraints.Jbx_e = np.eye(self.acados_ocp.dims.nx)
self.acados_ocp.constraints.lbx_e = self.x_bound_min[:, -1]
self.acados_ocp.constraints.ubx_e = self.x_bound_max[:, -1]
self.acados_ocp.constraints.idxbx_e = np.array(range(self.acados_ocp.dims.nx))
self.acados_ocp.dims.nbx_e = self.acados_ocp.dims.nx
# setup algebraic constraint
self.acados_ocp.constraints.lh = np.array(self.all_g_bounds.min[:, 0])
self.acados_ocp.constraints.uh = np.array(self.all_g_bounds.max[:, 0])
# setup terminal algebraic constraint
self.acados_ocp.constraints.lh_e = np.array(self.end_g_bounds.min[:, 0])
self.acados_ocp.constraints.uh_e = np.array(self.end_g_bounds.max[:, 0])
def __set_cost_type(self, cost_type: str = "NONLINEAR_LS"):
"""
Set the type of cost functions
Parameters
----------
cost_type: str
The type of cost function
"""
self.acados_ocp.cost.cost_type = cost_type
self.acados_ocp.cost.cost_type_e = cost_type
def __set_costs(self, ocp):
"""
Set the cost functions from ocp
Parameters
----------
ocp: OptimalControlProgram
A reference to the current OptimalControlProgram
"""
def add_linear_ls_lagrange(acados, objectives):
def add_objective(n_variables, is_state):
v_var = np.zeros(n_variables)
var_type = acados.ocp.nlp[0].states if is_state else acados.ocp.nlp[0].controls
rows = objectives.rows + var_type[objectives.params["key"]].index[0]
v_var[rows] = 1.0
if is_state:
acados.Vx = np.vstack((acados.Vx, np.diag(v_var)))
acados.Vu = np.vstack((acados.Vu, np.zeros((n_states, n_controls))))
else:
acados.Vx = np.vstack((acados.Vx, np.zeros((n_controls, n_states))))
acados.Vu = np.vstack((acados.Vu, np.diag(v_var)))
acados.W = linalg.block_diag(acados.W, np.diag([objectives.weight] * n_variables))
node_idx = objectives.node_idx[:-1] if objectives.node[0] == Node.ALL else objectives.node_idx
y_ref = [np.zeros((n_states if is_state else n_controls, 1)) for _ in node_idx]
if objectives.target is not None:
for idx in node_idx:
y_ref[idx][rows] = objectives.target[0][..., idx].T.reshape((-1, 1))
acados.y_ref.append(y_ref)
if objectives.type in allowed_control_objectives:
add_objective(n_controls, False)
elif objectives.type in allowed_state_objectives:
add_objective(n_states, True)
else:
raise RuntimeError(
f"{objectives[0]['objective'].type.name} is an incompatible objective term with LINEAR_LS cost type"
)
def add_linear_ls_mayer(acados, objectives):
if objectives.type in allowed_state_objectives:
vxe = np.zeros(n_states)
rows = objectives.rows + acados.ocp.nlp[0].states[objectives.params["key"]].index[0]
vxe[rows] = 1.0
acados.Vxe = np.vstack((acados.Vxe, np.diag(vxe)))
acados.W_e = linalg.block_diag(acados.W_e, np.diag([objectives.weight] * n_states))
y_ref_end = np.zeros((n_states, 1))
if objectives.target is not None:
y_ref_end[rows] = objectives.target[0][..., -1].T.reshape((-1, 1))
acados.y_ref_end.append(y_ref_end)
else:
raise RuntimeError(f"{objectives.type.name} is an incompatible objective term with LINEAR_LS cost type")
def add_nonlinear_ls_lagrange(acados, objectives, x, u, p):
acados.lagrange_costs = vertcat(acados.lagrange_costs, objectives.function(x, u, p).reshape((-1, 1)))
acados.W = linalg.block_diag(acados.W, np.diag([objectives.weight] * objectives.function.numel_out()))
node_idx = objectives.node_idx[:-1] if objectives.node[0] == Node.ALL else objectives.node_idx
if objectives.target is not None:
acados.y_ref.append([objectives.target[0][..., idx].T.reshape((-1, 1)) for idx in node_idx])
else:
acados.y_ref.append([np.zeros((objectives.function.numel_out(), 1)) for _ in node_idx])
def add_nonlinear_ls_mayer(acados, objectives, x, u, p):
acados.W_e = linalg.block_diag(acados.W_e, np.diag([objectives.weight] * objectives.function.numel_out()))
x = x if objectives.function.sparsity_in("i0").shape != (0, 0) else []
u = u if objectives.function.sparsity_in("i1").shape != (0, 0) else []
acados.mayer_costs = vertcat(acados.mayer_costs, objectives.function(x, u, p).reshape((-1, 1)))
if objectives.target is not None:
acados.y_ref_end.append(objectives.target[0][..., -1].T.reshape((-1, 1)))
else:
acados.y_ref_end.append(np.zeros((objectives.function.numel_out(), 1)))
if ocp.n_phases != 1:
raise NotImplementedError("ACADOS with more than one phase is not implemented yet.")
# costs handling in self.acados_ocp
self.y_ref = []
self.y_ref_end = []
self.lagrange_costs = SX()
self.mayer_costs = SX()
self.W = np.zeros((0, 0))
self.W_e = np.zeros((0, 0))
allowed_control_objectives = [ObjectiveFcn.Lagrange.MINIMIZE_CONTROL]
allowed_state_objectives = [ObjectiveFcn.Lagrange.MINIMIZE_STATE, ObjectiveFcn.Mayer.TRACK_STATE]
if self.acados_ocp.cost.cost_type == "LINEAR_LS":
n_states = ocp.nlp[0].states.shape
n_controls = ocp.nlp[0].controls.shape
self.Vu = np.array([], dtype=np.int64).reshape(0, n_controls)
self.Vx = np.array([], dtype=np.int64).reshape(0, n_states)
self.Vxe = np.array([], dtype=np.int64).reshape(0, n_states)
for i in range(ocp.n_phases):
for J in ocp.nlp[i].J:
if not J:
continue
if J.multi_thread:
raise RuntimeError(
f"The objective function {J.name} was declared with multi_thread=True, "
f"but this is not possible to multi_thread objective function with ACADOS"
)
if J.type.get_type() == ObjectiveFunction.LagrangeFunction:
add_linear_ls_lagrange(self, J)
# Deal with last node to match ipopt formulation
if J.node[0] == Node.ALL:
add_linear_ls_mayer(self, J)
elif J.type.get_type() == ObjectiveFunction.MayerFunction:
add_linear_ls_mayer(self, J)
else:
raise RuntimeError("The objective function is not Lagrange nor Mayer.")
if self.nparams:
raise RuntimeError("Params not yet handled with LINEAR_LS cost type")
# Set costs
self.acados_ocp.cost.Vx = self.Vx if self.Vx.shape[0] else np.zeros((0, 0))
self.acados_ocp.cost.Vu = self.Vu if self.Vu.shape[0] else np.zeros((0, 0))
self.acados_ocp.cost.Vx_e = self.Vxe if self.Vxe.shape[0] else np.zeros((0, 0))
# Set dimensions
self.acados_ocp.dims.ny = sum([len(data[0]) for data in self.y_ref])
self.acados_ocp.dims.ny_e = sum([len(data) for data in self.y_ref_end])
# Set weight
self.acados_ocp.cost.W = self.W
self.acados_ocp.cost.W_e = self.W_e
# Set target shape
self.acados_ocp.cost.yref = np.zeros((self.acados_ocp.cost.W.shape[0],))
self.acados_ocp.cost.yref_e = np.zeros((self.acados_ocp.cost.W_e.shape[0],))
elif self.acados_ocp.cost.cost_type == "NONLINEAR_LS":
for i, nlp in enumerate(ocp.nlp):
for j, J in enumerate(nlp.J):
if not J:
continue
if J.multi_thread:
raise RuntimeError(
f"The objective function {J.name} was declared with multi_thread=True, "
f"but this is not possible to multi_thread objective function with ACADOS"
)
if J.type.get_type() == ObjectiveFunction.LagrangeFunction:
add_nonlinear_ls_lagrange(self, J, nlp.states.cx, nlp.controls.cx, nlp.parameters.cx)
# Deal with last node to match ipopt formulation
if J.node[0] == Node.ALL:
add_nonlinear_ls_mayer(self, J, nlp.states.cx, nlp.controls.cx, nlp.parameters.cx)
elif J.type.get_type() == ObjectiveFunction.MayerFunction:
add_nonlinear_ls_mayer(self, J, nlp.states.cx, nlp.controls.cx, nlp.parameters.cx)
else:
raise RuntimeError("The objective function is not Lagrange nor Mayer.")
# parameter as mayer function
# IMPORTANT: it is considered that only parameters are stored in ocp.objectives, for now.
if self.nparams:
nlp = ocp.nlp[0] # Assume 1 phase
for j, J in enumerate(ocp.J):
add_nonlinear_ls_mayer(self, J, nlp.states.cx, nlp.controls.cx, nlp.parameters.cx)
# Set costs
self.acados_ocp.model.cost_y_expr = (
self.lagrange_costs.reshape((-1, 1)) if self.lagrange_costs.numel() else SX(1, 1)
)
self.acados_ocp.model.cost_y_expr_e = (
self.mayer_costs.reshape((-1, 1)) if self.mayer_costs.numel() else SX(1, 1)
)
# Set dimensions
self.acados_ocp.dims.ny = self.acados_ocp.model.cost_y_expr.shape[0]
self.acados_ocp.dims.ny_e = self.acados_ocp.model.cost_y_expr_e.shape[0]
# Set weight
self.acados_ocp.cost.W = np.zeros((1, 1)) if self.W.shape == (0, 0) else self.W
self.acados_ocp.cost.W_e = np.zeros((1, 1)) if self.W_e.shape == (0, 0) else self.W_e
# Set target shape
self.acados_ocp.cost.yref = np.zeros((self.acados_ocp.cost.W.shape[0],))
self.acados_ocp.cost.yref_e = np.zeros((self.acados_ocp.cost.W_e.shape[0],))
elif self.acados_ocp.cost.cost_type == "EXTERNAL":
raise RuntimeError("EXTERNAL is not interfaced yet, please use NONLINEAR_LS")
else:
raise RuntimeError("Available acados cost type: 'LINEAR_LS', 'NONLINEAR_LS' and 'EXTERNAL'.")
def __update_solver(self):
"""
Update the ACADOS solver to new values
"""
param_init = []
for n in range(self.acados_ocp.dims.N):
if self.y_ref: # Target
self.ocp_solver.cost_set(n, "yref", np.vstack([data[n] for data in self.y_ref])[:, 0])
# check following line
# self.ocp_solver.cost_set(n, "W", self.W)
if self.nparams:
param_init = self.params_initial_guess.init.evaluate_at(n)
self.ocp_solver.set(n, "x", np.concatenate((param_init, self.ocp.nlp[0].x_init.init.evaluate_at(n))))
self.ocp_solver.set(n, "u", self.ocp.nlp[0].u_init.init.evaluate_at(n))
self.ocp_solver.constraints_set(n, "lbu", self.ocp.nlp[0].u_bounds.min[:, 0])
self.ocp_solver.constraints_set(n, "ubu", self.ocp.nlp[0].u_bounds.max[:, 0])
self.ocp_solver.constraints_set(n, "uh", self.all_g_bounds.max[:, 0])
self.ocp_solver.constraints_set(n, "lh", self.all_g_bounds.min[:, 0])
if n == 0:
self.ocp_solver.constraints_set(n, "lbx", self.x_bound_min[:, 0])
self.ocp_solver.constraints_set(n, "ubx", self.x_bound_max[:, 0])
else:
self.ocp_solver.constraints_set(n, "lbx", self.x_bound_min[:, 1])
self.ocp_solver.constraints_set(n, "ubx", self.x_bound_max[:, 1])
if self.y_ref_end:
if len(self.y_ref_end) == 1:
self.ocp_solver.cost_set(self.acados_ocp.dims.N, "yref", np.array(self.y_ref_end[0])[:, 0])
else:
self.ocp_solver.cost_set(self.acados_ocp.dims.N, "yref", np.concatenate(self.y_ref_end)[:, 0])
# check following line
# self.ocp_solver.cost_set(self.acados_ocp.dims.N, "W", self.W_e)
self.ocp_solver.constraints_set(self.acados_ocp.dims.N, "lbx", self.x_bound_min[:, -1])
self.ocp_solver.constraints_set(self.acados_ocp.dims.N, "ubx", self.x_bound_max[:, -1])
if len(self.end_g_bounds.max[:, 0]):
self.ocp_solver.constraints_set(self.acados_ocp.dims.N, "uh", self.end_g_bounds.max[:, 0])
self.ocp_solver.constraints_set(self.acados_ocp.dims.N, "lh", self.end_g_bounds.min[:, 0])
if self.ocp.nlp[0].x_init.init.shape[1] == self.acados_ocp.dims.N + 1:
if self.nparams:
self.ocp_solver.set(
self.acados_ocp.dims.N,
"x",
np.concatenate(
(self.params_initial_guess.init[:, 0], self.ocp.nlp[0].x_init.init[:, self.acados_ocp.dims.N])
),
)
else:
self.ocp_solver.set(self.acados_ocp.dims.N, "x", self.ocp.nlp[0].x_init.init[:, self.acados_ocp.dims.N])
def online_optim(self, ocp):
raise NotImplementedError("online_optim is not implemented yet with ACADOS backend")
def get_optimized_value(self) -> Union[list, dict]:
"""
Get the previously optimized solution
Returns
-------
A solution or a list of solution depending on the number of phases
"""
ns = self.acados_ocp.dims.N
n_params = self.ocp.nlp[0].parameters.shape
acados_x = np.array([self.ocp_solver.get(i, "x") for i in range(ns + 1)]).T
acados_p = acados_x[:n_params, :]
acados_x = acados_x[n_params:, :]
acados_u = np.array([self.ocp_solver.get(i, "u") for i in range(ns)]).T
out = {
"x": [],
"u": acados_u,
"solver_time_to_optimize": self.ocp_solver.get_stats("time_tot")[0],
"real_time_to_optimize": self.real_time_to_optimize,
"iter": self.ocp_solver.get_stats("sqp_iter")[0],
"status": self.status,
"solver": SolverType.ACADOS,
}
out["x"] = vertcat(out["x"], acados_x.reshape(-1, 1, order="F"))
out["x"] = vertcat(out["x"], acados_u.reshape(-1, 1, order="F"))
out["x"] = vertcat(out["x"], acados_p[:, 0])
self.out["sol"] = out
out = []
for key in self.out.keys():
out.append(self.out[key])
return out[0] if len(out) == 1 else out
def solve(self) -> Union[list, dict]:
"""
Solve the prepared ocp
Returns
-------
A reference to the solution
"""
tic = perf_counter()
# Populate costs and constraints vectors
self.__set_costs(self.ocp)
self.__set_constraints(self.ocp)
options = self.opts.as_dict(self)
if self.ocp_solver is None:
for key in options:
setattr(self.acados_ocp.solver_options, key, options[key])
self.ocp_solver = AcadosOcpSolver(self.acados_ocp, json_file="acados_ocp.json")
self.opts.set_only_first_options_has_changed(False)
self.opts.set_has_tolerance_changed(False)
else:
if self.opts.only_first_options_has_changed:
raise RuntimeError(
"Some options has been changed the second time acados was run.",
"Only " + str(Solver.ACADOS.get_tolerance_keys()) + " can be modified.",
)
if self.opts.has_tolerance_changed:
for key in self.opts.get_tolerance_keys():
short_key = key[12:]
self.ocp_solver.options_set(short_key, options[key[1:]])
self.opts.set_has_tolerance_changed(False)
self.__update_solver()
self.status = self.ocp_solver.solve()
self.real_time_to_optimize = perf_counter() - tic
return self.get_optimized_value()
def main(interface_type='ctypes'):
# create ocp object to formulate the OCP
ocp = AcadosOcp()
# set model
model = export_pendulum_ode_model()
ocp.model = model
nx = model.x.size()[0]
nu = model.u.size()[0]
ny = nx + nu
ny_e = nx
# define the different options for the use-case demonstration
N0 = 20 # original number of shooting nodes
N12 = 15 # change the number of shooting nodes for use-cases 1 and 2
condN12 = max(
1, round(N12 / 1)
) # change the number of cond_N for use-cases 1 and 2 (for PARTIAL_* solvers only)
Tf_01 = 1.0 # original final time and for use-case 1
Tf_2 = Tf_01 * 0.7 # change final time for use-case 2 (but keep N identical)
# set dimensions
ocp.dims.N = N0
# set cost
Q = 2 * np.diag([1e3, 1e3, 1e-2, 1e-2])
R = 2 * np.diag([1e-2])
ocp.cost.W_e = Q
ocp.cost.W = scipy.linalg.block_diag(Q, R)
ocp.cost.cost_type = 'LINEAR_LS'
ocp.cost.cost_type_e = 'LINEAR_LS'
ocp.cost.Vx = np.zeros((ny, nx))
ocp.cost.Vx[:nx, :nx] = np.eye(nx)
Vu = np.zeros((ny, nu))
Vu[4, 0] = 1.0
ocp.cost.Vu = Vu
ocp.cost.Vx_e = np.eye(nx)
ocp.cost.yref = np.zeros((ny, ))
ocp.cost.yref_e = np.zeros((ny_e, ))
# set constraints
Fmax = 80
ocp.constraints.lbu = np.array([-Fmax])
ocp.constraints.ubu = np.array([+Fmax])
ocp.constraints.idxbu = np.array([0])
ocp.constraints.x0 = np.array([0.0, np.pi, 0.0, 0.0])
# set options
ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES
# PARTIAL_CONDENSING_HPIPM, FULL_CONDENSING_QPOASES, FULL_CONDENSING_HPIPM,
# PARTIAL_CONDENSING_QPDUNES, PARTIAL_CONDENSING_OSQP
ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
ocp.solver_options.integrator_type = 'ERK'
# ocp.solver_options.print_level = 1
ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI, SQP
# set prediction horizon
ocp.solver_options.tf = Tf_01
print(80 * '-')
print('generate code and compile...')
if interface_type == 'cython':
AcadosOcpSolver.generate(ocp, json_file='acados_ocp.json')
AcadosOcpSolver.build(ocp.code_export_directory, with_cython=True)
ocp_solver = AcadosOcpSolver.create_cython_solver('acados_ocp.json')
elif interface_type == 'ctypes':
ocp_solver = AcadosOcpSolver(ocp, json_file='acados_ocp.json')
elif interface_type == 'cython_prebuilt':
from c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverCython
ocp_solver = AcadosOcpSolverCython(ocp.model.name,
ocp.solver_options.nlp_solver_type,
ocp.dims.N)
# --------------------------------------------------------------------------------
# 0) solve the problem defined here (original from code export), analog to 'minimal_example_ocp.py'
nvariant = 0
simX0 = np.ndarray((N0 + 1, nx))
simU0 = np.ndarray((N0, nu))
print(80 * '-')
print(f'solve original code with N = {N0} and Tf = {Tf_01} s:')
status = ocp_solver.solve()
if status != 0:
ocp_solver.print_statistics(
) # encapsulates: stat = ocp_solver.get_stats("statistics")
raise Exception('acados returned status {}. Exiting.'.format(status))
# get solution
for i in range(N0):
simX0[i, :] = ocp_solver.get(i, "x")
simU0[i, :] = ocp_solver.get(i, "u")
simX0[N0, :] = ocp_solver.get(N0, "x")
ocp_solver.print_statistics(
) # encapsulates: stat = ocp_solver.get_stats("statistics")
ocp_solver.store_iterate(
filename=f'final_iterate_{interface_type}_variant{nvariant}.json',
overwrite=True)
if PLOT: # plot but don't halt
plot_pendulum(np.linspace(0, Tf_01, N0 + 1),
Fmax,
simU0,
simX0,
latexify=False,
plt_show=False,
X_true_label=f'original: N={N0}, Tf={Tf_01}')
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
# dimensions
nx = model.x.size()[0]
nu = model.u.size()[0]
ny = nx + nu
ny_e = nx
nsbx = 1
nh = constraint.expr.shape[0]
nsh = nh
ns = nsh + nsbx
# discretization
ocp.dims.N = N
# 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.zu = 100 * np.ones((ns,))
ocp.cost.Zl = 1 * np.ones((ns,))
ocp.cost.Zu = 1 * 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.zeros([nsbx])
ocp.constraints.usbx = np.zeros([nsbx])
ocp.constraints.idxsbx = np.array(range(nsbx))
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(range(nsh))
# 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"
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_step_length = 0.05
ocp.solver_options.nlp_solver_max_iter = 200
ocp.solver_options.tol = 1e-4
# ocp.solver_options.nlp_solver_tol_comp = 1e-1
# create solver
acados_solver = AcadosOcpSolver(ocp, json_file="acados_ocp.json")
return constraint, model, acados_solver
# set options
ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES
# PARTIAL_CONDENSING_HPIPM, FULL_CONDENSING_QPOASES, FULL_CONDENSING_HPIPM,
# PARTIAL_CONDENSING_QPDUNES, PARTIAL_CONDENSING_OSQP
# ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
ocp.solver_options.integrator_type = 'ERK'
# ocp.solver_options.print_level = 1
ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI, SQP
ocp.solver_options.globalization = 'MERIT_BACKTRACKING'
ocp.solver_options.nlp_solver_max_iter = 500
# set prediction horizon
ocp.solver_options.tf = Tf
ocp_solver = AcadosOcpSolver(ocp, json_file = 'acados_ocp.json')
ocp_solver.options_set("line_search_use_sufficient_descent", 0)
ocp_solver.options_set("full_step_dual", 1)
simX = np.ndarray((N+1, nx))
simU = np.ndarray((N, nu))
for i, tau in enumerate(np.linspace(0, 1, N+1)):
ocp_solver.set(i, 'x', x0*(1-tau) + tau*xf)
status = ocp_solver.solve()
if status != 0:
ocp_solver.print_statistics() # encapsulates: stat = ocp_solver.get_stats("statistics")
# raise Exception(f'acados returned status {status}.')
# get solution
x0 = np.array([0.0, np.pi, 0.0, 0.0])
ocp.constraints.x0 = x0
ocp.constraints.idxbu = np.array([0])
ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES
ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
ocp.solver_options.integrator_type = integrator_type
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_options.initialize_t_slacks = 1
ocp_solver = AcadosOcpSolver(ocp, json_file='acados_ocp.json')
simX = np.ndarray((N + 1, nx))
simU = np.ndarray((N, nu))
# change options after creating ocp_solver
ocp_solver.options_set("step_length", 0.99999)
ocp_solver.options_set("globalization",
"fixed_step") # fixed_step, merit_backtracking
ocp_solver.options_set("tol_eq", 1e-2)
ocp_solver.options_set("tol_stat", 1e-2)
ocp_solver.options_set("tol_ineq", 1e-2)
ocp_solver.options_set("tol_comp", 1e-2)
# initialize solver
for i in range(N):
# ocp.constraints.idxbu = np.array([0]) # ocp.dims.nbu = nu # terminal constraints ocp.constraints.Jbx_e = np.eye(nx) ocp.constraints.ubx_e = xT ocp.constraints.lbx_e = xT ocp.constraints.idxbx_e = np.array(range(nx)) ocp.dims.nbx_e = nx ocp.solver_options.tf = Tf ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' ocp.solver_options.integrator_type = 'ERK' ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI ocp_solver = AcadosOcpSolver(ocp, json_file = 'acados_ocp.json') # initial guess t_traj = np.linspace(0, Tf, N+1) x_traj = np.linspace(x0,xT,N+1) u_traj = np.ones((N,1))+np.random.rand(N,1)*1e-6 for n in range(N+1): ocp_solver.set(n, 'x', x_traj[n,:]) for n in range(N): ocp_solver.set(n, 'u', u_traj[n]) # solve status = ocp_solver.solve() if status != 0:
def acados_settings_kin(Tf, N, modelparams):
# create render arguments
ocp = AcadosOcp()
#load model
model, constraints = kinematic_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 = 4
#ocp.dims.nsbx = 4
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.zl = 1000 * npy.ones((ocp.dims.ns,))
ocp.cost.Zu = 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.001*npy.ones(ocp.dims.nsh)
ocp.constraints.idxsh = npy.array([0])
#ocp.constraints.Jsh = 1
# boxconstraints
# change these later
ocp.constraints.lbx = npy.array([model.vx_min, model.theta_min, model.d_min, model.delta_min])
ocp.constraints.ubx = npy.array([model.vx_max, model.theta_max, model.d_max, model.delta_max])
ocp.constraints.idxbx = npy.array([3,4,5,6])
#ocp.constraints.lsbx= npy.zeros(ocp.dims.nbx)
#ocp.constraints.usbx= npy.zeros(ocp.dims.nbx)
#ocp.constraints.idxsbx= npy.array([3,4,5,6])
#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.05
ocp.solver_options.nlp_solver_max_iter = 100
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_ocp2.json")
print("solver created returning to main")
return constraints, model, acados_solver, ocp
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)
