def prepare_ocp(biorbd_model_path, final_time, number_shooting_points, min_g, max_g, target_g):
# --- Options --- #
biorbd_model = biorbd.Model(biorbd_model_path)
tau_min, tau_max, tau_init = -30, 30, 0
n_q = biorbd_model.nbQ()
n_qdot = biorbd_model.nbQdot()
n_tau = biorbd_model.nbGeneralizedTorque()
# Add objective functions
objective_functions = ObjectiveOption(Objective.Lagrange.MINIMIZE_TORQUE, weight=10)
# Dynamics
dynamics = DynamicsTypeOption(DynamicsType.TORQUE_DRIVEN)
# Path constraint
x_bounds = BoundsOption(QAndQDotBounds(biorbd_model))
x_bounds[:, [0, -1]] = 0
x_bounds[1, -1] = 3.14
# Initial guess
x_init = InitialGuessOption([0] * (n_q + n_qdot))
# Define control path constraint
u_bounds = BoundsOption([[tau_min] * n_tau, [tau_max] * n_tau])
u_bounds[1, :] = 0
u_init = InitialGuessOption([tau_init] * n_tau)
# Define the parameter to optimize
# Give the parameter some min and max bounds
parameters = ParameterList()
bound_gravity = Bounds(min_bound=min_g, max_bound=max_g, interpolation=InterpolationType.CONSTANT)
# and an initial condition
initial_gravity = InitialGuess((min_g + max_g) / 2)
parameter_objective_functions = ObjectiveOption(
my_target_function, weight=10, quadratic=True, custom_type=Objective.Parameter, target=target_g
)
parameters.add(
"gravity_z", # The name of the parameter
my_parameter_function, # The function that modifies the biorbd model
initial_gravity, # The initial guess
bound_gravity, # The bounds
size=1, # The number of elements this particular parameter vector has
penalty_list=parameter_objective_functions, # Objective of constraint for this particular parameter
extra_value=1, # You can define as many extra arguments as you want
)
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
parameters=parameters,
)
def test_wrong_parameter():
param = ParameterList()
my_parameter_function = 1
initial_gravity = 1
bounds = 1
with pytest.raises(
ValueError,
match="Parameters are declared for all phases at once. You must therefore "
"not use 'phase' but 'list_index' instead",
):
param.add("gravity_z", my_parameter_function, initial_gravity, bounds, size=1, phase=0)
def test_update_bounds_and_init_with_param():
def my_parameter_function(biorbd_model, value, extra_value):
biorbd_model.setGravity(biorbd.Vector3d(0, 0, value + extra_value))
def my_target_function(ocp, value, target_value):
return value + target_value
biorbd_model = biorbd.Model(TestUtils.bioptim_folder() + "/examples/track/cube_and_line.bioMod")
nq = biorbd_model.nbQ()
ns = 10
g_min, g_max, g_init = -10, -6, -8
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
parameters = ParameterList()
bounds_gravity = Bounds(g_min, g_max, interpolation=InterpolationType.CONSTANT)
initial_gravity = InitialGuess(g_init)
parameter_objective_functions = Objective(
my_target_function, weight=10, quadratic=True, custom_type=ObjectiveFcn.Parameter, target_value=-8
)
parameters.add(
"gravity_z",
my_parameter_function,
initial_gravity,
bounds_gravity,
size=1,
penalty_list=parameter_objective_functions,
extra_value=1,
)
ocp = OptimalControlProgram(biorbd_model, dynamics, ns, 1.0, parameters=parameters)
x_bounds = Bounds(-np.ones((nq * 2, 1)), np.ones((nq * 2, 1)))
u_bounds = Bounds(-2.0 * np.ones((nq, 1)), 2.0 * np.ones((nq, 1)))
ocp.update_bounds(x_bounds, u_bounds)
expected = np.array([[-1] * (nq * 2) * (ns + 1) + [-2] * nq * ns]).T
np.testing.assert_almost_equal(ocp.v.bounds.min, np.append(expected, [g_min])[:, np.newaxis])
expected = np.array([[1] * (nq * 2) * (ns + 1) + [2] * nq * ns]).T
np.testing.assert_almost_equal(ocp.v.bounds.max, np.append(expected, [g_max])[:, np.newaxis])
x_init = InitialGuess(0.5 * np.ones((nq * 2, 1)))
u_init = InitialGuess(-0.5 * np.ones((nq, 1)))
ocp.update_initial_guess(x_init, u_init)
expected = np.array([[0.5] * (nq * 2) * (ns + 1) + [-0.5] * nq * ns]).T
np.testing.assert_almost_equal(ocp.v.init.init, np.append(expected, [g_init])[:, np.newaxis])
def prepare_ocp(phase_time_constraint, use_parameter):
# --- Inputs --- #
final_time = (2, 5, 4)
time_min = [1, 3, 0.1]
time_max = [2, 4, 0.8]
ns = (20, 30, 20)
biorbd_model_path = TestUtils.bioptim_folder(
) + "/examples/optimal_time_ocp/cube.bioMod"
ode_solver = OdeSolver.RK4()
# --- Options --- #
n_phases = len(ns)
# Model path
biorbd_model = (biorbd.Model(biorbd_model_path),
biorbd.Model(biorbd_model_path),
biorbd.Model(biorbd_model_path))
# Problem parameters
tau_min, tau_max, tau_init = -100, 100, 0
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE,
weight=100,
phase=0)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE,
weight=100,
phase=1)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE,
weight=100,
phase=2)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, phase=0)
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, phase=1)
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, phase=2)
# Constraints
constraints = ConstraintList()
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.START,
first_marker="m0",
second_marker="m1",
phase=0)
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.END,
first_marker="m0",
second_marker="m2",
phase=0)
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.END,
first_marker="m0",
second_marker="m1",
phase=1)
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.END,
first_marker="m0",
second_marker="m2",
phase=2)
constraints.add(
ConstraintFcn.TIME_CONSTRAINT,
node=Node.END,
minimum=time_min[0],
maximum=time_max[0],
phase=phase_time_constraint,
)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) # Phase 0
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) # Phase 1
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) # Phase 2
for bounds in x_bounds:
for i in [1, 3, 4, 5]:
bounds[i, [0, -1]] = 0
x_bounds[0][2, 0] = 0.0
x_bounds[2][2, [0, -1]] = [0.0, 1.57]
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(),
[tau_max] * biorbd_model[0].nbGeneralizedTorque())
u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(),
[tau_max] * biorbd_model[0].nbGeneralizedTorque())
u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(),
[tau_max] * biorbd_model[0].nbGeneralizedTorque())
u_init = InitialGuessList()
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
parameters = ParameterList()
if use_parameter:
def my_target_function(ocp, value, target_value):
return value - target_value
def my_parameter_function(biorbd_model, value, extra_value):
new_gravity = MX.zeros(3, 1)
new_gravity[2] = value + extra_value
biorbd_model.setGravity(new_gravity)
min_g = -10
max_g = -6
target_g = -8
bound_gravity = Bounds(min_g,
max_g,
interpolation=InterpolationType.CONSTANT)
initial_gravity = InitialGuess((min_g + max_g) / 2)
parameter_objective_functions = Objective(
my_target_function,
weight=10,
quadratic=True,
custom_type=ObjectiveFcn.Parameter,
target_value=target_g)
parameters.add(
"gravity_z",
my_parameter_function,
initial_gravity,
bound_gravity,
size=1,
penalty_list=parameter_objective_functions,
extra_value=1,
)
# ------------- #
return OptimalControlProgram(
biorbd_model[:n_phases],
dynamics,
ns,
final_time[:n_phases],
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
parameters=parameters,
)
def prepare_ocp_parameters(
biorbd_model_path,
final_time,
n_shooting,
optim_gravity,
optim_mass,
min_g,
max_g,
target_g,
min_m,
max_m,
target_m,
ode_solver=OdeSolver.RK4(),
use_sx=False,
) -> OptimalControlProgram:
"""
Prepare the program
Parameters
----------
biorbd_model_path: str
The path of the biorbd model
final_time: float
The time at the final node
n_shooting: int
The number of shooting points
optim_gravity: bool
If the gravity should be optimized
optim_mass: bool
If the mass should be optimized
min_g: np.ndarray
The minimal value for the gravity
max_g: np.ndarray
The maximal value for the gravity
target_g: np.ndarray
The target value for the gravity
min_m: float
The minimal value for the mass
max_m: float
The maximal value for the mass
target_m: float
The target value for the mass
ode_solver: OdeSolver
The type of ode solver used
use_sx: bool
If the program should be constructed using SX instead of MX (longer to create the CasADi graph, faster to solve)
Returns
-------
The ocp ready to be solved
"""
# --- Options --- #
biorbd_model = biorbd.Model(biorbd_model_path)
n_tau = biorbd_model.nbGeneralizedTorque()
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=10)
objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_TIME, weight=10)
# 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
tau_min, tau_max, tau_init = -300, 300, 0
u_bounds = Bounds([tau_min] * n_tau, [tau_max] * n_tau)
u_bounds[1, :] = 0
u_init = InitialGuess([tau_init] * n_tau)
# Define the parameter to optimize
parameters = ParameterList()
if optim_gravity:
# Give the parameter some min and max bounds
bound_gravity = Bounds(min_g,
max_g,
interpolation=InterpolationType.CONSTANT)
# and an initial condition
initial_gravity = InitialGuess((min_g + max_g) / 2)
# and an objective function
parameter_objective_functions = Objective(
my_target_function,
weight=1000,
quadratic=False,
custom_type=ObjectiveFcn.Parameter,
target=target_g)
parameters.add(
"gravity_xyz", # The name of the parameter
my_parameter_function, # The function that modifies the biorbd model
initial_gravity, # The initial guess
bound_gravity, # The bounds
size=
3, # The number of elements this particular parameter vector has
penalty_list=
parameter_objective_functions, # ObjectiveFcn of constraint for this particular parameter
scaling=np.array([1, 1, 10.0]),
extra_value=1, # You can define as many extra arguments as you want
)
if optim_mass:
bound_mass = Bounds(min_m,
max_m,
interpolation=InterpolationType.CONSTANT)
initial_mass = InitialGuess((min_m + max_m) / 2)
mass_objective_functions = Objective(
my_target_function,
weight=100,
quadratic=False,
custom_type=ObjectiveFcn.Parameter,
target=target_m)
parameters.add(
"mass", # The name of the parameter
set_mass, # The function that modifies the biorbd model
initial_mass, # The initial guess
bound_mass, # The bounds
size=
1, # The number of elements this particular parameter vector has
penalty_list=
mass_objective_functions, # ObjectiveFcn of constraint for this particular parameter
scaling=np.array([10.0]),
)
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
parameters=parameters,
ode_solver=ode_solver,
use_sx=use_sx,
)
def test_add_wrong_param():
g_min, g_max, g_init = -10, -6, -8
def my_parameter_function(biorbd_model, value, extra_value):
biorbd_model.setGravity(biorbd.Vector3d(0, 0, value + extra_value))
def my_target_function(ocp, value, target_value):
return value + target_value
parameters = ParameterList()
initial_gravity = InitialGuess(g_init)
bounds_gravity = Bounds(g_min,
g_max,
interpolation=InterpolationType.CONSTANT)
parameter_objective_functions = Objective(
my_target_function,
weight=10,
quadratic=True,
custom_type=ObjectiveFcn.Parameter,
target_value=-8)
with pytest.raises(
RuntimeError,
match=
"function, initial_guess, bounds and size are mandatory elements to declare a parameter"
):
parameters.add(
"gravity_z",
[],
initial_gravity,
bounds_gravity,
size=1,
penalty_list=parameter_objective_functions,
extra_value=1,
)
with pytest.raises(
RuntimeError,
match=
"function, initial_guess, bounds and size are mandatory elements to declare a parameter"
):
parameters.add(
"gravity_z",
my_parameter_function,
None,
bounds_gravity,
size=1,
penalty_list=parameter_objective_functions,
extra_value=1,
)
with pytest.raises(
RuntimeError,
match=
"function, initial_guess, bounds and size are mandatory elements to declare a parameter"
):
parameters.add(
"gravity_z",
my_parameter_function,
initial_gravity,
None,
size=1,
penalty_list=parameter_objective_functions,
extra_value=1,
)
with pytest.raises(
RuntimeError,
match=
"function, initial_guess, bounds and size are mandatory elements to declare a parameter"
):
parameters.add(
"gravity_z",
my_parameter_function,
initial_gravity,
bounds_gravity,
penalty_list=parameter_objective_functions,
extra_value=1,
)
def test_param_scaling():
param = ParameterList()
my_parameter_function = 1
initial_gravity = 1
bounds = 1
with pytest.raises(
ValueError,
match="Parameter scaling must be a numpy array",
):
param.add("gravity_z", my_parameter_function, initial_gravity, bounds, size=1, scaling="a")
with pytest.raises(
ValueError,
match="Parameter scaling must be a numpy array",
):
param.add("gravity_z", my_parameter_function, initial_gravity, bounds, size=1, scaling=1.0)
with pytest.raises(
ValueError,
match="Parameter scaling must be a numpy array",
):
param.add("gravity_z", my_parameter_function, initial_gravity, bounds, size=1, scaling=[])
with pytest.raises(
ValueError,
match="Parameter scaling must contain only positive values",
):
param.add("gravity_z", my_parameter_function, initial_gravity, bounds, size=1, scaling=np.array([-1]))
with pytest.raises(
ValueError,
match="Parameter scaling must be a 1- or 2- dimensional numpy array",
):
param.add("gravity_z", my_parameter_function, initial_gravity, bounds, size=1, scaling=np.array([[[1]]]))
with pytest.raises(
ValueError, match=f"The shape \(2\) of the scaling of parameter gravity_z does not match the params shape."
):
param.add("gravity_z", my_parameter_function, initial_gravity, bounds, size=3, scaling=np.array([1, 2]))
with pytest.raises(
ValueError, match=f"Invalid ncols for Parameter Scaling \(ncols = 2\), the expected number of column is 1"
):
param.add("gravity_z", my_parameter_function, initial_gravity, bounds, size=3, scaling=np.ones((1, 2)))
def prepare_ocp(biorbd_model,
final_time,
number_shooting_points,
marker_ref,
excitations_ref,
q_ref,
state_init,
f_init,
use_residual_torque,
activation_driven,
kin_data_to_track,
nb_threads,
use_SX=True,
param=False):
# --- Options --- #
nb_mus = biorbd_model.nbMuscleTotal()
activation_min, activation_max, activation_init = 0, 1, 0.3
excitation_min, excitation_max, excitation_init = 0, 1, 0.1
torque_min, torque_max, torque_init = -100, 100, 0
# nb_tau = biorbd_model.segment(0).nbQ()
# tau_mapping = BidirectionalMapping(Mapping(range(6)), Mapping(range(nb_tau)))
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Lagrange.TRACK_MUSCLES_CONTROL,
weight=10,
target=excitations_ref)
# objective_functions.add(Objective.Lagrange.MINIMIZE_STATE, weight=1000,
# idx_states=(0, 1, 2, 3, 4, 5, 11, 12, 13, 14, 15, 16))
objective_functions.add(Objective.Lagrange.MINIMIZE_STATE,
weight=1,
idx_states=(6, 7, 8, 9, 10))
if use_residual_torque:
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE,
weight=1) # controls_idx=[6, 7, 8, 9, 10],
if kin_data_to_track == "markers":
objective_functions.add(Objective.Lagrange.TRACK_MARKERS,
weight=1000,
target=marker_ref)
elif kin_data_to_track == "q":
objective_functions.add(Objective.Lagrange.TRACK_STATE,
weight=100,
target=q_ref,
states_idx=range(biorbd_model.nbQ()))
else:
raise RuntimeError("Wrong choice of kin_data_to_track")
# Dynamics
dynamics = DynamicsTypeList()
if use_residual_torque:
dynamics.add(DynamicsType.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN)
elif activation_driven:
dynamics.add(DynamicsType.MUSCLE_ACTIVATIONS_DRIVEN)
else:
dynamics.add(DynamicsType.MUSCLE_EXCITATIONS_DRIVEN)
# Constraints
constraints = ()
# constraints = ConstraintList()
# constraints.add(Constraint.TRACK_TORQUE, instant=Instant.ALL, controls_idx=(6, 7, 8, 9, 10),
# target=np.zeros((biorbd_model.nbQ()*2, number_shooting_points)))
# Path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
# if use_SX:
# x_bounds[0].min[biorbd_model.nbQ():, 0] = -10
# x_bounds[0].max[biorbd_model.nbQ():, 0] = 10
# Add muscle to the bounds
if activation_driven is not True:
x_bounds[0].concatenate(
Bounds([activation_min] * biorbd_model.nbMuscles(),
[activation_max] * biorbd_model.nbMuscles()))
# Initial guess
x_init = InitialGuessList()
if activation_driven:
# state_base = np.ndarray((12, n_shooting_points+1))
# for i in range(n_shooting_points+1):
# state_base[:, i] = np.concatenate((state_init[:6, 0], np.zeros((6))))
x_init.add(state_init[:-nb_mus, :],
interpolation=InterpolationType.EACH_FRAME)
# x_init.add(state_init[:-nb_mus, :], interpolation=InterpolationType.EACH_FRAME)
else:
x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()) +
[0] * biorbd_model.nbMuscles())
# x_init.add(state_init[biorbd_model.nbQ():, :], interpolation=InterpolationType.EACH_FRAME)
# Add muscle to the bounds
u_bounds = BoundsList()
u_init = InitialGuessList()
nb_tau = biorbd_model.nbGeneralizedTorque()
init_residual_torque = np.concatenate((np.ones(
(nb_tau, n_shooting_points)) * 0.5, excitations_ref))
if use_residual_torque:
u_bounds.add([
[torque_min] * nb_tau +
[excitation_min] * biorbd_model.nbMuscleTotal(),
[torque_max] * nb_tau +
[excitation_max] * biorbd_model.nbMuscleTotal(),
])
# u_init.add([torque_init] * tau_mapping.reduce.len + [excitation_init] * biorbd_model.nbMuscleTotal())
u_init.add(init_residual_torque,
interpolation=InterpolationType.EACH_FRAME)
else:
u_bounds.add([[excitation_min] * biorbd_model.nbMuscleTotal(),
[excitation_max] * biorbd_model.nbMuscleTotal()])
if activation_driven:
# u_init.add([activation_init] * biorbd_model.nbMuscleTotal())
u_init.add(excitations_ref,
interpolation=InterpolationType.EACH_FRAME)
else:
# u_init.add([excitation_init] * biorbd_model.nbMuscleTotal())
u_init.add(excitations_ref,
interpolation=InterpolationType.EACH_FRAME)
# Get initial isometric forces
fiso = []
for k in range(nb_mus):
fiso.append(
biorbd_model.muscle(k).characteristics().forceIsoMax().to_mx())
# Define the parameter to optimize
bound_p_iso = Bounds(
# min_bound=np.repeat(0.01, nb_mus+1), max_bound=np.repeat(7, nb_mus+1), interpolation=InterpolationType.CONSTANT)
min_bound=[0.5] * nb_mus,
max_bound=[3] * nb_mus,
interpolation=InterpolationType.CONSTANT)
bound_shape_factor = Bounds(min_bound=np.repeat(-3, nb_mus),
max_bound=np.repeat(0, nb_mus),
interpolation=InterpolationType.CONSTANT)
p_iso_init = InitialGuess(f_init)
initial_guess_A = InitialGuess([-3] * nb_mus)
parameters = ParameterList()
parameters.add(
"p_iso", # The name of the parameter
modify_isometric_force, # The function that modifies the biorbd model
p_iso_init,
bound_p_iso, # The bounds
size=
nb_mus, # The number of elements this particular parameter vector has
fiso_init=fiso,
)
# parameters.add(
# "shape_factor", # The name of the parameter
# modify_shape_factor,
# initial_guess_A,
# bound_shape_factor, # The bounds
# size=nb_mus, # The number of elements this particular parameter vector has
# )
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
nb_threads=nb_threads,
use_SX=use_SX,
parameters=parameters,
# tau_mapping=tau_mapping,
)
def test_update_bounds_and_init_with_param():
def my_parameter_function(biorbd_model, value, extra_value):
biorbd_model.setGravity(biorbd.Vector3d(0, 0, value + extra_value))
def my_target_function(ocp, value, target_value):
return value + target_value
PROJECT_FOLDER = Path(__file__).parent / ".."
biorbd_model = biorbd.Model(
str(PROJECT_FOLDER) + "/examples/align/cube_and_line.bioMod")
nq = biorbd_model.nbQ()
ns = 10
g_min, g_max, g_init = -10, -6, -8
dynamics = DynamicsTypeList()
dynamics.add(DynamicsType.TORQUE_DRIVEN)
parameters = ParameterList()
bounds_gravity = Bounds(min_bound=g_min,
max_bound=g_max,
interpolation=InterpolationType.CONSTANT)
initial_gravity = InitialGuess(g_init)
parameter_objective_functions = ObjectiveOption(
my_target_function,
weight=10,
quadratic=True,
custom_type=Objective.Parameter,
target_value=-8)
parameters.add(
"gravity_z",
my_parameter_function,
initial_gravity,
bounds_gravity,
size=1,
penalty_list=parameter_objective_functions,
extra_value=1,
)
ocp = OptimalControlProgram(biorbd_model,
dynamics,
ns,
1.0,
parameters=parameters)
x_bounds = BoundsOption([-np.ones((nq * 2, 1)), np.ones((nq * 2, 1))])
u_bounds = BoundsOption([-2.0 * np.ones((nq, 1)), 2.0 * np.ones((nq, 1))])
ocp.update_bounds(x_bounds, u_bounds)
expected = np.append(
np.tile(np.append(-np.ones((nq * 2, 1)), -2.0 * np.ones((nq, 1))), ns),
-np.ones((nq * 2, 1)))
np.testing.assert_almost_equal(
ocp.V_bounds.min,
np.append([g_min], expected).reshape(129, 1))
expected = np.append(
np.tile(np.append(np.ones((nq * 2, 1)), 2.0 * np.ones((nq, 1))), ns),
np.ones((nq * 2, 1)))
np.testing.assert_almost_equal(
ocp.V_bounds.max,
np.append([[g_max]], expected).reshape(129, 1))
x_init = InitialGuessOption(0.5 * np.ones((nq * 2, 1)))
u_init = InitialGuessOption(-0.5 * np.ones((nq, 1)))
ocp.update_initial_guess(x_init, u_init)
expected = np.append(
np.tile(np.append(0.5 * np.ones((nq * 2, 1)), -0.5 * np.ones((nq, 1))),
ns), 0.5 * np.ones((nq * 2, 1)))
np.testing.assert_almost_equal(
ocp.V_init.init,
np.append([g_init], expected).reshape(129, 1))