def test_penalty_minimize_contact_forces(penalty_origin, value):
ocp = prepare_test_ocp(with_contact=True)
x = [DM.ones((8, 1)) * value]
u = [DM.ones((4, 1)) * value]
penalty_type = penalty_origin.MINIMIZE_CONTACT_FORCES
penalty = Objective(penalty_type)
penalty_type.value[0](penalty, PenaltyNodes(ocp, ocp.nlp[0], [], x, u, []))
if isinstance(penalty_type, (ObjectiveFcn.Lagrange, ObjectiveFcn.Mayer)):
res = ocp.nlp[0].J[0][0]["val"]
else:
res = ocp.nlp[0].g[0][0]["val"]
if value == 0.1:
np.testing.assert_almost_equal(
res,
np.array([[-9.6680105, 127.2360329, 5.0905995]]).T,
)
else:
np.testing.assert_almost_equal(
res,
np.array([[25.6627161, 462.7973306, -94.0182191]]).T,
)
if isinstance(penalty_type, ConstraintFcn):
np.testing.assert_almost_equal(ocp.nlp[0].g[0][0]["bounds"].min,
np.array([[0.0]]).T)
np.testing.assert_almost_equal(ocp.nlp[0].g[0][0]["bounds"].max,
np.array([[0.0]]).T)
def test_tau_max_from_actuators(value, threshold):
ocp = prepare_test_ocp(with_actuator=True)
t = [0]
x = [DM.zeros((6, 1)), DM.zeros((6, 1))]
u = [DM.ones((3, 1)) * value, DM.ones((3, 1)) * value]
penalty_type = ConstraintFcn.TORQUE_MAX_FROM_Q_AND_QDOT
penalty = Constraint(penalty_type, min_torque=threshold)
if threshold and threshold < 0:
with pytest.raises(
ValueError,
match="min_torque cannot be negative in tau_max_from_actuators"
):
get_penalty_value(ocp, penalty, t, x, u, [])
return
else:
res = get_penalty_value(ocp, penalty, t, x, u, [])
if threshold:
np.testing.assert_almost_equal(
res,
np.repeat([value + threshold, value - threshold], 3)[:,
np.newaxis])
else:
np.testing.assert_almost_equal(
res,
np.repeat([value + 5, value - 10], 3)[:, np.newaxis])
def test_penalty_non_slipping(value):
ocp = prepare_test_ocp(with_contact=True)
t = [0]
x = [DM.ones((8, 1)) * value]
u = [DM.ones((4, 1)) * value]
penalty_type = ConstraintFcn.NON_SLIPPING
penalty = Constraint(penalty_type)
penalty_type.value[0](
penalty,
PenaltyNodes(ocp, ocp.nlp[0], t, x, u, []),
tangential_component_idx=0,
normal_component_idx=1,
static_friction_coefficient=2,
)
res = []
for i in range(len(ocp.nlp[0].g[0])):
res.append(ocp.nlp[0].g[0][i]["val"])
if value == 0.1:
expected = [[64662.56185612, 64849.5027121], [0, 0], [np.inf, np.inf]]
elif value == -10:
expected = [[856066.90177734, 857384.05177395], [0, 0],
[np.inf, np.inf]]
else:
raise RuntimeError("Test not ready")
np.testing.assert_almost_equal(
np.concatenate(res)[:, 0], np.array(expected[0]))
if isinstance(penalty_type, ConstraintFcn):
np.testing.assert_almost_equal(ocp.nlp[0].g[0][0]["bounds"].min,
np.array([expected[1]]).T)
np.testing.assert_almost_equal(ocp.nlp[0].g[0][0]["bounds"].max,
np.array([expected[2]]).T)
def _sample_parameters(self):
n_samples = self.n_samples
n_uncertain = self.n_uncertain
mean = vertcat(self.socp.p_unc_mean,
self.socp.uncertain_initial_conditions_mean)
if self.socp.n_p_unc > 0 and self.socp.n_uncertain_initial_condition > 0:
covariance = diagcat(self.socp.p_unc_cov,
self.socp.uncertain_initial_conditions_cov)
elif self.socp.n_p_unc > 0:
covariance = self.socp.p_unc_cov
elif self.socp.n_uncertain_initial_condition > 0:
covariance = self.socp.uncertain_initial_conditions_cov
else:
raise ValueError("No uncertainties found n_p_unc = {}, "
"n_uncertain_initial_condition={}".format(
self.socp.n_p_unc,
self.socp.n_uncertain_initial_condition))
dist = self.socp.p_unc_dist + self.socp.uncertain_initial_conditions_distribution
for d in dist:
if not d == 'normal':
raise NotImplementedError(
'Distribution "{}" not implemented, only "normal" is available.'
.format(d))
sampled_epsilon = sample_parameter_normal_distribution_with_sobol(
DM.zeros(mean.shape), DM.eye(covariance.shape[0]), n_samples)
sampled_parameters = SX.zeros(n_uncertain, n_samples)
for s in range(n_samples):
sampled_parameters[:, s] = mean + mtimes(sampled_epsilon[:, s].T,
chol(covariance)).T
return sampled_parameters
def test_blockdiag(self):
# Test blockdiag with DM
correct_res = DM([[1, 1, 0, 0, 0], [1, 1, 0, 0, 0], [0, 0, 1, 1, 1],
[0, 0, 1, 1, 1], [0, 0, 1, 1, 1]])
a = DM.ones(2, 2)
b = DM.ones(3, 3)
res = blockdiag(a, b)
self.assertTrue(is_equal(res, correct_res))
# MX and DM mix
a = MX.sym('a', 2, 2)
b = DM.ones(1, 1)
correct_res = MX.zeros(3, 3)
correct_res[:2, :2] = a
correct_res[2:, 2:] = b
res = blockdiag(a, b)
self.assertTrue(is_equal(res, correct_res, 30))
# SX and DM mix
a = SX.sym('a', 2, 2)
b = DM.ones(1, 1)
correct_res = SX.zeros(3, 3)
correct_res[:2, :2] = a
correct_res[2:, 2:] = b
res = blockdiag(a, b)
self.assertTrue(is_equal(res, correct_res, 30))
# SX and MX
a = SX.sym('a', 2, 2)
b = MX.sym('b', 2, 2)
self.assertRaises(ValueError, blockdiag, a, b)
def test_include_inequality_ub_wrong_size(self):
ub = -DM(range(1, 5))
lb = DM(range(5, 8))
aop = AbstractOptimizationProblem()
x = aop.create_variable('x', 2)
g = x[0] - x[1]
self.assertRaises(ValueError, aop.include_inequality, g, lb=lb, ub=ub)
def test_penalty_non_slipping(value):
ocp = prepare_test_ocp(with_contact=True)
x = [DM.ones((8, 1)) * value]
u = [DM.ones((4, 1)) * value]
penalty_type = ConstraintFcn.NON_SLIPPING
penalty = Constraint(penalty_type)
penalty_type.value[0](
penalty,
PenaltyNodes(ocp, ocp.nlp[0], [], x, u, []),
tangential_component_idx=0,
normal_component_idx=1,
static_friction_coefficient=2,
)
res = []
for i in range(len(ocp.nlp[0].g[0])):
res.append(ocp.nlp[0].g[0][i]["val"])
if value == 0.1:
expected = [[264.1400764, 244.8040553], 0, np.inf]
elif value == -10:
expected = [[899.9319451, 951.2573773], 0, np.inf]
np.testing.assert_almost_equal(res, np.array(expected[0]))
if isinstance(penalty_type, ConstraintFcn):
np.testing.assert_almost_equal(ocp.nlp[0].g[0][0]["bounds"].min,
np.array([[expected[1]]]))
np.testing.assert_almost_equal(ocp.nlp[0].g[0][0]["bounds"].max,
np.array([[expected[2]]]))
def test_tau_max_from_actuators(value, threshold):
ocp = prepare_test_ocp(with_actuator=True)
x = [DM.zeros((6, 1)), DM.zeros((6, 1))]
u = [DM.ones((3, 1)) * value, DM.ones((3, 1)) * value]
penalty_type = ConstraintFcn.TORQUE_MAX_FROM_ACTUATORS
penalty = Constraint(penalty_type)
if threshold and threshold < 0:
with pytest.raises(
ValueError,
match="min_torque cannot be negative in tau_max_from_actuators"
):
penalty_type.value[0](penalty,
PenaltyNodes(ocp, ocp.nlp[0], [], x, u, []),
min_torque=threshold),
else:
penalty_type.value[0](penalty,
PenaltyNodes(ocp, ocp.nlp[0], [], x, u, []),
min_torque=threshold)
val = []
for i in range(len(ocp.nlp[0].g[0])):
val.append(ocp.nlp[0].g[0][i]["val"])
for res in val:
if threshold:
np.testing.assert_almost_equal(
res,
np.repeat([value + threshold, value - threshold],
3)[:, np.newaxis])
else:
np.testing.assert_almost_equal(
res,
np.repeat([value + 5, value - 10], 3)[:, np.newaxis])
def _fix_types(self):
"""
Transform attributes in casadi types.
"""
self.x_max = vertcat(self.x_max)
self.y_max = vertcat(self.y_max)
self.u_max = vertcat(self.u_max)
self.delta_u_max = vertcat(self.delta_u_max)
self.x_min = vertcat(self.x_min)
self.y_min = vertcat(self.y_min)
self.u_min = vertcat(self.u_min)
self.delta_u_min = vertcat(self.delta_u_min)
self.h_final = vertcat(self.h_final)
self.h_initial = vertcat(self.h_initial)
self.h = vertcat(self.h)
self.g_eq = vertcat(self.g_eq)
self.g_ineq = vertcat(self.g_ineq)
self.x_0 = vertcat(self.x_0)
if self.y_guess is not None:
self.y_guess = vertcat(self.y_guess)
if self.u_guess is not None:
self.u_guess = vertcat(self.u_guess)
if isinstance(self.L, (int, float)):
self.L = DM(self.L)
if isinstance(self.V, (int, float)):
self.V = DM(self.V)
if isinstance(self.S, (int, float)):
self.S = DM(self.S)
def sample_parameter_normal_distribution_with_sobol(mean,
covariance,
n_samples=1):
"""Sample parameter using Sobol sampling with a normal distribution.
:param mean:
:param covariance:
:param n_samples:
:return:
"""
if isinstance(mean, list):
mean = vertcat(*mean)
n_uncertain = mean.size1()
# Uncertain parameter design
sobol_design = sobol_seq.i4_sobol_generate(n_uncertain, n_samples,
math.ceil(np.log2(n_samples)))
sobol_samples = DM(sobol_design.T)
for i in range(n_uncertain):
sobol_samples[i, :] = norm(loc=0., scale=1).ppf(sobol_samples[i, :])
unscaled_sample = DM.zeros(n_uncertain, n_samples)
for i in range(n_samples):
unscaled_sample[:, i] = mean + mtimes(
chol(covariance).T, sobol_samples[:, i])
return unscaled_sample
def __init__(self, name='dataset', **kwargs):
"""
Generic time dependent data storage.
The data is stored in the self.data dictionary.
self.data['entry_name']['time'] is a row vector
self.data['entry_name']['values'] is a matrix with the same number of columns as the time vector, and
rows equal to self.data['entry_name']['size'].
The data can be more easily managed using create_entry, get_entry, insert_data.
:param str name: name of th dataset
:param str plot_style: default plot style. plot = linear interpolation, step = piecewise constant
('plot' | 'step')
:param bool find_discontinuity: Default: True. If True, it will try to find discontinuity on the data, and plot
with gaps where data is missing/not available, instead of a line connecting all data points.
:param float max_sampling_time: maximum expected distance between two time data. This is used to detect
discontinuity on the data, and plot it separately.
"""
self.name = name
self.data = defaultdict(
partial(dict, [('time', DM([])), ('names', None),
('values', DM([])), ('size', None)]))
self.plot_style = 'step'
self.max_delta_t = None
self.find_discontinuity = True
for (k, val) in kwargs.items():
setattr(self, k, val)
def set_theta_as_optimization_theta(self,
new_theta_opt,
new_theta_opt_min=None,
new_theta_opt_max=None):
if new_theta_opt_min is None:
new_theta_opt_min = -DM.inf(new_theta_opt.numel())
if new_theta_opt_max is None:
new_theta_opt_max = DM.inf(new_theta_opt.numel())
new_theta_opt = vertcat(new_theta_opt)
new_theta_opt_min = vertcat(new_theta_opt_min)
new_theta_opt_max = vertcat(new_theta_opt_max)
if not new_theta_opt.numel() == new_theta_opt_max.numel():
raise ValueError(
'Size of "new_theta_opt" and "new_theta_opt_max" differ. new_theta_opt.numel()={} '
'and new_theta_opt_max.numel()={}'.format(
new_theta_opt.numel(), new_theta_opt_max.numel()))
if not new_theta_opt.numel() == new_theta_opt_min.numel():
raise ValueError(
'Size of "new_theta_opt" and "new_theta_opt_max" differ. new_theta_opt.numel()={} '
'and new_theta_opt_min.numel()={}'.format(
new_theta_opt.numel(), new_theta_opt_min.numel()))
self.theta_opt = vertcat(self.theta_opt, new_theta_opt)
self.theta_opt_min = vertcat(self.theta_opt_min, new_theta_opt_min)
self.theta_opt_max = vertcat(self.theta_opt_max, new_theta_opt_max)
return new_theta_opt
def include_algebraic(self,
var,
alg=None,
y_min=None,
y_max=None,
y_guess=None):
if y_min is None:
y_min = -DM.inf(var.numel())
if y_max is None:
y_max = DM.inf(var.numel())
if isinstance(y_min, list):
y_min = vertcat(*y_min)
if isinstance(y_max, list):
y_max = vertcat(*y_max)
if not var.numel() == y_min.numel():
raise ValueError(
"Given 'var' and 'y_min' does not have the same size, {}!={}".
format(var.numel(), y_min.numel()))
if not var.numel() == y_max.numel():
raise ValueError(
"Given 'var' and 'y_max' does not have the same size, {}!={}".
format(var.numel(), y_max.numel()))
if self.y_guess is not None:
if y_guess is None:
self.y_guess = vertcat(self.y_guess, DM.zeros(var.numel()))
else:
self.y_guess = vertcat(self.y_guess, y_guess)
self.model.include_algebraic(var, alg)
self.y_min = vertcat(self.y_min, y_min)
self.y_max = vertcat(self.y_max, y_max)
def test_include_variable_with_bounds(self):
lb = -DM(range(1, 4))
ub = DM(range(5, 8))
aop = AbstractOptimizationProblem()
x = MX.sym('x', 3)
aop.include_variable(x, lb=lb, ub=ub)
self.assertTrue(is_equal(aop.x_lb, lb))
self.assertTrue(is_equal(aop.x_ub, ub))
def test_include_variable_lb_wrong_size(self):
ub = -DM(range(1, 4))
lb = DM(range(5, 10))
aop = AbstractOptimizationProblem()
self.assertRaises(ValueError,
aop.create_variable,
name='x',
size=3,
lb=lb,
ub=ub)
def __init__(self, **kwargs):
SystemModel.__init__(self, **kwargs)
a = DM([[-1, -2], [5, -1]])
b = DM([[1, 0], [0, 1]])
x = self.create_state("x", 2)
u = self.create_control("u", 2)
self.include_equations(ode=mtimes(a, x) + mtimes(b, u))
def __init__(self, model, **kwargs):
OptimalControlProblem.__init__(self,
model,
name=model.name + "_stabilization",
obj={
"Q": DM.eye(2),
"R": DM.eye(2)
},
x_0=[1, 1],
**kwargs)
def __init__(self, name='optimization_problem', **kwargs):
r"""
Abstract Optimization Problem class
Optimization problem
.. math::
\\min_x f(x, p)
\\textrm{s.t.:} g_{lb} \leq g(x,p) \leq g_{ub}
Object attributes:
x -> optimization variables
g -> constraint
:param str name: Optimization problem name
:param dict kwargs:
"""
self.name = name
self.f = DM([])
self.g = DM([])
self.x = DM([])
self.p = DM([])
self.g_lb = DM([])
self.g_ub = DM([])
self.x_lb = DM([])
self.x_ub = DM([])
self.solver_options = {}
self._solver = None
for (key, val) in kwargs.items():
setattr(self, key, val)
def test_include_equations_ode_multi_dim(empty_model):
x = empty_model.create_state("x", 2)
u = empty_model.create_control("u", 2)
a = DM([[-1, -2], [5, -1]])
b = DM([[1, 0], [0, 1]])
ode = (mtimes(a, x) + mtimes(b, u))
empty_model.include_equations(ode=ode)
assert empty_model.ode.shape == (2, 1)
assert is_equal(empty_model.ode, ode)
def include_optimization_parameter(self,
var,
p_opt_min=None,
p_opt_max=None):
if p_opt_min is None:
p_opt_min = -DM.inf(var.numel())
if p_opt_max is None:
p_opt_max = DM.inf(var.numel())
self.model.include_parameter(var)
self.set_parameter_as_optimization_parameter(var, p_opt_min, p_opt_max)
def create_siso():
a = DM([-1])
b = DM([1])
model = _create_linear_system(n_x=1, n_u=1, a=a, b=b, name="SISO")
problem = OptimalControlProblem(model,
obj={
"Q": DM.eye(1),
"R": DM.eye(1)
},
x_0=[1])
return model, problem
def create_2x2_mimo():
a = DM([[-1, -2], [5, -1]])
b = DM([[1, 0], [0, 1]])
model = _create_linear_system(n_x=2, n_u=2, a=a, b=b, name="MIMO_2x2")
problem = OptimalControlProblem(model,
obj={
"Q": DM.eye(2),
"R": DM.eye(2)
},
x_0=[1, 1])
return model, problem
def get_measurement(self):
"""Return the plant measurement of a simulated model and advance time by 't_s'.
Return the measurement time, the measurement [x; y], and the controls.
:rtype: tuple
:return: (timestamp, measuremnt, control)
"""
# perform the simulation
if self.has_noise:
v_rand = DM(
numpy.random.multivariate_normal([0] * self.r_v.shape[0],
self.r_v))
else:
v_rand = 0
# Simulation (Try to do the simulation with disturbance, if not able to use the disturbance, try again without)
sim_result = self.model.simulate(
x_0=self.x,
t_0=self.t,
t_f=self.t + self.t_s,
y_0=self.y_guess,
u=self.u,
p=self.p,
theta=self.theta,
integrator_options=self.integrator_options)
x, y, u = sim_result.final_condition()
if self.has_noise:
x = x + v_rand
self.t += self.t_s
self.x = x
measurement_wo_noise = mtimes(self.c_matrix, vertcat(x, y))
if self.has_noise:
n_rand = DM(
numpy.random.multivariate_normal([0] * self.r_n.shape[0],
self.r_n))
measurement = measurement_wo_noise + n_rand
else:
measurement = measurement_wo_noise
self.dataset.insert_data('x', self.t, x)
self.dataset.insert_data('y', self.t, y)
self.dataset.insert_data('u', self.t, u)
self.dataset.insert_data('meas', self.t, measurement)
self.dataset.insert_data('meas_wo_noise', self.t, measurement_wo_noise)
if self.verbosity >= 1:
print('Real state: {}'.format(x))
return self.t, measurement, self.u
def test_penalty_contact_force_inequality(penalty_origin, value):
ocp = prepare_test_ocp(with_contact=True)
t = [0]
x = [DM.ones((8, 1)) * value]
u = [DM.ones((4, 1)) * value]
penalty_type = penalty_origin.TRACK_CONTACT_FORCES
penalty = Constraint(penalty_type, contact_index=0)
res = get_penalty_value(ocp, penalty, t, x, u, [])
expected = [[-9.6680105, 127.2360329, 5.0905995]
] if value == 0.1 else [[25.6627161, 462.7973306, -94.0182191]]
np.testing.assert_almost_equal(res.T, expected)
def include_optimization_theta(self,
var,
theta_opt_min=None,
theta_opt_max=None):
if theta_opt_min is None:
theta_opt_min = -DM.inf(var.numel())
if theta_opt_max is None:
theta_opt_max = DM.inf(var.numel())
self.model.include_theta(var)
self.set_theta_as_optimization_theta(var,
new_theta_opt_min=theta_opt_min,
new_theta_opt_max=theta_opt_max)
def setUp(self):
self.model, self.problem = create_2x2_mimo()
self.obj_tol = 1e-4
self.obj_value = 0 # DM(0.131427)
self.answer_obj_value = {
'direct_pw_continuous': 1.03103,
'direct_polynomial': 1.03068
}
self.answer_initial_states = DM([1, 1, 1.3557, 0.705653])
self.answer_final_states = DM([-0.272, -0.312316, 0, 0])
self.nlpsol_opts = {'ipopt.print_level': 0, 'print_time': False}
def test_expm(self):
# Test for eye
correct_res = diag(exp(DM.ones(3)))
a = expm(DM.eye(3))
self.assertAlmostEqual(norm_fro(a - correct_res), 0, 3)
# Test for -magic(3) (compared with MATLAB solution)
a = DM([[-8, -1, -6], [-3, -5, -7], [-4, -9, -2]])
correct_res = DM(
[[3.646628887990924, 32.404567030885005, -36.051195612973601],
[5.022261973341555, 44.720086474306093, -49.742348141745325],
[-8.668890555430160, -77.124653199288772, 85.793544060621244]])
self.assertAlmostEqual(norm_fro(expm(a) - correct_res), 0, 2)
def test_penalty_contact_force_inequality(penalty_origin, value, direction):
ocp = prepare_test_ocp(with_contact=True)
x = [DM.ones((8, 1)) * value]
u = [DM.ones((4, 1)) * value]
if direction == "GREATER_THAN":
min_bound = 1
max_bound = np.inf
if value == 0.1:
expected = [-9.6680105, 1.0, np.inf]
elif value == -10:
expected = [25.6627161, 1.0, np.inf]
else:
raise RuntimeError("Wrong test")
elif direction == "LESSER_THAN":
min_bound = -np.inf
max_bound = 1
if value == 0.1:
expected = [-9.6680105, -np.inf, 1.0]
elif value == -10:
expected = [25.6627161, -np.inf, 1.0]
else:
raise RuntimeError("Wrong test")
else:
raise RuntimeError("Wrong test")
penalty_type = penalty_origin.CONTACT_FORCE
penalty = ConstraintOption(penalty_type,
min_bound=min_bound,
max_bound=max_bound)
penalty_type.value[0](
penalty,
ocp,
ocp.nlp[0],
[],
x,
u,
[],
contact_force_idx=0,
)
res = ocp.nlp[0].g[0][0]
np.testing.assert_almost_equal(res, np.array([[expected[0]]]))
if isinstance(penalty_type, Constraint):
np.testing.assert_almost_equal(ocp.nlp[0].g_bounds[0][0].min,
np.array([[expected[1]]]))
np.testing.assert_almost_equal(ocp.nlp[0].g_bounds[0][0].max,
np.array([[expected[2]]]))
def include_state(self,
var,
ode=None,
x_0=None,
x_min=None,
x_max=None,
h_initial=None,
x_0_sym=None,
suppress=False):
if x_0 is not None:
x_0 = vertcat(x_0)
if x_min is None:
x_min = -DM.inf(var.numel())
if x_max is None:
x_max = DM.inf(var.numel())
if x_0 is None and h_initial is None and not suppress:
raise Exception('No initial condition given')
var = vertcat(var)
x_min = vertcat(x_min)
x_max = vertcat(x_max)
if not var.numel() == x_max.numel():
raise ValueError('Size of "x" and "x_max" differ. x.numel()={} '
'and x_max.numel()={}'.format(
var.numel(), x_max.numel()))
if not var.numel() == x_min.numel():
raise ValueError('Size of "x" and "x_min" differ. x.numel()={} '
'and x_min.numel()={}'.format(
var.numel(), x_min.numel()))
if not var.numel() == x_min.numel():
raise ValueError('Size of "x" and "x_0" differ. x.numel()={} '
'and x_0.numel()={}'.format(
var.numel(), x_0.numel()))
x_0_sym = self.model.include_state(var, ode, x_0_sym)
if x_0 is not None:
self.x_0 = vertcat(self.x_0, x_0)
h_initial = x_0_sym - var
else:
x_0 = DM.zeros(var.shape)
self.x_0 = vertcat(self.x_0, x_0)
if h_initial is not None:
self.h_initial = vertcat(self.h_initial, h_initial)
self.x_min = vertcat(self.x_min, x_min)
self.x_max = vertcat(self.x_max, x_max)
def test_penalty_non_slipping(value):
ocp = prepare_test_ocp(with_contact=True)
t = [0]
x = [DM.ones((8, 1)) * value]
u = [DM.ones((4, 1)) * value]
penalty_type = ConstraintFcn.NON_SLIPPING
penalty = Constraint(penalty_type,
tangential_component_idx=0,
normal_component_idx=1,
static_friction_coefficient=2)
res = get_penalty_value(ocp, penalty, t, x, u, [])
expected = [[64662.56185612, 64849.5027121]
] if value == 0.1 else [[856066.90177734, 857384.05177395]]
np.testing.assert_almost_equal(res.T, expected)