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_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_param(self):
ocp = Ocp(T=1)
x = ocp.state()
u = ocp.control()
p = ocp.parameter()
ocp.set_der(x, u)
ocp.subject_to(u <= 1)
ocp.subject_to(-1 <= u)
ocp.add_objective(ocp.at_tf(x))
ocp.subject_to(ocp.at_t0(x) == p)
ocp.solver('ipopt')
ocp.method(MultipleShooting())
ocp.set_value(p, 0)
sol = ocp.solve()
ts, xs = sol.sample(x, grid='control')
self.assertAlmostEqual(xs[0], 0)
ocp.set_value(p, 1)
sol = ocp.solve()
ts, xs = sol.sample(x, grid='control')
self.assertAlmostEqual(xs[0], 1)
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 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_der(self):
T = 1
M = 1
b = 1
t0 = 0
x0 = 0
ocp = Ocp(t0=t0, T=T)
x = ocp.state()
u = ocp.control()
ocp.set_der(x, u)
y = 2 * x
ocp.subject_to(ocp.der(y) <= 2 * b)
ocp.subject_to(-2 * b <= ocp.der(y))
ocp.add_objective(ocp.at_tf(x))
ocp.subject_to(ocp.at_t0(x) == x0)
ocp.solver('ipopt')
ocp.method(MultipleShooting(N=4, M=M, intg='rk'))
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)
self.assertAlmostEqual(ts[0], t0)
self.assertAlmostEqual(ts[-1], t0 + T)
def create_bouncing_ball_stage(ocp):
"""
Create a bouncing ball stage.
This function creates a stage of a bouncing ball that bounces no higher
than 5 meters above the ground.
Returns
-------
stage : :obj:`~rockit.stage.Stage`
An ocp stage describing the bouncing ball
p : :obj:`~casadi.MX`
position variable
v : :obj:`~casadi.MX`
velocity variable
"""
stage = ocp.stage(t0=FreeTime(0), T=FreeTime(1))
p = stage.state()
v = stage.state()
stage.set_der(p, v)
stage.set_der(v, -9.81)
stage.subject_to(stage.at_t0(v) >= 0)
stage.subject_to(p >= 0)
stage.method(MultipleShooting(N=1, M=20, intg='rk'))
return stage, p, v
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_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_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_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_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_grid_inf_subject_to(self):
ocp, x1, x2, u = vdp(MultipleShooting(N=10))
sol = ocp.solve()
x1sol = sol.sample(x1, grid='integrator', refine=100)[1]
self.assertFalse(np.all(x1sol > -0.25))
for method in [MultipleShooting, DirectCollocation]:
margins = [np.inf]
for M in [1, 2, 4]:
ocp, x1, x2, u = vdp(method(N=10, M=M), grid='inf')
sol = ocp.solve()
x1sol = sol.sample(x1, grid='integrator', refine=100)[1]
margin = np.min(x1sol - (-0.25))
self.assertTrue(np.all(margin > 0))
self.assertTrue(np.all(margin < 0.01))
# Assert that margin shrinks for increasing M
self.assertTrue(margin < 0.5 * margins[-1])
margins.append(margin)
self.assertTrue(margin)
def test_grid_integrator_subject_to(self):
ocp, x1, x2, u = vdp(MultipleShooting(N=10))
sol = ocp.solve()
x1sol = sol.sample(x1, grid='integrator', refine=100)[1]
self.assertFalse(np.all(x1sol > -0.25))
for method, grids in [(MultipleShooting, ['integrator']),
(DirectCollocation,
['integrator', 'integrator_roots'])]:
for grid in grids:
margins = [np.inf]
for M in [1, 2, 4]:
ocp, x1, x2, u = vdp(method(N=10, M=M), grid=grid)
sol = ocp.solve()
x1sol = sol.sample(x1, grid=grid)[1]
x1solf = sol.sample(x1, grid='integrator', refine=100)[1]
margin = np.min(x1sol - (-0.25))
marginf = np.min(x1solf - (-0.25))
self.assertTrue(marginf < 1e-5)
assert_array_almost_equal(margin, 0, decimal=8)
def test_grid_integrator(self):
N, T, u_max, x0 = 10, 10, 2, 1
tolerance = 1e-6
ocp, x, u = integrator_control_problem(T, u_max, x0, MultipleShooting(N=N,M=3,intg='rk'))
sol = ocp.solve()
ts, xs = sol.sample(x, grid='integrator')
ts, us = sol.sample(u, grid='integrator')
ts, uxs = sol.sample(u * x, grid='integrator')
t_exact = np.linspace(0, T, N * 3 + 1)
x_exact = np.linspace(1, x0 - 10 * u_max, N * 3 + 1)
u_exact = np.ones(N * 3 + 1) * (-u_max)
assert_allclose(ts, t_exact, atol=tolerance)
assert_allclose(xs, x_exact, atol=tolerance)
assert_allclose(us, u_exact, atol=tolerance)
assert_allclose(uxs, u_exact * x_exact, atol=tolerance)
tsa, tsb = sol.sample(ocp.t, grid='integrator')
assert_allclose(tsa, tsb, atol=tolerance)
def integrator_control_problem(T=1, u_max=1, x0=0, stage_method=None, t0=0):
if stage_method is None:
stage_method = MultipleShooting()
ocp = Ocp(t0=t0, T=T)
x = ocp.state()
u = ocp.control()
ocp.set_der(x, u)
ocp.subject_to(u <= u_max)
ocp.subject_to(-u_max <= u)
ocp.add_objective(ocp.at_tf(x))
if x0 is not None:
ocp.subject_to(ocp.at_t0(x) == x0)
ocp.solver('ipopt')
ocp.method(stage_method)
return (ocp, x, u)
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)
u = stage.control() # Thrust stage.set_der(p, v) stage.set_der(v, (u - 0.05 * v * v) / m) stage.set_der(m, -0.1 * u * u) # Regularize the control stage.add_objective(stage.integral(u**2)) # Path constraints stage.subject_to(u >= 0) stage.subject_to(u <= 0.5) # Initial constraints stage.subject_to(stage.at_t0(p) == 0) stage.subject_to(stage.at_t0(v) == 0) stage.subject_to(stage.at_t0(m) == 1) # Final constraints stage.subject_to(stage.at_tf(p) == 10) stage.subject_to(stage.at_tf(v) == 0) ocp.method(DirectMethod(solver='ipopt')) stage.method(MultipleShooting(N=50, M=2, intg='rk')) # Missing: a nonzero initial guess for m sol = ocp.solve()
# 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)
data = {'x': x, 'p': u, 'ode': model_rhs}
model_sim = integrator('xkp1', 'cvodes', data, opts).mapaccum('simulator', N)
# Run ILC algorithm
u_prev_val = np.zeros(N)
def test_stage_cloning_t0_T(self):
for t0_stage, t0_sol_stage in [(None, 0), (-1, -1),
(FreeTime(-1), -1)]:
for T_stage, T_sol_stage in [(None, 2), (2, 2), (FreeTime(1), 2)]:
kwargs = {}
if t0_stage is not None:
kwargs["t0"] = t0_stage
if T_stage is not None:
kwargs["T"] = T_stage
stage = Stage(**kwargs)
p = stage.state()
v = stage.state()
u = stage.control()
stage.set_der(p, v)
stage.set_der(v, u)
stage.subject_to(u <= 1)
stage.subject_to(-1 <= u)
stage.add_objective(stage.tf)
stage.subject_to(stage.at_t0(p) == 0)
stage.subject_to(stage.at_t0(v) == 0)
stage.subject_to(stage.at_tf(p) == 1)
stage.subject_to(stage.at_tf(v) == 0)
stage.method(MultipleShooting(N=2))
for t0, t0_sol in ([] if t0_stage is None else [
(None, t0_sol_stage)
]) + [(-1, -1), (FreeTime(-1), -1)]:
for T, T_sol in ([] if T_stage is None else [
(None, T_sol_stage)
]) + [(2, 2), (FreeTime(1), 2)]:
ocp = Ocp()
kwargs = {}
if t0 is not None:
kwargs["t0"] = t0
if T is not None:
kwargs["T"] = T
mystage = ocp.stage(stage, **kwargs)
if mystage.is_free_starttime():
ocp.subject_to(mystage.t0 >= t0_sol)
ocp.solver('ipopt')
sol = ocp.solve()
tolerance = 1e-6
ts, ps = sol(mystage).sample(p,
grid='integrator',
refine=10)
ps_ref = np.hstack(
((0.5 * np.linspace(0, 1, 10 + 1)**2)[:-1],
np.linspace(0.5, 1.5, 10 + 1) -
0.5 * np.linspace(0, 1, 10 + 1)**2))
np.testing.assert_allclose(ps, ps_ref, atol=tolerance)
ts_ref = t0_sol + np.linspace(0, 2, 10 * 2 + 1)
ts, vs = sol(mystage).sample(v,
grid='integrator',
refine=10)
np.testing.assert_allclose(ts, ts_ref, atol=tolerance)
vs_ref = np.hstack((np.linspace(0, 1, 10 + 1)[:-1],
np.linspace(1, 0, 10 + 1)))
np.testing.assert_allclose(vs, vs_ref, atol=tolerance)
u_ref = np.array([1.0] * 10 + [-1.0] * 11)
ts, us = sol(mystage).sample(u,
grid='integrator',
refine=10)
np.testing.assert_allclose(us, u_ref, atol=tolerance)
from rockit import Ocp, DirectMethod, MultipleShooting, FreeTime
import matplotlib.pyplot as plt
ocp = Ocp()
stage = ocp.stage(t0=FreeTime(0), T=FreeTime(1))
p = stage.state()
v = stage.state()
stage.set_der(p, v)
stage.set_der(v, -9.81)
stage.subject_to(stage.at_t0(v) >= 0)
stage.subject_to(p >= 0)
stage.method(MultipleShooting(N=1, M=20, intg='rk'))
stage.subject_to(stage.at_t0(p) == 0)
ocp.subject_to(stage.t0 == 0)
stage_prev = stage
n_bounce = 2
for i in range(n_bounce):
ocp.subject_to(stage.at_tf(p) == 0)
stage = ocp.stage(stage_prev)
ocp.subject_to(stage.at_t0(v) == -0.9 * stage_prev.at_tf(v))
ocp.subject_to(stage.t0 == stage_prev.tf)
stage_prev = stage
