def find_algebraic_variable(self,
x,
u,
guess=None,
t=0.0,
p=None,
theta_value=None,
rootfinder_options=None):
if guess is None:
guess = [1] * self.n_y
if rootfinder_options is None:
rootfinder_options = dict(
nlpsol="ipopt",
nlpsol_options=config.SOLVER_OPTIONS["nlpsol_options"])
if p is None:
p = []
if theta_value is None:
theta_value = []
# replace known variables
alg = self.alg
known_var = vertcat(self.t, self.x, self.u, self.p, self.theta)
known_var_values = vertcat(t, x, u, p, theta_value)
alg = substitute(alg, known_var, known_var_values)
f_alg = Function("f_alg", [self.y], [alg])
rf = rootfinder("rf_algebraic_variable", "nlpsol", f_alg,
rootfinder_options)
res = rf(guess)
return res
def test_ball_euclidean_xc_inside(self):
ball = og.constraints.Ball2([1, 2], 1)
x = [1.2, 1.55]
x_sym = cs.SX.sym("x", 2)
d_num = ball.distance_squared(x)
d_sym = cs.substitute(ball.distance_squared(x_sym), x_sym, x)
self.assertAlmostEqual(d_sym, d_num, 8, "computation of distance")
correct_squared_distance = 0.0
self.assertAlmostEqual(d_sym, correct_squared_distance, 8,
"expected squared distance")
def test_ball_euclidean_origin_inside(self):
ball = og.constraints.Ball2(None, 1)
x = np.array([0.2, 0.8])
x_sym = cs.SX.sym("x", 2)
d_num = ball.distance_squared(x)
d_sym = float(cs.substitute(ball.distance_squared(x_sym), x_sym, x))
self.assertAlmostEqual(d_sym, d_num, 8, "computation of distance")
correct_squared_distance = 0.0
self.assertAlmostEqual(d_sym, correct_squared_distance, 8,
"expected squared distance")
def test_ball_inf_xc(self):
ball = og.constraints.BallInf([-1, -1], 0.5)
x = np.array([1, -2])
x_sym = cs.SX.sym("x", 2)
d_num = ball.distance_squared(x)
d_sym = float(cs.substitute(ball.distance_squared(x_sym), x_sym, x))
self.assertAlmostEqual(d_sym, d_num, 8, "computation of distance")
correct_squared_distance = 2.5
self.assertAlmostEqual(d_num, correct_squared_distance, 8,
"expected squared distance")
def test_ball_inf_origin(self):
ball = og.constraints.BallInf(None, 1)
x = np.array([3, 2])
x_sym = cs.SX.sym("x", 2)
d_num = ball.distance_squared(x)
d_sym = float(cs.substitute(ball.distance_squared(x_sym), x_sym, x))
self.assertAlmostEqual(d_sym, d_num, 8, "computation of distance")
correct_squared_distance = 5.0
self.assertAlmostEqual(d_num, correct_squared_distance, 8,
"expected squared distance")
# verify that it works with cs.MX
x_sym_mx = cs.MX.sym("xmx", 2)
sqdist_mx = ball.distance_squared(x_sym_mx)
sqdist_mx_fun = cs.Function('sqd', [x_sym_mx], [sqdist_mx])
self.assertAlmostEqual(correct_squared_distance,
sqdist_mx_fun(x)[0], 5)
def replace_variable(self, original, replacement):
if isinstance(original, list):
original = vertcat(*original)
if isinstance(replacement, list):
replacement = vertcat(*replacement)
if not original.numel() == replacement.numel():
raise ValueError(
"Original and replacement must have the same number of elements!"
"original.numel()={}, replacement.numel()={}".format(
original.numel(), replacement.numel()))
if callable(getattr(super(), 'replace_variable', None)):
super().replace_variable(original, replacement)
self.alg = substitute(self.alg, original, replacement)
def test_rectangle_simple(self):
rect = og.constraints.Rectangle([-1, -2], [4, -1])
# some basic assertions
self.assertListEqual([0, 1], rect.idx_bound_finite_all())
self.assertTrue(len(rect.idx_infinite_only_xmax()) == 0)
self.assertTrue(len(rect.idx_infinite_only_xmin()) == 0)
self.assertEqual(2, rect.dimension())
# squared distance
self.assertAlmostEqual(1, rect.distance_squared([3, 0]), 8)
self.assertAlmostEqual(4, rect.distance_squared([0, 1]), 8)
self.assertAlmostEqual(1, rect.distance_squared([5, -1.5]), 8)
self.assertAlmostEqual(5, rect.distance_squared([5, 1]), 8)
# symbolic
x_sym = cs.SX.sym("x", 2)
d_sym = float(
cs.substitute(rect.distance_squared(x_sym), x_sym, [5, 1]))
self.assertAlmostEqual(5, d_sym, 8)
