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 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_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)
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_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)
from rockit import Ocp, DirectMethod, MultipleShooting ocp = Ocp() # Rocket example stage = ocp.stage(t0=0, T=1) # Omitting means variable p = stage.state() # Position v = stage.state() # Velocity m = stage.control() # Mass 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)
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 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 pendulum_simulator(x0, u, sim):
xsim = horzcat(x0, sim.call({'x0': x0, 'p': u})['xf'])
return {'xsim': xsim, 'ysim': np.squeeze(xsim[0, :-1])}
#Define problem parameter
plant_param = {'m': 1, 'I': 1, 'L': 1, 'c': 1}
model_param = {'m': 1.2, 'I': 1.2, 'L': 0.9, 'c': 0.9}
N = 100
T = 10
tgrid = np.linspace(0, T, N)
y_ref_val = np.sin(tgrid)
x0 = [0, 0]
ocp = Ocp(t0=tgrid[0], T=tgrid[-1])
# Define states
x = ocp.state(2)
# Define controls
u = ocp.control()
# Specify ODE
model_rhs = pendulum_ode(x, u, model_param)
ocp.set_der(x, model_rhs)
# Set initial conditions
ocp.subject_to(ocp.at_t0(x) == x0)
# Define reference
def bang_bang_problem(stage_method):
ocp = Ocp(T=FreeTime(1))
p = ocp.state()
v = ocp.state()
u = ocp.control()
ocp.set_der(p, v)
ocp.set_der(v, u)
ocp.subject_to(u <= 1)
ocp.subject_to(-1 <= u)
ocp.add_objective(ocp.T)
ocp.subject_to(ocp.at_t0(p) == 0)
ocp.subject_to(ocp.at_t0(v) == 0)
ocp.subject_to(ocp.at_tf(p) == 1)
ocp.subject_to(ocp.at_tf(v) == 0)
ocp.solver('ipopt')
ocp.method(stage_method)
return (ocp, ocp.solve(), p, v, u)
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)
global_end_goal_x = 1 global_end_goal_y = 1 initial_pos_x = 0 initial_pos_y = 0 xlim_min = -1 xlim_max = 5 ylim_min = -2 ylim_max = 6 N = 20 dt = 0.5 #effect unclear = how to connect this to rosrate ? #------------- Initialize OCP ocp = Ocp(T=N * dt) #----------------------------------- waypoints ------------------------ #------------------------- System model x = ocp.state() y = ocp.state() theta = ocp.state() v = ocp.control() w = ocp.control() #--------------------------path parameters s_obs = ocp.state()
global_end_goal_x = 1 global_end_goal_y = 1 initial_pos_x = 0 initial_pos_y = 0 xlim_min = -1 xlim_max = 5 ylim_min = -2 ylim_max = 6 N = 20 dt = 0.5 #effect unclear = how to connect this to rosrate ? #------------- Initialize OCP ocp = Ocp(T=N * dt) #------------------------- System model x = ocp.state() y = ocp.state() theta = ocp.state() v = ocp.control() w = ocp.control() #-----------------------------ODEs ocp.set_der(x, v * cos(theta)) ocp.set_der(y, v * sin(theta)) ocp.set_der(theta, w)
# License along with CasADi; if not, write to the Free Software # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA # # """ Bouncing ball example (using for loop) ====================================== In this example, we want to shoot a ball from the ground up so that after 2 bounces, it will reach the height of 0.5 meter. """ 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)
def test_basic_time_free(self):
xf = 2
for t0 in [0, 1]:
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=t0, T=FreeTime(1))
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.T)
ocp.subject_to(ocp.at_t0(x) == x0)
ocp.subject_to(ocp.at_tf(x) == xf)
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 + (xf - x0) / b)
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)
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
ocp = Ocp()
# Shoot up the ball
stage1, p1, v1 = create_bouncing_ball_stage(ocp)
ocp.subject_to(stage1.t0 == 0) # Stage starts at time 0
ocp.subject_to(stage1.at_t0(p1) == 0)
ocp.subject_to(stage1.at_tf(p1) == 0)
# After bounce 1
stage2, p2, v2 = create_bouncing_ball_stage(ocp)
ocp.subject_to(stage2.at_t0(v2) == -0.9 * stage1.at_tf(v1))
ocp.subject_to(stage2.at_t0(p2) == stage1.at_tf(p1))
ocp.subject_to(stage2.t0 == stage1.tf)
ocp.subject_to(stage2.at_tf(p2) == 0)
# After bounce 2
Nsim = 3 N = 20 dt = 0.5 NB = N NP = N #------------- Initialize OCP ocp = Ocp(T = N*dt) #----------------------- waypoints ------ waypoints_x = [1,2,3] waypoints_y = [1,2,3] #---------------- Initialize path #N+1 goal points global_path_x = np.linspace(0,3,N+1) global_path_y = np.linspace(0,3,N+1) #---------------------- bubbles
