I created a minimal functional example. This time the model is a spring mass damper system. The current way this is set up is that the OcpSolver model has no damping, and no control effort (Fmax is 0) but the ocp_sim model has a model that has some damping.
When I simulate it I only see the output as per the undamped version not the damped version which should be what is simulated.
This is example is all that you need to run and view the issue. Hope it helps!
from acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver, AcadosSimSolver
from casadi import SX, vertcat
import numpy as np
import scipy.linalg
import matplotlib.pyplot as plt
def export_spring_mass_damper_model() -> AcadosModel:
model_name='spring_mass_damper'
g=9.81
#Parameters
M = SX.sym('M') #Mass
D = SX.sym('D') #damping constant
K = SX.sym('K') #spring constant
mu= SX.sym('mu') #Coefficient of Friction
p = vertcat(M, D, K, mu) #Parameter Vector
x = SX.sym('x') #mass position
v = SX.sym('v') #mass velocity
x_state = vertcat(x, v)
xdot = SX.sym('xdot')
vdot = SX.sym('vdot')
xdot_state =vertcat(xdot, vdot)
#Controls
F = SX.sym('F')
u = vertcat(F)
#Dynamics
f_explicit = vertcat(
v,
(F-D*v -K*x)/M
)
f_implicit = xdot_state - f_explicit
model = AcadosModel()
model.f_expl_expr = f_explicit
model.f_impl_expr = f_implicit
model.x = x_state
model.xdot = xdot_state
model.u = u
model.p = p
model.name=model_name
return model
def setOCP(ocp, x0, N_horizon=None, Tf=None, FMax=None):
nx = ocp.model.x.size()[0]
nu = ocp.model.u.size()[0]
print(f'nu: {nu:d}')
ny = nx + nu
ny_e = nx
ocp.dims.N = N_horizon
ocp.cost.cost_type = 'NONLINEAR_LS'
ocp.cost.cost_type_e = 'NONLINEAR_LS'
Q_mat = 2.*np.diag(np.ones((nx)))
R_mat = 2e-2*np.diag(np.ones((nu)))
ocp.cost.W = scipy.linalg.block_diag(Q_mat, R_mat)
ocp.cost.W_e = Q_mat
ocp.model.cost_y_expr = vertcat(ocp.model.x, ocp.model.u)
ocp.model.cost_y_expr_e = ocp.model.x
ocp.cost.yref = np.zeros((ny, ))
ocp.cost.yref_e = np.zeros((ny_e, ))
#control constraints
ocp.constraints.lbu = np.array([-FMax])
ocp.constraints.ubu = np.array([FMax])
ocp.constraints.idxbu = np.array([0])
ocp.constraints.x0 = x0
ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'
ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
ocp.solver_options.sim_method_newton_iter = 10
ocp.solver_options.qp_solver_iter_max=50
ocp.solver_options.nlp_solver_type = 'SQP'
ocp.solver_options.qp_solver_cond_N = N_horizon
ocp.solver_options.tf = Tf
return ocp
def setup(x0, N_horizon, Tf, FMax):
ocp = AcadosOcp()
model = export_spring_mass_damper_model()
ocp.model=model
ocp = setOCP(ocp, x0, N_horizon, Tf, FMax)
M=1.0
D=0.0
K=0.5
mu=0.01 #Coefficient of Friction
ocp.parameter_values = np.array([M,D, K, mu])
ocp_sim=AcadosOcp()
model_sim = export_spring_mass_damper_model()
ocp_sim.model=model_sim
ocp_sim = setOCP(ocp_sim, x0, N_horizon, Tf, FMax)
mu_sim = 0.01
ocp_sim.parameter_values = np.array([M,D+0.1, K, mu_sim])
acados_ocp_solver = AcadosOcpSolver(ocp)
acados_integrator = AcadosSimSolver(ocp_sim)
return acados_ocp_solver, acados_integrator
def main():
FMax=0.0
ubu=np.array([FMax])
x0=np.array([0.3, 0.0])
Tf=2.0
N_Horizon=20
ocp_solver, integrator = setup(x0, N_Horizon, Tf, FMax)
nx = ocp_solver.acados_ocp.dims.nx
nu = ocp_solver.acados_ocp.dims.nu
Nsim=300
simX = np.ndarray((Nsim+1, nx))
simU = np.ndarray((Nsim, nu))
simRef = np.ndarray((Nsim, nx+nu))
simX[0, :] =x0
t = np.zeros((Nsim))
#do som initial iterations to start with a good guess
num_iter_initial=5
for _ in range(num_iter_initial):
ocp_solver.solve_for_x0(x0_bar = x0)
time=0.
for i in range(Nsim):
time =i*(Tf/N_Horizon)
#Get and Set Reference
'''
yref=genCircularTrajectory(time, N=N_Horizon, tF=Tf)
for j in range(N_Horizon):
ocp_solver.set(j, "yref", yref[j,:])
yref_e = yref[N_Horizon, :-4]
ocp_solver.set(N_Horizon, "yref", yref_e)
'''
simRef[i, :] =0
#Solve Control Solution
simU[i, :] = ocp_solver.solve_for_x0(x0_bar = simX[i, :])
t[i] = ocp_solver.get_stats('time_tot')
simX[i+1, :] = integrator.simulate(x=simX[i, :], u=simU[i,:])
t*= 1000
print(f'Computation time in ms: min {np.min(t):.3f} median {np.median(t):.3f} max {np.max(t):.3f}')
plot_system(np.linspace(0, (Tf/N_Horizon)*Nsim, Nsim+1),ubu , simX, simU, simRef)
def plot_system(shooting_nodes, u_max, X_true, U=None, Y=None, X_est=None, Y_measured=None, latexify=False, plt_show=True, X_true_label=None):
"""
Params:
shooting_nodes: time values of the discretization
u_max: maximum absolute value of u
U: arrray with shape (N_sim-1, nu) or (N_sim, nu)
X_true: arrray with shape (N_sim, nx)
X_est: arrray with shape (N_sim-N_mhe, nx)
Y_measured: array with shape (N_sim, ny)
latexify: latex style plots
"""
if latexify:
latexify_plot()
WITH_ESTIMATION = X_est is not None and Y_measured is not None
N_sim = X_true.shape[0]
nx = X_true.shape[1]
Tf = shooting_nodes[N_sim-1]
t = shooting_nodes
Ts = t[1] - t[0]
if WITH_ESTIMATION:
N_mhe = N_sim - X_est.shape[0]
t_mhe = np.linspace(N_mhe * Ts, Tf, N_sim-N_mhe)
if U is not None:
nu = U.shape[1]
input_labels=['$F$']
for i in range(nu):
plt.subplot(nu+nx+1, 1, i+1)
line, = plt.step(t[:-1], U[:,i])
line.set_color('r')
plt.ylabel(input_labels[i])
plt.xlabel('$t$')
plt.hlines(u_max[i], t[0], t[-1], linestyles='dashed', alpha=0.7)
plt.hlines(-u_max[i], t[0], t[-1], linestyles='dashed', alpha=0.7)
plt.ylim([-1.2*u_max[i], 1.2*u_max[i]])
plt.xlim(t[0], t[-1])
plt.grid()
states_lables = ['$x$', '$v$']
assert(len(states_lables) == nx)
for i in range(nx):
if U is not None:
plt.subplot(nx+nu+1, 1, nu+i+1)
else:
plt.subplot(nx+1, 1, i+1)
line, = plt.plot(t, X_true[:, i], label='true')
if X_true_label is not None:
line.set_label(X_true_label)
plt.plot(t[1:], Y[:, i], 'g', label='ref')
if WITH_ESTIMATION:
plt.plot(t_mhe, X_est[:, i], '--', label='estimated')
plt.plot(t, Y_measured[:, i], 'x', label='measured')
plt.ylabel(states_lables[i])
plt.xlabel('$t$')
plt.grid()
plt.legend(loc=1)
plt.xlim(t[0], t[-1])
plt.subplots_adjust(left=None, bottom=None, right=None, top=None, hspace=0.4)
if plt_show:
plt.show()
if __name__ == "__main__":
main()