def test_penalty_track_markers_with_nan(penalty_origin, value):
ocp = prepare_test_ocp()
penalty_type = penalty_origin.TRACK_MARKERS
target = np.ones((3, 7, 11)) * value
target[:, -2, [0, -1]] = np.nan
if isinstance(penalty_type, ObjectiveFcn.Lagrange):
penalty = Objective(penalty_type, node=Node.ALL, target=target)
X = ocp.nlp[0].X[0]
elif isinstance(penalty_type, ObjectiveFcn.Mayer):
penalty = Objective(penalty_type,
node=Node.END,
target=target[:, :, -1:])
X = ocp.nlp[0].X[10]
else:
raise RuntimeError("Test not ready")
ocp.update_objectives(penalty)
res = Function(
"res", [X],
[IpoptInterface.finalize_objective_value(ocp.nlp[0].J[0][0])
]).expand()()["o0"]
if value == 0.1:
expected = 8.73 * ocp.nlp[0].J[0][0]["dt"]
else:
expected = 1879.25 * ocp.nlp[0].J[0][0]["dt"]
np.testing.assert_almost_equal(
np.array(res),
expected,
)
def test_penalty_custom(penalty_origin, value):
def custom(pn, mult):
my_values = DM.zeros((12, 1)) + pn.x[0] * mult
return my_values
ocp = prepare_test_ocp()
t = [0]
x = [DM.ones((12, 1)) * value]
penalty_type = penalty_origin.CUSTOM
if isinstance(penalty_type, (ObjectiveFcn.Lagrange, ObjectiveFcn.Mayer)):
penalty = Objective(penalty_type, index=0)
else:
penalty = Constraint(penalty_type, index=0)
penalty.custom_function = custom
mult = 2
penalty_type.value[0](penalty,
PenaltyNodes(ocp, ocp.nlp[0], t, x, [], []),
mult=mult)
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"]
np.testing.assert_almost_equal(res, np.array([[value * mult]] * 12))
if isinstance(penalty_type, ConstraintFcn):
np.testing.assert_almost_equal(ocp.nlp[0].g[0][0]["bounds"].min,
np.array([[0]] * 12))
np.testing.assert_almost_equal(ocp.nlp[0].g[0][0]["bounds"].max,
np.array([[0]] * 12))
def test_penalty_custom_fail(penalty_origin, value):
def custom_no_mult(ocp, nlp, t, x, u, p):
my_values = DM.zeros((12, 1)) + x[0]
return my_values
def custom_with_mult(ocp, nlp, t, x, u, p, mult):
my_values = DM.zeros((12, 1)) + x[0] * mult
return my_values
ocp = prepare_test_ocp()
x = [DM.ones((12, 1)) * value]
penalty_type = penalty_origin.CUSTOM
if isinstance(penalty_type, (ObjectiveFcn.Lagrange, ObjectiveFcn.Mayer)):
penalty = Objective(penalty_type)
else:
penalty = Constraint(penalty_type)
with pytest.raises(TypeError):
penalty.custom_function = custom_no_mult
penalty_type.value[0](penalty, ocp, ocp.nlp[0], [], x, [], [], mult=2)
with pytest.raises(TypeError):
penalty.custom_function = custom_with_mult
penalty_type.value[0](penalty, ocp, ocp.nlp[0], [], x, [], [])
with pytest.raises(TypeError):
keywords = [
"phase",
"list_index",
"name",
"type",
"params",
"node",
"quadratic",
"index",
"target",
"sliced_target",
"min_bound",
"max_bound",
"custom_function",
"weight",
]
for keyword in keywords:
exec(f"""def custom_with_keyword(ocp, nlp, t, x, u, p, {keyword}):
my_values = DM.zeros((12, 1)) + x[index]
return my_values""")
exec("""penalty.custom_function = custom_with_keyword""")
exec(
f"""penalty_type.value[0](penalty, ocp, ocp.nlp[0], [], x, [], [], {keyword}=0)"""
)
def test_penalty_track_markers(penalty_origin, value):
ocp = prepare_test_ocp()
t = [0]
x = [DM.ones((8, 1)) * value]
u = [0]
penalty_type = penalty_origin.TRACK_MARKERS
if isinstance(penalty_type, (ObjectiveFcn.Lagrange, ObjectiveFcn.Mayer)):
penalty = Objective(penalty_type, target=np.ones((3, 7, 1)) * value)
else:
penalty = Constraint(penalty_type, target=np.ones((3, 7, 1)) * value)
res = get_penalty_value(ocp, penalty, t, x, u, [])
expected = np.array([
[0.1, 0.99517075, 1.9901749, 1.0950042, 1, 2, 0.49750208],
[0, 0, 0, 0, 0, 0, 0],
[0.1, -0.9948376, -1.094671, 0.000166583, 0, 0, -0.0499167],
])
if value == -10:
expected = np.array([
[-10, -11.3830926, -12.2221642, -10.8390715, 1.0, 2.0, -0.4195358],
[0, 0, 0, 0, 0, 0, 0],
[-10, -9.7049496, -10.2489707, -10.5440211, 0, 0, -0.2720106],
])
np.testing.assert_almost_equal(res, expected)
def test_penalty_minimize_markers_velocity(penalty_origin, value):
ocp = prepare_test_ocp()
t = [0]
x = [DM.ones((8, 1)) * value]
u = [0]
penalty_type = penalty_origin.MINIMIZE_MARKERS_VELOCITY
penalty = Objective(penalty_type)
res = get_penalty_value(ocp, penalty, t, x, u, [])
if value == 0.1:
np.testing.assert_almost_equal(
res,
np.array([
[0.1, -0.00948376, -0.0194671, 0.0900167, 00, 00, -0.00499167],
[0, 0, 0, 0, 00, 00, 0],
[0.1, 0.0104829, -0.0890175, 0.000499583, 0, 0, -0.0497502],
]),
)
else:
np.testing.assert_almost_equal(
res,
np.array([
[-10, -12.9505, -7.51029, -4.55979, 00, 00, 2.72011],
[0, 0, 0, 0, 00, 00, 0],
[-10, -23.8309, -32.2216, -18.3907, 0, 0, -4.19536],
]),
decimal=4,
)
def test_penalty_track_markers_velocity(penalty_origin, value):
ocp = prepare_test_ocp()
t = [0]
x = [DM.ones((8, 1)) * value]
u = [0]
penalty_type = penalty_origin.TRACK_MARKERS_VELOCITY
if isinstance(penalty_type, (ObjectiveFcn.Lagrange, ObjectiveFcn.Mayer)):
penalty = Objective(penalty_type, target=np.ones((3, 7, 1)) * value)
else:
penalty = Constraint(penalty_type, target=np.ones((3, 7, 1)) * value)
res = get_penalty_value(ocp, penalty, t, x, u, [])
if value == 0.1:
np.testing.assert_almost_equal(
res,
np.array([
[0.1, -0.00948376, -0.0194671, 0.0900167, 00, 00, -0.00499167],
[0, 0, 0, 0, 00, 00, 0],
[0.1, 0.0104829, -0.0890175, 0.000499583, 0, 0, -0.0497502],
]),
)
else:
np.testing.assert_almost_equal(
res,
np.array([
[-10, -12.9505, -7.51029, -4.55979, 00, 00, 2.72011],
[0, 0, 0, 0, 00, 00, 0],
[-10, -23.8309, -32.2216, -18.3907, 0, 0, -4.19536],
]),
decimal=4,
)
def test_penalty_minimize_comddot(value, penalty_origin, implicit):
ocp = prepare_test_ocp(with_contact=True, implicit=implicit)
t = [0]
x = [DM.ones((8, 1)) * value]
u = [0]
penalty_type = penalty_origin.MINIMIZE_COM_ACCELERATION
if isinstance(penalty_type, (ObjectiveFcn.Lagrange, ObjectiveFcn.Mayer)):
penalty = Objective(penalty_type)
else:
penalty = Constraint(penalty_type)
if not implicit:
res = get_penalty_value(ocp, penalty, t, x, u, [])
expected = np.array([[0.0], [-0.7168803], [-0.0740871]])
if value == -10:
expected = np.array([[0.0], [1.455063], [16.3741091]])
np.testing.assert_almost_equal(res, expected)
else:
res = get_penalty_value(ocp, penalty, t, x, u, [])
expected = np.array([[0], [-0.0008324], [0.002668]])
if value == -10:
expected = np.array([[0], [-17.5050533], [-18.2891901]])
np.testing.assert_almost_equal(res, expected)
def test_penalty_proportional_control(penalty_origin, value):
ocp = prepare_test_ocp()
t = [0]
x = [0]
u = [DM.ones((4, 1)) * value]
penalty_type = penalty_origin.PROPORTIONAL_CONTROL
first = 0
second = 1
coef = 2
if isinstance(penalty_type, (ObjectiveFcn.Lagrange, ObjectiveFcn.Mayer)):
penalty = Objective(penalty_type,
key="tau",
first_dof=first,
second_dof=second,
coef=coef)
else:
penalty = Constraint(penalty_type,
key="tau",
first_dof=first,
second_dof=second,
coef=coef)
res = get_penalty_value(ocp, penalty, t, x, u, [])
np.testing.assert_almost_equal(res,
np.array(u[0][first] - coef * u[0][second]))
def test_penalty_track_segment_with_custom_rt(penalty_origin, value):
ocp = prepare_test_ocp()
x = [DM.ones((12, 1)) * value]
penalty_type = penalty_origin.TRACK_SEGMENT_WITH_CUSTOM_RT
if isinstance(penalty_type, (ObjectiveFcn.Lagrange, ObjectiveFcn.Mayer)):
penalty = Objective(penalty_type)
else:
penalty = Constraint(penalty_type)
penalty_type.value[0](penalty,
PenaltyNodes(ocp, ocp.nlp[0], [], x, [], []),
segment="ground",
rt_idx=0)
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"]
expected = np.array([[0], [0.1], [0]])
if value == -10:
expected = np.array([[3.1415927], [0.575222], [3.1415927]])
np.testing.assert_almost_equal(
res,
expected,
)
if isinstance(penalty_type, ConstraintFcn):
np.testing.assert_almost_equal(ocp.nlp[0].g[0][0]["bounds"].min,
np.array([[0], [0], [0]]))
np.testing.assert_almost_equal(ocp.nlp[0].g[0][0]["bounds"].max,
np.array([[0], [0], [0]]))
def test_penalty_minimize_com_position(value, penalty_origin):
ocp = prepare_test_ocp()
x = [DM.ones((12, 1)) * value]
if "TRACK_COM_POSITION" in penalty_origin._member_names_:
penalty_type = penalty_origin.TRACK_COM_POSITION
else:
penalty_type = penalty_origin.MINIMIZE_COM_POSITION
if isinstance(penalty_type, (ObjectiveFcn.Lagrange, ObjectiveFcn.Mayer)):
penalty = Objective(penalty_type)
else:
penalty = Constraint(penalty_type)
penalty_type.value[0](penalty, PenaltyNodes(ocp, ocp.nlp[0], [], x, [],
[]))
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"]
expected = np.array([[0.05], [0.05], [0.05]])
if value == -10:
expected = np.array([[-5], [0.05], [-5]])
np.testing.assert_almost_equal(res, expected)
if isinstance(penalty_type, ConstraintFcn):
np.testing.assert_almost_equal(ocp.nlp[0].g[0][0]["bounds"].min,
np.array(0))
np.testing.assert_almost_equal(ocp.nlp[0].g[0][0]["bounds"].max,
np.array(0))
def test_penalty_track_marker_with_segment_axis(penalty_origin, value):
ocp = prepare_test_ocp()
x = [DM.ones((12, 1)) * value]
penalty_type = penalty_origin.TRACK_MARKER_WITH_SEGMENT_AXIS
if isinstance(penalty_type, (ObjectiveFcn.Lagrange, ObjectiveFcn.Mayer)):
penalty = Objective(penalty_type)
else:
penalty = Constraint(penalty_type)
penalty_type.value[0](penalty,
PenaltyNodes(ocp, ocp.nlp[0], [], x, [], []),
marker="m0",
segment="ground",
axis=Axis.X)
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"]
np.testing.assert_almost_equal(
res,
np.array([[0]]),
)
if isinstance(penalty_type, ConstraintFcn):
np.testing.assert_almost_equal(ocp.nlp[0].g[0][0]["bounds"].min,
np.array([[0]]))
np.testing.assert_almost_equal(ocp.nlp[0].g[0][0]["bounds"].max,
np.array([[0]]))
def test_penalty_track_all_controls(penalty_origin, value):
ocp = prepare_test_ocp(with_muscles=True)
u = [DM.ones((12, 1)) * value]
penalty_type = penalty_origin.TRACK_ALL_CONTROLS
if isinstance(penalty_type, (ObjectiveFcn.Lagrange, ObjectiveFcn.Mayer)):
penalty = Objective(penalty_type, target=np.ones((8, 1)) * value)
else:
penalty = Constraint(penalty_type, target=np.ones((8, 1)) * value)
penalty_type.value[0](penalty, PenaltyNodes(ocp, ocp.nlp[0], [6], [], 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"]
np.testing.assert_almost_equal(
res,
np.array([[value, value, value, value, value, value, value, value]]).T,
)
if isinstance(penalty_type, ConstraintFcn):
np.testing.assert_almost_equal(
ocp.nlp[0].g[0][0]["bounds"].min,
np.array([[0.0, 0, 0, 0, 0, 0, 0, 0]]).T)
np.testing.assert_almost_equal(
ocp.nlp[0].g[0][0]["bounds"].max,
np.array([[0.0, 0, 0, 0, 0, 0, 0, 0]]).T)
def test_penalty_minimize_comddot(value, penalty_origin, implicit):
ocp = prepare_test_ocp(with_contact=True, implicit=implicit)
t = [0]
x = [DM.ones((8, 1)) * value]
u = [0]
penalty_type = penalty_origin.MINIMIZE_COM_ACCELERATION
if isinstance(penalty_type, (ObjectiveFcn.Lagrange, ObjectiveFcn.Mayer)):
penalty = Objective(penalty_type)
else:
penalty = Constraint(penalty_type)
if not implicit:
with pytest.raises(
NotImplementedError,
match=re.escape(
"MINIMIZE_COM_ACCELERATION is only working if qddot is defined as a state or a control."
),
):
res = get_penalty_value(ocp, penalty, t, x, u, [])
else:
res = get_penalty_value(ocp, penalty, t, x, u, [])
expected = np.array([[0], [-0.0008324], [0.002668]])
if value == -10:
expected = np.array([[0], [-17.5050533], [-18.2891901]])
np.testing.assert_almost_equal(res, expected)
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_penalty_track_torque(penalty_origin, value):
ocp = prepare_test_ocp()
u = [DM.ones((12, 1)) * value]
penalty_type = penalty_origin.TRACK_TORQUE
if isinstance(penalty_type, (ObjectiveFcn.Lagrange, ObjectiveFcn.Mayer)):
penalty = Objective(penalty_type, target=np.ones((4, 1)) * value)
else:
penalty = Constraint(penalty_type, target=np.ones((4, 1)) * value)
penalty_type.value[0](penalty, PenaltyNodes(ocp, ocp.nlp[0], [4], [], 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"]
np.testing.assert_almost_equal(
res,
np.array([[value, value, value, value]]).T,
)
if isinstance(penalty_type, ConstraintFcn):
np.testing.assert_almost_equal(ocp.nlp[0].g[0][0]["bounds"].min,
np.zeros((4, 1)))
np.testing.assert_almost_equal(ocp.nlp[0].g[0][0]["bounds"].max,
np.zeros((4, 1)))
def test_penalty_proportional_state(penalty_origin, value):
ocp = prepare_test_ocp()
x = [DM.ones((12, 1)) * value]
penalty_type = penalty_origin.PROPORTIONAL_STATE
if isinstance(penalty_type, (ObjectiveFcn.Lagrange, ObjectiveFcn.Mayer)):
penalty = Objective(penalty_type)
else:
penalty = Constraint(penalty_type)
penalty_type.value[0](penalty,
PenaltyNodes(ocp, ocp.nlp[0], [], x, [], []),
which_var="states",
first_dof=0,
second_dof=1,
coef=2)
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"]
np.testing.assert_almost_equal(
res,
np.array([[-value]]),
)
if isinstance(penalty_type, ConstraintFcn):
np.testing.assert_almost_equal(ocp.nlp[0].g[0][0]["bounds"].min,
np.array([[0]]))
np.testing.assert_almost_equal(ocp.nlp[0].g[0][0]["bounds"].max,
np.array([[0]]))
def test_penalty_minimize_markers_velocity(penalty_origin, value):
ocp = prepare_test_ocp()
x = [DM.ones((12, 1)) * value]
penalty_type = penalty_origin.MINIMIZE_MARKERS_VELOCITY
penalty = Objective(penalty_type)
penalty_type.value[0](penalty, PenaltyNodes(ocp, ocp.nlp[0], [], x, [],
[]))
if value == 0.1:
np.testing.assert_almost_equal(
ocp.nlp[0].J[0][6]["val"],
np.array([
[-0.00499167],
[0],
[-0.0497502],
]),
)
else:
np.testing.assert_almost_equal(
ocp.nlp[0].J[0][6]["val"],
np.array([
[2.7201056],
[0],
[-4.1953576],
]),
)
def test_penalty_track_state(penalty_origin, value):
ocp = prepare_test_ocp()
x = [DM.ones((12, 1)) * value]
penalty_type = penalty_origin.TRACK_STATE
if isinstance(penalty_type, (ObjectiveFcn.Lagrange, ObjectiveFcn.Mayer)):
penalty = Objective(penalty_type, target=np.ones((8, 1)) * value)
else:
penalty = Constraint(penalty_type, target=np.ones((8, 1)) * value)
penalty_type.value[0](penalty, PenaltyNodes(ocp, ocp.nlp[0], [1], x, [],
[]))
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"]
expected = np.array([[value]] * 8)
np.testing.assert_almost_equal(
res,
expected,
)
if isinstance(penalty_type, ConstraintFcn):
np.testing.assert_almost_equal(ocp.nlp[0].g[0][0]["bounds"].min,
np.array([[0]] * 8))
np.testing.assert_almost_equal(
ocp.nlp[0].g[0][0]["bounds"].max,
np.array([[0]] * 8),
)
def test_penalty_minimize_state(penalty_origin, value):
ocp = prepare_test_ocp()
t = [0]
x = [DM.ones((8, 1)) * value]
u = [0]
penalty = Objective(penalty_origin.MINIMIZE_STATE, key="qdot")
res = get_penalty_value(ocp, penalty, t, x, u, [])
np.testing.assert_almost_equal(res, np.array([[value]] * 4))
def prepare_ocp(biorbd_model_path, ode_solver=OdeSolver.RK):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
# Problem parameters
number_shooting_points = 30
final_time = 2
tau_min, tau_max, tau_init = -100, 100, 0
# Add objective functions
objective_functions = Objective(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE,
weight=100)
# Dynamics
dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN)
# Constraints
constraints = ConstraintList()
constraints.add(custom_func_align_markers,
node=Node.START,
first_marker_idx=0,
second_marker_idx=1)
constraints.add(custom_func_align_markers,
node=Node.END,
first_marker_idx=0,
second_marker_idx=2)
# Path constraint
x_bounds = QAndQDotBounds(biorbd_model)
x_bounds[1:6, [0, -1]] = 0
x_bounds[2, -1] = 1.57
# Initial guess
x_init = InitialGuess([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()))
# Define control path constraint
u_bounds = Bounds([tau_min] * biorbd_model.nbGeneralizedTorque(),
[tau_max] * biorbd_model.nbGeneralizedTorque())
u_init = InitialGuess([tau_init] * biorbd_model.nbGeneralizedTorque())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
)
def prepare_ocp(biorbd_model_path: str, final_time: float,
n_shooting: int) -> OptimalControlProgram:
"""
The initialization of an ocp
Parameters
----------
biorbd_model_path: str
The path to the biorbd model
final_time: float
The time in second required to perform the task
n_shooting: int
The number of shooting points to define int the direct multiple shooting program
Returns
-------
The OptimalControlProgram ready to be solved
"""
biorbd_model = biorbd.Model(biorbd_model_path)
# Add objective functions
objective_functions = Objective(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE)
# Dynamics
dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN)
# Path constraint
x_bounds = QAndQDotBounds(biorbd_model)
x_bounds[:, [0, -1]] = 0
x_bounds[1, -1] = 3.14
# Initial guess
n_q = biorbd_model.nbQ()
n_qdot = biorbd_model.nbQdot()
x_init = InitialGuess([0] * (n_q + n_qdot))
# Define control path constraint
n_tau = biorbd_model.nbGeneralizedTorque()
tau_min, tau_max, tau_init = -100, 100, 0
u_bounds = Bounds([tau_min] * n_tau, [tau_max] * n_tau)
u_bounds[n_tau - 1, :] = 0
u_init = InitialGuess([tau_init] * n_tau)
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init=x_init,
u_init=u_init,
x_bounds=x_bounds,
u_bounds=u_bounds,
objective_functions=objective_functions,
)
def test_penalty_minimize_torque(penalty_origin, value):
ocp = prepare_test_ocp()
t = [0, 1]
x = [0]
u = [DM.ones((4, 1)) * value]
penalty = Objective(penalty_origin.MINIMIZE_CONTROL, key="tau")
res = get_penalty_value(ocp, penalty, t, x, u, [])
np.testing.assert_almost_equal(res,
np.array([[value, value, value, value]]).T)
def test_penalty_minimize_time(penalty_origin, value):
ocp = prepare_test_ocp()
penalty_type = penalty_origin.MINIMIZE_TIME
penalty = Objective(penalty_type)
penalty_type.value[0](penalty, ocp, ocp.nlp[0], [], [], [], [])
np.testing.assert_almost_equal(
ocp.nlp[0].J[0][0]["val"],
np.array(1),
)
def test_penalty_minimize_predicted_com_height(value):
ocp = prepare_test_ocp()
x = [DM.ones((12, 1)) * value]
penalty_type = ObjectiveFcn.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT
penalty = Objective(penalty_type)
penalty_type.value[0](penalty, PenaltyNodes(ocp, ocp.nlp[0], [], x, [],
[]))
res = np.array(0.0501274 if value == 0.1 else -3.72579)
np.testing.assert_almost_equal(ocp.nlp[0].J[0][0]["val"], res)
def test_penalty_minimize_predicted_com_height(value):
ocp = prepare_test_ocp()
t = [0]
x = [DM.ones((8, 1)) * value]
u = [0]
penalty_type = ObjectiveFcn.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT
penalty = Objective(penalty_type)
res = get_penalty_value(ocp, penalty, t, x, u, [])
expected = np.array(0.0501274 if value == 0.1 else -3.72579)
np.testing.assert_almost_equal(res, expected)
def test_penalty_minimize_markers_displacement(penalty_origin, value):
ocp = prepare_test_ocp()
x = [DM.ones((12, 1)) * value]
penalty_type = penalty_origin.MINIMIZE_MARKERS_DISPLACEMENT
penalty = Objective(penalty_type)
penalty_type.value[0](penalty, ocp, ocp.nlp[0], [], x, [], [])
np.testing.assert_almost_equal(
ocp.nlp[0].J[0],
np.array([]),
)
def test_penalty_minimize_state(penalty_origin, value):
ocp = prepare_test_ocp()
x = [DM.ones((12, 1)) * value]
penalty_type = penalty_origin.MINIMIZE_STATE
penalty = Objective(penalty_type)
penalty_type.value[0](penalty, ocp, ocp.nlp[0], [], x, [], [])
np.testing.assert_almost_equal(
ocp.nlp[0].J[0][0]["val"],
np.array([[value]] * 8),
)
def test_penalty_minimize_muscles_control(penalty_origin, value):
ocp = prepare_test_ocp(with_muscles=True)
t = [0, 1]
x = [0]
u = [DM.ones((8, 1)) * value]
penalty_type = penalty_origin.MINIMIZE_CONTROL
penalty = Objective(penalty_type, key="muscles")
res = get_penalty_value(ocp, penalty, t, x, u, [])
np.testing.assert_almost_equal(
res,
np.array([[value, value, value, value, value, value]]).T)
def set_bow_target_objective(self,
bow_target: np.ndarray,
weight: float = 10000,
sol: Solution = None):
new_objectives = Objective(
ObjectiveFcn.Lagrange.TRACK_STATE,
node=Node.ALL,
weight=weight,
target=bow_target,
index=self.bow.hair_idx,
list_index=5,
)
self.ocp.update_objectives(new_objectives)
def test_penalty_minimize_time(penalty_origin, value):
ocp = prepare_test_ocp()
t = [0]
x = [DM.ones((8, 1)) * value]
u = [0]
penalty_type = penalty_origin.MINIMIZE_TIME
penalty = Objective(penalty_type)
penalty_type.value[0](penalty,
PenaltyNodeList(ocp, ocp.nlp[0], [], [], [], []))
res = get_penalty_value(ocp, penalty, t, x, u, [])
np.testing.assert_almost_equal(res, np.array(1))
