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_collocation_equivalence(self):
for problem in [vdp, vdp_dae]:
ocp, x1, x2, u = problem(
MultipleShooting(N=6,
intg='collocation',
intg_options={
"number_of_finite_elements": 1,
"interpolation_order": 4
}))
ocp.solver("ipopt", {"ipopt.tol": 1e-12})
sol = ocp.solve()
x1_a = sol.sample(x1, grid='control')[1]
x2_a = sol.sample(x2, grid='control')[1]
u_a = sol.sample(u, grid='control')[1]
ocp, x1, x2, u = problem(DirectCollocation(N=6))
ocp.solver("ipopt", {"ipopt.tol": 1e-12})
sol = ocp.solve()
x1_b = sol.sample(x1, grid='control')[1]
x2_b = sol.sample(x2, grid='control')[1]
u_b = sol.sample(u, grid='control')[1]
assert_array_almost_equal(x1_a, x1_b, decimal=12)
assert_array_almost_equal(x2_a, x2_b, decimal=12)
assert_array_almost_equal(u_a, u_b, decimal=12)
def test_time_dep_ode(self):
t0 = 1.2
T = 5.7
ocp = Ocp(t0=t0, T=5.7)
x = ocp.state()
ocp.set_der(x, ocp.t**2)
ocp.subject_to(ocp.at_t0(x) == 0)
tf = t0 + T
x_ref = tf**3 / 3 - t0**3 / 3
ocp.solver('ipopt')
opts = {"abstol": 1e-9, "reltol": 1e-9}
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(x, grid='control')
x_ref = ts**3 / 3 - t0**3 / 3
assert_array_almost_equal(xs, x_ref)
def test_show_infeasibilities(self):
for method in [MultipleShooting(), DirectCollocation()]:
ocp, x, u = integrator_control_problem(stage_method=method, x0=0)
ocp.subject_to(ocp.at_t0(x) == 2)
with self.assertRaises(Exception):
sol = ocp.solve()
with StringIO() as buf, redirect_stdout(buf):
ocp.show_infeasibilities(1e-4)
out = buf.getvalue()
self.assertIn("ocp.subject_to(ocp.at_t0(x)==2)", out)
def test_all(self):
T = 1
M = 1
b = 1
t0 = 0
x0 = 0
for scheme in [MultipleShooting(N=40, M=1, intg='rk'),
DirectCollocation(N=40)]:
(ocp, x, u) = integrator_control_problem(T, b, x0, scheme, t0)
sol = ocp.solve()
ts, xs = sol.sample(x, grid='control')
self.assertAlmostEqual(xs[0],x0,places=6)
self.assertAlmostEqual(xs[-1],x0-b*T,places=6)
def test_initial(self):
for stage_method in [MultipleShooting(), DirectCollocation()]:
ocp, x, u = integrator_control_problem(x0=None,
stage_method=stage_method)
v = ocp.variable()
ocp.subject_to(ocp.at_t0(x) == v)
ocp.subject_to(0 == sin(v))
sol = ocp.solve()
ts, xs = sol.sample(x, grid='control')
self.assertAlmostEqual(xs[0], 0, places=6)
ocp.set_initial(v, 2 * pi)
sol = ocp.solve()
ts, xs = sol.sample(x, grid='control')
self.assertAlmostEqual(xs[0], 2 * pi, places=6)
def test_basic_t0_free(self):
xf = 2
t0 = 0
for T in [2]:
for x0 in [0, 1]:
for b in [1, 2]:
for method in [
MultipleShooting(N=4, intg='rk'),
MultipleShooting(N=4, intg='cvodes'),
MultipleShooting(N=4, intg='idas'),
DirectCollocation(N=4)
]:
ocp = Ocp(t0=FreeTime(2), T=T)
x = ocp.state()
u = ocp.control()
ocp.set_der(x, u)
ocp.subject_to(u <= b)
ocp.subject_to(-b <= u)
ocp.add_objective(ocp.tf)
ocp.subject_to(ocp.at_t0(x) == x0)
ocp.subject_to(ocp.at_tf(x) == xf)
ocp.subject_to(ocp.t0 >= 0)
ocp.solver('ipopt')
ocp.method(method)
sol = ocp.solve()
ts, xs = sol.sample(x, grid='control')
self.assertAlmostEqual(xs[0], x0, places=6)
self.assertAlmostEqual(xs[-1], xf, places=6)
self.assertAlmostEqual(ts[0], t0)
self.assertAlmostEqual(ts[-1], t0 + T)
def test_basic(self):
for T in [1, 3.2]:
for M in [1, 2]:
for u_max in [1, 2]:
for t0 in [0, 1]:
for x0 in [0, 1]:
for method in [
MultipleShooting(N=4, M=M, intg='rk'),
MultipleShooting(N=4, M=M, intg='cvodes'),
MultipleShooting(N=4, M=M, intg='idas'),
DirectCollocation(N=4, M=M)
]:
ocp, x, u = integrator_control_problem(
T, u_max, x0, method, t0)
sol = ocp.solve()
ts, xs = sol.sample(x, grid='control')
self.assertAlmostEqual(xs[0], x0, places=6)
self.assertAlmostEqual(xs[-1],
x0 - u_max * T,
places=6)
self.assertAlmostEqual(ts[0], t0)
self.assertAlmostEqual(ts[-1], t0 + T)
def test_intg_refine(self):
for M in [1, 2]:
for method in [DirectCollocation(N=2,M=M), MultipleShooting(N=2,M=M,intg='rk')]:
ocp, sol, p, v, u = bang_bang_problem(method)
tolerance = 1e-6
ts, ps = sol.sample(p, grid='integrator', refine=10)
ps_ref = np.hstack(((0.5*np.linspace(0,1, 10*M+1)**2)[:-1],np.linspace(0.5,1.5,10*M+1)-0.5*np.linspace(0,1, 10*M+1)**2))
assert_allclose(ps, ps_ref, atol=tolerance)
ts_ref = np.linspace(0, 2, 10*2*M+1)
assert_allclose(ts, ts_ref, atol=tolerance)
ts, vs = sol.sample(v, grid='integrator', refine=10)
assert_allclose(ts, ts_ref, atol=tolerance)
vs_ref = np.hstack((np.linspace(0,1, 10*M+1)[:-1],np.linspace(1,0, 10*M+1)))
assert_allclose(vs, vs_ref, atol=tolerance)
u_ref = np.array([1.0]*M*10+[-1.0]*(M*10+1))
ts, us = sol.sample(u, grid='integrator', refine=10)
assert_allclose(us, u_ref, atol=tolerance)
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)