def test_accessors_on_bounds_option_multidimensional():
x_min = [[-100, -50, 0] for i in range(6)]
x_max = [[100, 150, 200] for i in range(6)]
x_bounds = Bounds(
x_min,
x_max,
interpolation=InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT)
x_bounds[:3, 0] = 0
x_bounds.min[1:5, 1:] = -10
x_bounds.max[1:5, 1:] = 10
# Check accessor and min/max values to be equal
np.testing.assert_almost_equal(x_bounds[:].min, x_bounds.min[:])
np.testing.assert_almost_equal(x_bounds[:].max, x_bounds.max[:])
# Check min and max have the right value
np.testing.assert_almost_equal(
x_bounds.min[:],
np.array([[0, -50, 0], [0, -10, -10], [0, -10, -10], [-100, -10, -10],
[-100, -10, -10], [-100, -50, 0]]),
)
np.testing.assert_almost_equal(
x_bounds.max[:],
np.array([[0, 150, 200], [0, 10, 10], [0, 10, 10], [100, 10, 10],
[100, 10, 10], [100, 150, 200]]),
)
def test_mhe_redim_xbounds_and_init():
root_folder = TestUtils.bioptim_folder() + "/examples/moving_horizon_estimation/"
biorbd_model = biorbd.Model(root_folder + "cart_pendulum.bioMod")
nq = biorbd_model.nbQ()
ntau = biorbd_model.nbGeneralizedTorque()
n_cycles = 3
window_len = 5
window_duration = 0.2
x_init = InitialGuess(np.zeros((nq * 2, 1)), interpolation=InterpolationType.CONSTANT)
u_init = InitialGuess(np.zeros((ntau, 1)), interpolation=InterpolationType.CONSTANT)
x_bounds = Bounds(np.zeros((nq * 2, 1)), np.zeros((nq * 2, 1)), interpolation=InterpolationType.CONSTANT)
u_bounds = Bounds(np.zeros((ntau, 1)), np.zeros((ntau, 1)))
mhe = MovingHorizonEstimator(
biorbd_model,
Dynamics(DynamicsFcn.TORQUE_DRIVEN),
window_len,
window_duration,
x_init=x_init,
u_init=u_init,
x_bounds=x_bounds,
u_bounds=u_bounds,
n_threads=4,
)
def update_functions(mhe, t, _):
return t < n_cycles
mhe.solve(update_functions, Solver.IPOPT)
def __init__(
self,
model_path: str,
violin: Violin,
bow: Bow,
n_cycles: int,
bow_starting: BowPosition.TIP,
init_file: str = None,
use_muscles: bool = True,
time_per_cycle: float = 1,
n_shooting_per_cycle: int = 30,
solver: Solver = Solver.IPOPT,
n_threads: int = 8,
):
self.model_path = model_path
self.model = biorbd.Model(self.model_path)
self.n_q = self.model.nbQ()
self.n_tau = self.model.nbGeneralizedTorque()
self.use_muscles = use_muscles
self.n_mus = self.model.nbMuscles() if self.use_muscles else 0
self.violin = violin
self.bow = bow
self.bow_starting = bow_starting
self.n_cycles = n_cycles
self.n_shooting_per_cycle = n_shooting_per_cycle
self.n_shooting = self.n_shooting_per_cycle * self.n_cycles
self.time_per_cycle = time_per_cycle
self.time = self.time_per_cycle * self.n_cycles
self.solver = solver
self.n_threads = n_threads
if self.use_muscles:
self.dynamics = Dynamics(
DynamicsFcn.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN)
else:
self.dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN)
self.x_bounds = Bounds()
self.u_bounds = Bounds()
self._set_bounds()
self.x_init = InitialGuess()
self.u_init = InitialGuess()
self._set_initial_guess(init_file)
self.objective_functions = ObjectiveList()
self._set_generic_objective_functions()
self.constraints = ConstraintList()
self._set_generic_constraints()
self._set_generic_ocp()
if use_muscles:
online_muscle_torque(self.ocp)
def prepare_test_ocp(with_muscles=False,
with_contact=False,
with_actuator=False):
bioptim_folder = TestUtils.bioptim_folder()
if with_muscles and with_contact or with_muscles and with_actuator or with_contact and with_actuator:
raise RuntimeError(
"With muscles and with contact and with_actuator together is not defined"
)
elif with_muscles:
biorbd_model = biorbd.Model(
bioptim_folder + "/examples/muscle_driven_ocp/models/arm26.bioMod")
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.MUSCLE_DRIVEN, with_torque=True)
nx = biorbd_model.nbQ() + biorbd_model.nbQdot()
nu = biorbd_model.nbGeneralizedTorque() + biorbd_model.nbMuscles()
elif with_contact:
biorbd_model = biorbd.Model(
bioptim_folder +
"/examples/muscle_driven_with_contact/models/2segments_4dof_2contacts_1muscle.bioMod"
)
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN,
with_contact=True,
expand=False)
nx = biorbd_model.nbQ() + biorbd_model.nbQdot()
nu = biorbd_model.nbGeneralizedTorque()
elif with_actuator:
biorbd_model = biorbd.Model(
bioptim_folder + "/examples/torque_driven_ocp/models/cube.bioMod")
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
nx = biorbd_model.nbQ() + biorbd_model.nbQdot()
nu = biorbd_model.nbGeneralizedTorque()
else:
biorbd_model = biorbd.Model(
bioptim_folder + "/examples/track/models/cube_and_line.bioMod")
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
nx = biorbd_model.nbQ() + biorbd_model.nbQdot()
nu = biorbd_model.nbGeneralizedTorque()
x_init = InitialGuess(np.zeros((nx, 1)))
u_init = InitialGuess(np.zeros((nu, 1)))
x_bounds = Bounds(np.zeros((nx, 1)), np.zeros((nx, 1)))
u_bounds = Bounds(np.zeros((nu, 1)), np.zeros((nu, 1)))
ocp = OptimalControlProgram(biorbd_model,
dynamics,
10,
1.0,
x_init,
u_init,
x_bounds,
u_bounds,
use_sx=True)
ocp.nlp[0].J = [[]]
ocp.nlp[0].g = [[]]
return ocp
def test_double_update_bounds_and_init():
bioptim_folder = TestUtils.bioptim_folder()
biorbd_model = biorbd.Model(bioptim_folder +
"/examples/track/cube_and_line.bioMod")
nq = biorbd_model.nbQ()
ns = 10
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
ocp = OptimalControlProgram(biorbd_model, dynamics, ns, 1.0)
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, expected)
expected = np.array([[1] * (nq * 2) * (ns + 1) + [2] * nq * ns]).T
np.testing.assert_almost_equal(ocp.v.bounds.max, expected)
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, expected)
x_bounds = Bounds(-2.0 * np.ones((nq * 2, 1)), 2.0 * np.ones((nq * 2, 1)))
u_bounds = Bounds(-4.0 * np.ones((nq, 1)), 4.0 * np.ones((nq, 1)))
ocp.update_bounds(x_bounds=x_bounds)
ocp.update_bounds(u_bounds=u_bounds)
expected = np.array([[-2] * (nq * 2) * (ns + 1) + [-4] * nq * ns]).T
np.testing.assert_almost_equal(ocp.v.bounds.min, expected)
expected = np.array([[2] * (nq * 2) * (ns + 1) + [4] * nq * ns]).T
np.testing.assert_almost_equal(ocp.v.bounds.max, expected)
x_init = InitialGuess(0.25 * np.ones((nq * 2, 1)))
u_init = InitialGuess(-0.25 * np.ones((nq, 1)))
ocp.update_initial_guess(x_init, u_init)
expected = np.array([[0.25] * (nq * 2) * (ns + 1) + [-0.25] * nq * ns]).T
np.testing.assert_almost_equal(ocp.v.init.init, expected)
with pytest.raises(
RuntimeError,
match=
"x_init should be built from a InitialGuess or InitialGuessList"):
ocp.update_initial_guess(x_bounds, u_bounds)
with pytest.raises(
RuntimeError,
match="x_bounds should be built from a Bounds or BoundsList"):
ocp.update_bounds(x_init, u_init)
def test_acados_bounds_not_implemented(failing):
if platform == "win32":
print("Test for ACADOS on Windows is skipped")
return
root_folder = TestUtils.bioptim_folder(
) + "/examples/moving_horizon_estimation/"
biorbd_model = biorbd.Model(root_folder + "models/cart_pendulum.bioMod")
nq = biorbd_model.nbQ()
ntau = biorbd_model.nbGeneralizedTorque()
n_cycles = 3
window_len = 5
window_duration = 0.2
x_init = InitialGuess(np.zeros((nq * 2, 1)),
interpolation=InterpolationType.CONSTANT)
u_init = InitialGuess(np.zeros((ntau, 1)),
interpolation=InterpolationType.CONSTANT)
if failing == "u_bounds":
x_bounds = Bounds(np.zeros((nq * 2, 1)), np.zeros((nq * 2, 1)))
u_bounds = Bounds(np.zeros((ntau, 1)),
np.zeros((ntau, 1)),
interpolation=InterpolationType.CONSTANT)
elif failing == "x_bounds":
x_bounds = Bounds(np.zeros((nq * 2, 1)),
np.zeros((nq * 2, 1)),
interpolation=InterpolationType.CONSTANT)
u_bounds = Bounds(np.zeros((ntau, 1)), np.zeros((ntau, 1)))
else:
raise ValueError("Wrong value for failing")
mhe = MovingHorizonEstimator(
biorbd_model,
Dynamics(DynamicsFcn.TORQUE_DRIVEN),
window_len,
window_duration,
x_init=x_init,
u_init=u_init,
x_bounds=x_bounds,
u_bounds=u_bounds,
n_threads=4,
)
def update_functions(mhe, t, _):
return t < n_cycles
with pytest.raises(
NotImplementedError,
match=
f"ACADOS must declare an InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT for the {failing}",
):
mhe.solve(update_functions, Solver.ACADOS)
def test_accessors_on_bounds_option():
x_min = [-100] * 6
x_max = [100] * 6
x_bounds = Bounds(x_min, x_max, interpolation=InterpolationType.CONSTANT)
x_bounds[:3] = 0
x_bounds.min[3:] = -10
x_bounds.max[1:3] = 10
# Check accessor and min/max values to be equal
np.testing.assert_almost_equal(x_bounds[:].min, x_bounds.min[:])
np.testing.assert_almost_equal(x_bounds[:].max, x_bounds.max[:])
# Check min and max have the right value
np.testing.assert_almost_equal(x_bounds.min[:], np.array([[0], [0], [0], [-10], [-10], [-10]]))
np.testing.assert_almost_equal(x_bounds.max[:], np.array([[0], [10], [10], [100], [100], [100]]))
def prepare_test_ocp(with_muscles=False,
with_contact=False,
with_actuator=False):
PROJECT_FOLDER = Path(__file__).parent / ".."
if with_muscles and with_contact or with_muscles and with_actuator or with_contact and with_actuator:
raise RuntimeError(
"With muscles and with contact and with_actuator together is not defined"
)
elif with_muscles:
biorbd_model = biorbd.Model(
str(PROJECT_FOLDER) + "/examples/muscle_driven_ocp/arm26.bioMod")
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN)
nx = biorbd_model.nbQ() + biorbd_model.nbQdot()
nu = biorbd_model.nbGeneralizedTorque() + biorbd_model.nbMuscles()
elif with_contact:
biorbd_model = biorbd.Model(
str(PROJECT_FOLDER) +
"/examples/muscle_driven_with_contact/2segments_4dof_2contacts_1muscle.bioMod"
)
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN_WITH_CONTACT)
nx = biorbd_model.nbQ() + biorbd_model.nbQdot()
nu = biorbd_model.nbGeneralizedTorque()
elif with_actuator:
biorbd_model = biorbd.Model(
str(PROJECT_FOLDER) + "/examples/torque_driven_ocp/cube.bioMod")
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
nx = biorbd_model.nbQ() + biorbd_model.nbQdot()
nu = biorbd_model.nbGeneralizedTorque()
else:
biorbd_model = biorbd.Model(
str(PROJECT_FOLDER) + "/examples/align/cube_and_line.bioMod")
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
nx = biorbd_model.nbQ() + biorbd_model.nbQdot()
nu = biorbd_model.nbGeneralizedTorque()
x_init = InitialGuess(np.zeros((nx, 1)))
u_init = InitialGuess(np.zeros((nu, 1)))
x_bounds = Bounds(np.zeros((nx, 1)), np.zeros((nx, 1)))
u_bounds = Bounds(np.zeros((nu, 1)), np.zeros((nu, 1)))
ocp = OptimalControlProgram(biorbd_model, dynamics, 10, 1.0, x_init,
u_init, x_bounds, u_bounds)
ocp.nlp[0].J = [list()]
ocp.nlp[0].g = [list()]
ocp.nlp[0].g_bounds = [list()]
return ocp
def prepare_ocp(biorbd_model_path,
n_shooting,
tf,
ode_solver=OdeSolver.RK4(),
use_sx=True):
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
# Dynamics
dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN)
# Path constraint
x_bounds = QAndQDotBounds(biorbd_model)
x_init = InitialGuess([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()))
# Define control path constraint
tau_min, tau_max, tau_init = -100, 100, 0
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,
n_shooting,
tf,
x_init,
u_init,
x_bounds,
u_bounds,
ode_solver=ode_solver,
use_sx=use_sx,
)
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(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_acados_one_parameter():
if platform == "win32":
print("Test for ACADOS on Windows is skipped")
return
from bioptim.examples.getting_started import custom_parameters as ocp_module
bioptim_folder = os.path.dirname(ocp_module.__file__)
ocp = ocp_module.prepare_ocp(
biorbd_model_path=bioptim_folder + "/models/pendulum.bioMod",
final_time=1,
n_shooting=100,
optim_gravity=True,
optim_mass=False,
min_g=np.array([-1, -1, -10]),
max_g=np.array([1, 1, -5]),
min_m=10,
max_m=30,
target_g=np.array([0, 0, -9.81]),
target_m=20,
use_sx=True,
)
model = ocp.nlp[0].model
objectives = ObjectiveList()
objectives.add(ObjectiveFcn.Mayer.TRACK_STATE, key="q", target=np.array([[0, 3.14]]).T, weight=100000)
objectives.add(ObjectiveFcn.Mayer.TRACK_STATE, key="qdot", target=np.array([[0, 0]]).T, weight=100)
objectives.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", index=1, weight=10, multi_thread=False)
objectives.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE, key="qdot", weight=0.000000010, multi_thread=False)
ocp.update_objectives(objectives)
# Path constraint
x_bounds = QAndQDotBounds(model)
x_bounds[[0, 1, 2, 3], 0] = 0
u_bounds = Bounds([-300] * model.nbQ(), [300] * model.nbQ())
ocp.update_bounds(x_bounds, u_bounds)
solver = Solver.ACADOS()
solver.set_nlp_solver_tol_eq(1e-3)
sol = ocp.solve(solver=solver)
# Check some of the results
q, qdot, tau, gravity = sol.states["q"], sol.states["qdot"], sol.controls["tau"], sol.parameters["gravity_xyz"]
# initial and final position
np.testing.assert_almost_equal(q[:, 0], np.array((0, 0)), decimal=6)
np.testing.assert_almost_equal(q[:, -1], np.array((0, 3.14)), decimal=6)
# initial and final velocities
np.testing.assert_almost_equal(qdot[:, 0], np.array((0, 0)), decimal=6)
np.testing.assert_almost_equal(qdot[:, -1], np.array((0, 0)), decimal=6)
# parameters
np.testing.assert_almost_equal(gravity[-1, :], np.array([-9.81]), decimal=6)
# Clean test folder
os.remove(f"./acados_ocp.json")
shutil.rmtree(f"./c_generated_code/")
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 _set_bounds(self):
self.x_bounds = QAndQDotBounds(self.model)
self.x_bounds[:self.n_q, 0] = self.violin.q(self.bow_starting)
self.x_bounds[self.n_q:, 0] = 0
# self.x_bounds.min[:self.n_q, -1] = np.array(self.violin.q(self.bow_starting)) - 0.01
# self.x_bounds.max[:self.n_q, -1] = np.array(self.violin.q(self.bow_starting)) + 0.01
u_min = [self.tau_min] * self.n_tau + [0] * self.n_mus
u_max = [self.tau_max] * self.n_tau + [1] * self.n_mus
self.u_bounds = Bounds(u_min, u_max)
def test_mhe_redim_xbounds_not_implemented():
root_folder = TestUtils.bioptim_folder(
) + "/examples/moving_horizon_estimation/"
biorbd_model = biorbd.Model(root_folder + "models/cart_pendulum.bioMod")
nq = biorbd_model.nbQ()
ntau = biorbd_model.nbGeneralizedTorque()
n_cycles = 3
window_len = 5
window_duration = 0.2
x_init = InitialGuess(np.zeros((nq * 2, 1)),
interpolation=InterpolationType.CONSTANT)
u_init = InitialGuess(np.zeros((ntau, 1)),
interpolation=InterpolationType.CONSTANT)
x_bounds = Bounds(
np.zeros((nq * 2, window_len + 1)),
np.zeros((nq * 2, window_len + 1)),
interpolation=InterpolationType.EACH_FRAME,
)
u_bounds = Bounds(np.zeros((ntau, 1)), np.zeros((ntau, 1)))
mhe = MovingHorizonEstimator(
biorbd_model,
Dynamics(DynamicsFcn.TORQUE_DRIVEN),
window_len,
window_duration,
x_init=x_init,
u_init=u_init,
x_bounds=x_bounds,
u_bounds=u_bounds,
n_threads=4,
)
def update_functions(mhe, t, _):
return t < n_cycles
with pytest.raises(
NotImplementedError,
match="The MHE is not implemented yet for x_bounds not being "
"CONSTANT or CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT",
):
mhe.solve(update_functions, Solver.IPOPT)
def prepare_nmpc(model_path, cycle_len, cycle_duration, n_cycles_simultaneous, n_cycles_to_advance, max_torque):
model = biorbd.Model(model_path)
dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN)
x_bound = QAndQDotBounds(model)
x_bound.min[0, :] = -2 * np.pi * n_cycles_simultaneous # Allow the wheel to spin as much as needed
x_bound.max[0, :] = 0
u_bound = Bounds([-max_torque] * model.nbQ(), [max_torque] * model.nbQ())
x_init = InitialGuess(
np.zeros(
model.nbQ() * 2,
)
)
u_init = InitialGuess(
np.zeros(
model.nbQ(),
)
)
new_objectives = Objective(ObjectiveFcn.Lagrange.MINIMIZE_STATE, key="q")
# Rotate the wheel and force the marker of the hand to follow the marker on the wheel
wheel_target = np.linspace(-2 * np.pi * n_cycles_simultaneous, 0, cycle_len * n_cycles_simultaneous + 1)[
np.newaxis, :
]
constraints = ConstraintList()
constraints.add(ConstraintFcn.TRACK_STATE, key="q", index=0, node=Node.ALL, target=wheel_target)
constraints.add(
ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.ALL,
first_marker="wheel",
second_marker="COM_hand",
axes=[Axis.X, Axis.Y],
)
return MyCyclicNMPC(
model,
dynamics,
cycle_len=cycle_len,
cycle_duration=cycle_duration,
n_cycles_simultaneous=n_cycles_simultaneous,
n_cycles_to_advance=n_cycles_to_advance,
objective_functions=new_objectives,
constraints=constraints,
x_init=x_init,
u_init=u_init,
x_bounds=x_bound,
u_bounds=u_bound,
)
def prepare_ocp(biorbd_model_path: str, final_time: float,
n_shooting: int) -> 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
"""
biorbd_model = biorbd.Model(biorbd_model_path)
# 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
torque_min, torque_max, torque_init = -100, 100, 0
n_tau = biorbd_model.nbGeneralizedTorque()
u_bounds = Bounds([torque_min] * n_tau, [torque_max] * n_tau)
u_bounds[n_tau - 1, :] = 0
u_init = InitialGuess([torque_init] * n_tau)
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
)
def prepare_ocp(biorbd_model, final_time, number_shooting_points, x0, use_sx=False, n_threads=8):
# --- Options --- #
# Model path
activation_min, activation_max, activation_init = 0, 1, 0.1
excitation_min, excitation_max, excitation_init = 0, 1, 0.2
# Add objective functions
objective_functions = ObjectiveList()
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.MUSCLE_EXCITATIONS_DRIVEN)
# State path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
# add muscle activation bounds
x_bounds[0].concatenate(
Bounds([activation_min] * biorbd_model.nbMuscles(), [activation_max] * biorbd_model.nbMuscles())
)
# Control path constraint
u_bounds = BoundsList()
u_bounds.add([excitation_min] * biorbd_model.nbMuscleTotal(), [excitation_max] * biorbd_model.nbMuscleTotal())
# Initial guesses
x_init = InitialGuess(
np.tile(np.concatenate((x0, [activation_init] * biorbd_model.nbMuscles())), (number_shooting_points + 1, 1)).T,
interpolation=InterpolationType.EACH_FRAME,
)
u0 = np.array([excitation_init] * biorbd_model.nbMuscles())
u_init = InitialGuess(np.tile(u0, (number_shooting_points, 1)).T, interpolation=InterpolationType.EACH_FRAME)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
use_sx=use_sx,
n_threads=n_threads,
)
def prepare_ocp(
biorbd_model_path, final_time, number_shooting_points, nb_threads, use_SX=False, ode_solver=OdeSolver.RK
):
# --- Options --- #
biorbd_model = biorbd.Model(biorbd_model_path)
tau_min, tau_max, tau_init = -100, 100, 0
n_q = biorbd_model.nbQ()
n_qdot = biorbd_model.nbQdot()
n_tau = biorbd_model.nbGeneralizedTorque()
# Add objective functions
objective_functions = Objective(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE_DERIVATIVE)
# 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
x_init = InitialGuess([0] * (n_q + n_qdot))
# Define control path constraint
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,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions=objective_functions,
nb_threads=nb_threads,
use_SX=use_SX,
ode_solver=ode_solver,
)
def prepare_ocp(biorbd_model_path: str = "models/mass_point.bioMod"):
# Model path
m = biorbd.Model(biorbd_model_path)
m.setGravity(np.array((0, 0, 0)))
# Add objective functions (high upward velocity at end point)
objective_functions = Objective(ObjectiveFcn.Mayer.MINIMIZE_STATE,
key="qdot",
index=0,
weight=-1)
# Dynamics
dynamics = Dynamics(custom_configure, dynamic_function=custom_dynamic)
# Path constraint
x_bounds = QAndQDotBounds(m)
x_bounds[:, 0] = [0] * m.nbQ() + [0] * m.nbQdot()
x_bounds.min[:, 1] = [-1] * m.nbQ() + [-100] * m.nbQdot()
x_bounds.max[:, 1] = [1] * m.nbQ() + [100] * m.nbQdot()
x_bounds.min[:, 2] = [-1] * m.nbQ() + [-100] * m.nbQdot()
x_bounds.max[:, 2] = [1] * m.nbQ() + [100] * m.nbQdot()
# Initial guess
x_init = InitialGuess([0] * (m.nbQ() + m.nbQdot()))
# Define control path constraint
u_bounds = Bounds([-100] * m.nbGeneralizedTorque(),
[0] * m.nbGeneralizedTorque())
u_init = InitialGuess([0] * m.nbGeneralizedTorque())
return OptimalControlProgram(
m,
dynamics,
n_shooting=30,
phase_time=0.5,
x_init=x_init,
u_init=u_init,
x_bounds=x_bounds,
u_bounds=u_bounds,
objective_functions=objective_functions,
)
def prepare_ocp(biorbd_model_path,
nbs,
tf,
ode_solver=OdeSolver.RK,
use_SX=True):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
# Problem parameters
tau_min, tau_max, tau_init = -100, 100, 0
# Dynamics
dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN)
# Path constraint
x_bounds = QAndQDotBounds(biorbd_model)
# 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,
nbs,
tf,
x_init,
u_init,
x_bounds,
u_bounds,
ode_solver=ode_solver,
use_SX=use_SX,
)
def prepare_mhe(biorbd_model, window_len, window_duration, max_torque, x_init,
u_init):
new_objectives = Objective(ObjectiveFcn.Lagrange.MINIMIZE_MARKERS,
node=Node.ALL,
weight=1000,
list_index=0)
return MovingHorizonEstimator(
biorbd_model,
Dynamics(DynamicsFcn.TORQUE_DRIVEN),
window_len,
window_duration,
objective_functions=new_objectives,
x_init=InitialGuess(x_init,
interpolation=InterpolationType.EACH_FRAME),
u_init=InitialGuess(u_init,
interpolation=InterpolationType.EACH_FRAME),
x_bounds=QAndQDotBounds(biorbd_model),
u_bounds=Bounds([-max_torque, 0.0], [max_torque, 0.0]),
n_threads=4,
)
def prepare_ocp(biorbd_model_path, final_time, number_shooting_points):
# --- Options --- #
biorbd_model = biorbd.Model(biorbd_model_path)
torque_min, torque_max, torque_init = -100, 100, 0
n_q = biorbd_model.nbQ()
n_qdot = biorbd_model.nbQdot()
n_tau = biorbd_model.nbGeneralizedTorque()
# 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
x_init = InitialGuess([0] * (n_q + n_qdot))
# Define control path constraint
u_bounds = Bounds([torque_min] * n_tau, [torque_max] * n_tau)
u_bounds[n_tau - 1, :] = 0
u_init = InitialGuess([torque_init] * n_tau)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
)
def prepare_ocp(
biorbd_model_path: str,
n_shooting: int,
final_time: float,
loop_from_constraint: bool,
ode_solver: OdeSolver = OdeSolver.RK4(),
) -> 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
loop_from_constraint: bool
If the looping cost should be a constraint [True] or an objective [False]
ode_solver: OdeSolver
The type of ode solver used
Returns
-------
The ocp ready to be solved
"""
biorbd_model = biorbd.Model(biorbd_model_path)
# Add objective functions
objective_functions = Objective(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE,
weight=100)
# Dynamics
dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN)
# Constraints
constraints = ConstraintList()
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.MID,
first_marker_idx=0,
second_marker_idx=2)
constraints.add(ConstraintFcn.TRACK_STATE, node=Node.MID, index=2)
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.END,
first_marker_idx=0,
second_marker_idx=1)
# Path constraint
x_bounds = QAndQDotBounds(biorbd_model)
# First node is free but mid and last are constrained to be exactly at a certain point.
# The cyclic penalty ensures that the first node and the last node are the same.
x_bounds[2:6, -1] = [1.57, 0, 0, 0]
# Initial guess
x_init = InitialGuess([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()))
# Define control path constraint
tau_min, tau_max, tau_init = -100, 100, 0
u_bounds = Bounds([tau_min] * biorbd_model.nbGeneralizedTorque(),
[tau_max] * biorbd_model.nbGeneralizedTorque())
u_init = InitialGuess([tau_init] * biorbd_model.nbGeneralizedTorque())
# ------------- #
# A phase transition loop constraint is treated as
# hard penalty (constraint) if weight is <= 0 [or if no weight is provided], or
# as a soft penalty (objective) otherwise
phase_transitions = PhaseTransitionList()
if loop_from_constraint:
phase_transitions.add(PhaseTransitionFcn.CYCLIC, weight=0)
else:
phase_transitions.add(PhaseTransitionFcn.CYCLIC, weight=10000)
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
phase_transitions=phase_transitions,
)
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(
biorbd_model_path: str,
n_shooting: int,
final_time: float,
interpolation_type: InterpolationType = InterpolationType.
CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT,
) -> OptimalControlProgram:
"""
Prepare the ocp for the specified interpolation type
Parameters
----------
biorbd_model_path: str
The path to the biorbd model
n_shooting: int
The number of shooting point
final_time: float
The movement time
interpolation_type: InterpolationType
The requested InterpolationType
Returns
-------
The OCP fully prepared and ready to be solved
"""
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
nq = biorbd_model.nbQ()
nqdot = biorbd_model.nbQdot()
ntau = biorbd_model.nbGeneralizedTorque()
tau_min, tau_max, tau_init = -100, 100, 0
# Add objective functions
objective_functions = Objective(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL,
key="tau")
# Dynamics
dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN)
# Constraints
constraints = ConstraintList()
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.START,
first_marker="m0",
second_marker="m1")
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.END,
first_marker="m0",
second_marker="m2")
# Path constraints
if interpolation_type == InterpolationType.CONSTANT:
x_min = [-100] * (nq + nqdot)
x_max = [100] * (nq + nqdot)
x_bounds = Bounds(x_min,
x_max,
interpolation=InterpolationType.CONSTANT)
u_min = [tau_min] * ntau
u_max = [tau_max] * ntau
u_bounds = Bounds(u_min,
u_max,
interpolation=InterpolationType.CONSTANT)
elif interpolation_type == InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT:
x_min = np.random.random((6, 3)) * (-10) - 5
x_max = np.random.random((6, 3)) * 10 + 5
x_bounds = Bounds(x_min,
x_max,
interpolation=InterpolationType.
CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT)
u_min = np.random.random((3, 3)) * tau_min + tau_min / 2
u_max = np.random.random((3, 3)) * tau_max + tau_max / 2
u_bounds = Bounds(u_min,
u_max,
interpolation=InterpolationType.
CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT)
elif interpolation_type == InterpolationType.LINEAR:
x_min = np.random.random((6, 2)) * (-10) - 5
x_max = np.random.random((6, 2)) * 10 + 5
x_bounds = Bounds(x_min, x_max, interpolation=InterpolationType.LINEAR)
u_min = np.random.random((3, 2)) * tau_min + tau_min / 2
u_max = np.random.random((3, 2)) * tau_max + tau_max / 2
u_bounds = Bounds(u_min, u_max, interpolation=InterpolationType.LINEAR)
elif interpolation_type == InterpolationType.EACH_FRAME:
x_min = np.random.random((nq + nqdot, n_shooting + 1)) * (-10) - 5
x_max = np.random.random((nq + nqdot, n_shooting + 1)) * 10 + 5
x_bounds = Bounds(x_min,
x_max,
interpolation=InterpolationType.EACH_FRAME)
u_min = np.random.random((ntau, n_shooting)) * tau_min + tau_min / 2
u_max = np.random.random((ntau, n_shooting)) * tau_max + tau_max / 2
u_bounds = Bounds(u_min,
u_max,
interpolation=InterpolationType.EACH_FRAME)
elif interpolation_type == InterpolationType.SPLINE:
spline_time = np.hstack(
(0, np.sort(np.random.random((3, )) * final_time), final_time))
x_min = np.random.random((nq + nqdot, 5)) * (-10) - 5
x_max = np.random.random((nq + nqdot, 5)) * 10 + 5
u_min = np.random.random((ntau, 5)) * tau_min + tau_min / 2
u_max = np.random.random((ntau, 5)) * tau_max + tau_max / 2
x_bounds = Bounds(x_min,
x_max,
interpolation=InterpolationType.SPLINE,
t=spline_time)
u_bounds = Bounds(u_min,
u_max,
interpolation=InterpolationType.SPLINE,
t=spline_time)
elif interpolation_type == InterpolationType.CUSTOM:
# The custom functions refer to the ones at the beginning of the file.
# For this particular instance, they emulate a Linear interpolation
extra_params_x = {"n_elements": nq + nqdot, "n_shooting": n_shooting}
extra_params_u = {"n_elements": ntau, "n_shooting": n_shooting}
x_bounds = Bounds(custom_x_bounds_min,
custom_x_bounds_max,
interpolation=InterpolationType.CUSTOM,
**extra_params_x)
u_bounds = Bounds(custom_u_bounds_min,
custom_u_bounds_max,
interpolation=InterpolationType.CUSTOM,
**extra_params_u)
else:
raise NotImplementedError("Not implemented yet")
# Initial guess
x_init = InitialGuess([0] * (nq + nqdot))
u_init = InitialGuess([tau_init] * ntau)
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
)
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 prepare_ocp_custom_objectives(
biorbd_model_path, ode_solver=OdeSolver.RK4()) -> OptimalControlProgram:
"""
Prepare the program
Parameters
----------
biorbd_model_path: str
The path of the biorbd model
ode_solver: OdeSolver
The type of ode solver used
Returns
-------
The ocp ready to be solved
"""
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
# Problem parameters
n_shooting = 30
final_time = 2
tau_min, tau_max, tau_init = -100, 100, 0
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE)
objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_TIME)
objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_COM_POSITION, node=2)
objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_COM_POSITION, node=3)
objective_functions.add(
custom_func_track_markers,
custom_type=ObjectiveFcn.Mayer,
node=Node.START,
quadratic=True,
first_marker="m0",
second_marker="m1",
weight=1000,
)
objective_functions.add(
custom_func_track_markers,
custom_type=ObjectiveFcn.Mayer,
node=Node.END,
quadratic=True,
first_marker="m0",
second_marker="m2",
weight=1000,
)
# Dynamics
dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN)
# 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,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
ode_solver=ode_solver,
)
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,
)
