# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE.; # import os, sys, json sys.path.insert(0, '../common') from acados_template import AcadosSim, AcadosSimSolver, acados_dae_model_json_dump from pendulum_model import export_pendulum_ode_model from utils import plot_pendulum import numpy as np import matplotlib.pyplot as plt sim = AcadosSim() # export model model = export_pendulum_ode_model() # set model_name sim.model = model Tf = 0.1 nx = model.x.size()[0] nu = model.u.size()[0] N = 200 # set simulation time sim.solver_options.T = Tf # set options
#################################
### MHE
#################################
Nmhe = 50
Qe = nmp.diag((2, 2))
Re = nmp.array(0.0001)
# Q0e = 10* nmp.eye(nx+2)
#x = q1 q2 dq1 dq2 b1 b2
Q0e = nmp.diag((1e0, 1e0, 2e0, 2e0, 1e3, 1e4))
acados_mhe_solver = export_pend_mhe_solver(model, Nmhe, dt, Qe, Q0e, Re)
#################################
### Dynamics Simulation
#################################
sim = AcadosSim()
sim.model = model
# set simulation time
sim.solver_options.T = dt
# set options
sim.solver_options.num_stages = 4
sim.solver_options.num_steps = 6
acados_integrator = AcadosSimSolver(sim)
acados_integrator.set("p", p)
Tsim = 40
Nsim = int(Tsim / dt)
simX = nmp.zeros((Nsim + 1, nx + 2))
simXref = nmp.zeros((Nsim + 1, 2))
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE.; # from acados_template import AcadosSim, AcadosSimSolver from export_sine_wave_mhe_ode_model import export_sine_wave_mhe_ode_model from export_sine_wave_mhe_solver import export_sine_wave_mhe_solver from utils import * from casadi import * import numpy as nmp import csv import matplotlib.pyplot as plt sinFunction = lambda x, tVec : x[0] + x[1]*sin(2*pi*x[2]*tVec + x[3]) + x[4]*tVec sim = AcadosSim() # export model model = export_sine_wave_mhe_ode_model() dt = 0.01 Tf = 2.0 N = int(Tf/dt) sim.model = model t = 0 sim.parameter_values = np.array([t]) # set simulation time sim.solver_options.T = dt # set options sim.solver_options.integrator_type = 'ERK' sim.solver_options.num_stages = 4
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE.; # from acados_template import AcadosSim, AcadosSimSolver from export_so_ode_model_continous_and_discrete import export_so_ode_ct, export_so_ode_dt_rk4 from utils import * from casadi import * import numpy as nmp import matplotlib.pyplot as plt dt = 0.05 Tf = 10.0 N = int(Tf / dt) sim_ct = AcadosSim() sim_dt = AcadosSim() # export model model_ct = export_so_ode_ct() zeta = 0.5 #Under Damped ts = 0.125 #Time Constant Kp = 1.0 #Steady State Gain inputDelay = 0.553 model_dt = export_so_ode_dt_rk4(dt, inputDelay) sim_ct.model = model_ct sim_dt.model = model_dt sim_ct.parameter_values = np.array([zeta, ts, Kp]) sim_dt.parameter_values = np.array(
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'
ocp.solver_options.hessian_approx = 'EXACT'
ocp.solver_options.exact_hess_constr = 0
ocp.solver_options.exact_hess_dyn = 0
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)
ocp_solver.reset()
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(f'acados returned status {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(f'acados returned status {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()
################################# ### MHE ################################# Nmhe = 50 Qe = nmp.diag((1, 1)) Re = nmp.array(0.0001) # Q0e = 10* nmp.eye(nx+2) #x = q1 q2 dq1 dq2 b1 b2 Q0e = nmp.diag((1e0, 1e0, 2e0, 2e0, 1e3, 1e4)) acados_mhe_solver = export_pend_mhe_solver(mhe_model, Nmhe, dt, Qe, Q0e, Re) ################################# ### Dynamics Simulation ################################# sim = AcadosSim() sim.model = sim_model sim.parameter_values = nmp.array([m1, m2, l1, l2, lc1, lc2, g]) # set simulation time sim.solver_options.T = dt # set options sim.solver_options.num_stages = 4 sim.solver_options.num_steps = 6 acados_integrator = AcadosSimSolver(sim) Tsim = 10 Nsim = int(Tsim / dt) simX = nmp.zeros((Nsim + 1, nx + 2)) simXref = nmp.zeros((Nsim + 1, 2))
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE.; # from acados_template import AcadosSim, AcadosSimSolver from export_quad_ode_model import export_quad_ode_model from utils import * import numpy as np import matplotlib.pyplot as plt sim = AcadosSim() # export model model = export_quad_ode_model() #Parameters rho = 1.225 A = 0.1 Cl = 0.125 Cd = 0.075 m = 10.0 g = 9.81 J3 = 0.25 J2 = J3 * 4 J1 = J3 * 4 p = np.array([rho, A, Cl, Cd, m, g, J1, J2, J3])