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_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 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_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 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 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)
shifted_midpoints_y = np.linspace(0, global_end_goal_y, NB)
shifted_radii = np.ones(NB)
tlength1 = len(shifted_midpoints_x)
tunnel_s1 = np.linspace(0, 1, tlength1)
bx = ocp.parameter(NB)
by = ocp.parameter(NB)
br = ocp.parameter(NB)
ocp.set_value(bx, shifted_midpoints_x)
ocp.set_value(by, shifted_midpoints_y)
ocp.set_value(br, shifted_radii)
ocp.subject_to(
ocp.at_tf(s_obs) <= 1
) # effect unclear === the path given is long = first ocp iteration will not get there if dt is small
obs_spline_x = interpolant('x', 'bspline', [tunnel_s1], 1, {
"algorithm": "smooth_linear",
"smooth_linear_frac": 0.49
})
obs_spline_y = interpolant('y', 'bspline', [tunnel_s1], 1, {
"algorithm": "smooth_linear",
"smooth_linear_frac": 0.49
})
obs_spline_r = interpolant('r', 'bspline', [tunnel_s1], 1, {
"algorithm": "smooth_linear",
"smooth_linear_frac": 0.49
})
ocp.subject_to(0 <= (v <= 1))
ocp.subject_to(-pi <= (w <= pi))
#-------- initial guesss
# 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',
tlength1 = len(shifted_midpoints_x[0:N])
tunnel_s1 = np.linspace(0,1,tlength1)
bx = ocp.parameter(N)
by = ocp.parameter(N)
br = ocp.parameter(N)
ocp.set_value(bx, shifted_midpoints_x[0:N])
ocp.set_value(by, shifted_midpoints_y[0:N])
# ocp.set_value(br, shifted_radii[0:N])
ocp.set_value(br, 100*np.ones(N))
ocp.subject_to(ocp.at_tf(s_obs) <= 1) # effect unclear === the path given is long = first ocp iteration will not get there if dt is small
obs_spline_x = interpolant('x','bspline',[tunnel_s1], 1 , {"algorithm": "smooth_linear","smooth_linear_frac":0.49})
obs_spline_y = interpolant('y','bspline',[tunnel_s1], 1 , {"algorithm": "smooth_linear","smooth_linear_frac":0.49})
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
#--------------- Bubbles with s
bubbles_x = ocp.parameter(N+1)
bubbles_y = ocp.parameter(N+1)
bubbles_radii = ocp.parameter(N+1)
ocp.set_value(bubbles_x, shifted_midpoints_x)
ocp.set_value(bubbles_y, shifted_midpoints_y)
ocp.set_value(bubbles_radii, shifted_radii)
tlength1 = len(shifted_midpoints_x)
tunnel_s1 = np.linspace(0,1,tlength1)
ocp.subject_to(ocp.at_tf(s_obs) <= 1)
obs_spline_x = interpolant('x','bspline',[tunnel_s1], 1 , {"algorithm": "smooth_linear","smooth_linear_frac":0.49})
obs_spline_y = interpolant('y','bspline',[tunnel_s1], 1 , {"algorithm": "smooth_linear","smooth_linear_frac":0.49})
obs_spline_r = interpolant('r','bspline',[tunnel_s1], 1 , {"algorithm": "smooth_linear","smooth_linear_frac":0.49})
ocp.subject_to( ( ( ( x - obs_spline_x(s_obs,bubbles_x) )**2 + ( y-obs_spline_y(s_obs,bubbles_y) )**2 ) - (obs_spline_r(s_obs,bubbles_radii)**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