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)