def test_integral(self):
t0 = 1.2
T = 5.7
ocp = Ocp(t0=t0, T=T)
x = ocp.state()
u = ocp.control()
ocp.set_der(x, u)
ocp.subject_to(ocp.at_t0(x) == 0)
ocp.subject_to(u <= 1)
f = ocp.integral(x * ocp.t)
ocp.add_objective(-f) # (t-t0)*t -> t^3/3-t^2/2*t0
ocp.solver('ipopt')
opts = {"abstol": 1e-8, "reltol": 1e-8, "quad_err_con": True}
for method in [
MultipleShooting(intg='rk'),
MultipleShooting(intg='cvodes', intg_options=opts),
#MultipleShooting(intg='idas',intg_options=opts),
DirectCollocation()
]:
ocp.method(method)
sol = ocp.solve()
ts, xs = sol.sample(f, grid='control')
I = lambda t: t**3 / 3 - t**2 / 2 * t0
x_ref = I(t0 + T) - I(t0)
assert_array_almost_equal(xs[-1], x_ref)
def test_variables(self):
N = 10
ocp = Ocp(t0=2 * pi, T=10)
p = ocp.parameter(grid='control')
v = ocp.variable(grid='control')
x = ocp.state()
ocp.set_der(x, 0)
ocp.subject_to(ocp.at_t0(x) == 0)
ts = linspace(0, 10, N)
ocp.add_objective(ocp.integral(sin(v - p)**2, grid='control'))
ocp.method(MultipleShooting(N=N))
ocp.solver('ipopt')
ocp.set_value(p, ts)
ocp.set_initial(v, ts)
sol = ocp.solve()
_, xs = sol.sample(v, grid='control')
assert_array_almost_equal(xs[:-1], ts)
ocp.set_initial(v, 0.1 + 2 * pi + ts)
sol = ocp.solve()
_, xs = sol.sample(v, grid='control')
assert_array_almost_equal(xs[:-1], 2 * pi + ts)
ocp.set_initial(v, 0.1 + ocp.t)
sol = ocp.solve()
_, xs = sol.sample(v, grid='control')
assert_array_almost_equal(xs[:-1], 2 * pi + ts)
ocp.set_initial(v, 0.1 + 2 * pi)
sol = ocp.solve()
_, xs = sol.sample(v, grid='control')
with self.assertRaises(AssertionError):
assert_array_almost_equal(xs[:-1], 2 * pi + ts)
def vdp(method,grid='control'):
ocp = Ocp(T=10)
# Define 2 states
x1 = ocp.state()
x2 = ocp.state()
# Define 1 control
u = ocp.control(order=0)
# Specify ODE
ocp.set_der(x1, (1 - x2**2) * x1 - x2 + u)
ocp.set_der(x2, x1)
# Lagrange objective
ocp.add_objective(ocp.integral(x1**2 + x2**2 + u**2))
# Path constraints
ocp.subject_to(-1 <= (u <= 1))
ocp.subject_to(x1 >= -0.25, grid=grid)
# Initial constraints
ocp.subject_to(ocp.at_t0(x1) == 0)
ocp.subject_to(ocp.at_t0(x2) == 1)
# Pick an NLP solver backend
ocp.solver('ipopt')
# Pick a solution method
ocp.method(method)
return (ocp, x1, x2, u)
def test_dae_casadi(self):
# cross check with dae_colloation
xref = 0.1 # chariot reference
l = 1. #- -> crane, + -> pendulum
m = 1.
M = 1.
g = 9.81
ocp = Ocp(T=5)
x = ocp.state()
y = ocp.state()
w = ocp.state()
dx = ocp.state()
dy = ocp.state()
dw = ocp.state()
xa = ocp.algebraic()
u = ocp.control()
ocp.set_der(x, dx)
ocp.set_der(y, dy)
ocp.set_der(w, dw)
ddx = (w - x) * xa / m
ddy = g - y * xa / m
ddw = ((x - w) * xa - u) / M
ocp.set_der(dx, ddx)
ocp.set_der(dy, ddy)
ocp.set_der(dw, ddw)
ocp.add_alg((x - w) * (ddx - ddw) + y * ddy + dy * dy + (dx - dw)**2)
ocp.add_objective(
ocp.at_tf((x - xref) * (x - xref) + (w - xref) * (w - xref) +
dx * dx + dy * dy))
ocp.add_objective(
ocp.integral((x - xref) * (x - xref) + (w - xref) * (w - xref)))
ocp.subject_to(-2 <= (u <= 2))
ocp.subject_to(ocp.at_t0(x) == 0)
ocp.subject_to(ocp.at_t0(y) == l)
ocp.subject_to(ocp.at_t0(w) == 0)
ocp.subject_to(ocp.at_t0(dx) == 0)
ocp.subject_to(ocp.at_t0(dy) == 0)
ocp.subject_to(ocp.at_t0(dw) == 0)
#ocp.subject_to(xa>=0,grid='integrator_roots')
ocp.set_initial(y, l)
ocp.set_initial(xa, 9.81)
# Pick an NLP solver backend
# NOTE: default scaling strategy of MUMPS leads to a singular matrix error
ocp.solver(
'ipopt', {
"ipopt.linear_solver": "mumps",
"ipopt.mumps_scaling": 0,
"ipopt.tol": 1e-12
})
# Pick a solution method
method = DirectCollocation(N=50)
ocp.method(method)
# Solve
sol = ocp.solve()
assert_array_almost_equal(
sol.sample(xa, grid='integrator', refine=1)[1][0],
9.81011622448889)
assert_array_almost_equal(
sol.sample(xa, grid='integrator', refine=1)[1][1],
9.865726317147214)
assert_array_almost_equal(
sol.sample(xa, grid='integrator')[1][0], 9.81011622448889)
assert_array_almost_equal(
sol.sample(xa, grid='integrator')[1][1], 9.865726317147214)
assert_array_almost_equal(
sol.sample(xa, grid='control')[1][0], 9.81011622448889)
assert_array_almost_equal(
sol.sample(xa, grid='control')[1][1], 9.865726317147214)
# Set initial conditions
ocp.subject_to(ocp.at_t0(x) == x0)
# Define reference
y_ref = ocp.parameter(grid='control')
ocp.set_value(y_ref, y_ref_val)
# Define output correction
beta = ocp.parameter(grid='control')
# Define previous control
u_prev = ocp.parameter(grid='control')
# Set ILC objective
ocp.add_objective(
ocp.integral((y_ref - beta - x[0])**2, grid='control') +
1e-3 * ocp.integral((u - u_prev)**2, grid='control'))
# Pick a solution method
ocp.solver('ipopt')
# Pick a solution method
ocp.method(MultipleShooting(N=N, M=4, intg='rk'))
# Define simulator for plant and model
plant_rhs = pendulum_ode(x, u, plant_param)
opts = {'tf': T / N}
data = {'x': x, 'p': u, 'ode': plant_rhs}
plant_sim = integrator('xkp1', 'cvodes', data, opts).mapaccum('simulator', N)
})
obs_spline_r = interpolant('r', 'bspline', [tunnel_s1], 1, {
"algorithm": "smooth_linear",
"smooth_linear_frac": 0.49
})
ocp.subject_to(x > 0)
ocp.subject_to((x - obs_spline_x(s_obs, bx))**2 +
(y - obs_spline_y(s_obs, by))**2 -
(obs_spline_r(s_obs, br))**2 < 0)
# -------------------------------------- Objective function
ocp.add_objective(1 * ocp.integral(
(x - end_goal_x)**2 +
(y - end_goal_y)**2)) # integral = cause of extra state in acados
# ----------------- Solver
options = {"ipopt": {"print_level": 5}}
options["expand"] = False
options["print_time"] = True
ocp.solver('ipopt', options)
# qp_solvers = ('PARTIAL_CONDENSING_HPIPM', 'FULL_CONDENSING_QPOASES', 'FULL_CONDENSING_HPIPM', 'PARTIAL_CONDENSING_QPDUNES', 'PARTIAL_CONDENSING_OSQP')
# integrator_types = ('ERK', 'IRK', 'GNSF', 'DISCRETE')
# SOLVER_TYPE_values = ['SQP', 'SQP_RTI']
# HESS_APPROX_values = ['GAUSS_NEWTON', 'EXACT']
# REGULARIZATION_values = ['NO_REGULARIZE', 'MIRROR', 'PROJECT', 'PROJECT_REDUC_HESS', 'CONVEXIFY']
# ocp.set_initial(x, np.zeros(N+1))
# ocp.set_initial(y, np.zeros(N+1))
# ocp.set_initial(theta, np.zeros(N+1))
# ocp.set_initial(v , np.zeros(N+1))
# ocp.set_initial(w , np.zeros(N+1))
#----------- end point constraint
ocp.subject_to(ocp.at_tf(x) == 1)
# -------------------------------------- Objective function
ocp.add_objective(
1 * ocp.integral((x)**2 +
(y)**2)) # integral = cause of extra state in acados
# ----------------- Solver
options = {"ipopt": {"print_level": 5}}
options["expand"] = False
options["print_time"] = True
ocp.solver('ipopt', options)
method = external_method('acados',
N=N,
intg='rk',
qp_solver='FULL_CONDENSING_HPIPM',
expand=False,
nlp_solver_max_iter=1000,
hessian_approx='EXACT',