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 test_simulate_from_initial_single_shoot():
# Load pendulum
bioptim_folder = TestUtils.bioptim_folder()
pendulum = TestUtils.load_module(bioptim_folder + "/examples/getting_started/example_save_and_load.py")
ocp = pendulum.prepare_ocp(
biorbd_model_path=bioptim_folder + "/examples/getting_started/pendulum.bioMod",
final_time=2,
n_shooting=10,
n_threads=4,
)
X = InitialGuess([-1, -2, 1, 0.5])
U = InitialGuess(np.array([[-0.1, 0], [1, 2]]).T, interpolation=InterpolationType.LINEAR)
sol = Solution(ocp, [X, U])
controls = sol.controls
sol = sol.integrate(shooting_type=Shooting.SINGLE_CONTINUOUS, keep_intermediate_points=True)
# Check some of the results
states = sol.states
q, qdot, tau = states["q"], states["qdot"], controls["tau"]
# initial and final position
np.testing.assert_almost_equal(q[:, 0], np.array((-1.0, -2.0)))
np.testing.assert_almost_equal(q[:, -1], np.array((-0.70545232, 2.02188073)))
# initial and final velocities
np.testing.assert_almost_equal(qdot[:, 0], np.array((1.0, 0.5)))
np.testing.assert_almost_equal(qdot[:, -1], np.array((1.21773723, -0.77896332)))
# initial and final controls
np.testing.assert_almost_equal(tau[:, 0], np.array((-0.1, 0.0)))
np.testing.assert_almost_equal(tau[:, -2], np.array((0.89, 1.8)))
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_initial_guess_spline():
nb_shoot = 10
spline_time = np.hstack((0.0, 1.0, 2.2, 6.0))
init_val = np.array(
[
[0.5, 0.6, 0.2, 0.8],
[0.4, 0.6, 0.8, 0.2],
[0.0, 0.3, 0.2, 0.5],
[0.5, 0.2, 0.9, 0.4],
[0.5, 0.6, 0.2, 0.8],
[0.5, 0.6, 0.2, 0.8],
]
)
nb_elements = init_val.shape[0]
# Raise if time is not sent
with pytest.raises(RuntimeError):
InitialGuess(init_val, interpolation=InterpolationType.SPLINE)
init = InitialGuess(init_val, t=spline_time, interpolation=InterpolationType.SPLINE)
init.check_and_adjust_dimensions(nb_elements, nb_shoot)
time_to_test = [0, nb_shoot // 3, nb_shoot // 2, nb_shoot]
expected_matrix = np.array(
[
[0.5, 0.4, 0.0, 0.5, 0.5, 0.5],
[0.33333333, 0.73333333, 0.23333333, 0.66666667, 0.33333333, 0.33333333],
[0.32631579, 0.67368421, 0.26315789, 0.79473684, 0.32631579, 0.32631579],
[0.8, 0.2, 0.5, 0.4, 0.8, 0.8],
]
).T
for i, t in enumerate(time_to_test):
expected_val = expected_matrix[:, i]
np.testing.assert_almost_equal(init.init.evaluate_at(t), expected_val)
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 __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_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 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_initial_guess_constant():
n_elements = 6
n_shoot = 10
init_val = np.random.random(n_elements, )
init = InitialGuess(init_val, interpolation=InterpolationType.CONSTANT)
init.check_and_adjust_dimensions(n_elements, n_shoot)
expected_val = init_val
for i in range(n_shoot):
np.testing.assert_almost_equal(init.init.evaluate_at(i), expected_val)
def prepare_ocp(biorbd_model_path, final_time, n_shooting, x_warm=None, use_sx=False, n_threads=1):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
tau_min, tau_max, tau_init = -50, 50, 0
muscle_min, muscle_max, muscle_init = 0, 1, 0.5
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=10)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE, weight=10)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_MUSCLES_CONTROL, weight=10)
objective_functions.add(
ObjectiveFcn.Mayer.SUPERIMPOSE_MARKERS, weight=100000, first_marker="target", second_marker="COM_hand"
)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.MUSCLE_DRIVEN, with_residual_torque=True)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
x_bounds[0][:, 0] = (1.0, 1.0, 0, 0)
# Initial guess
if x_warm is None:
x_init = InitialGuess([1.57] * biorbd_model.nbQ() + [0] * biorbd_model.nbQdot())
else:
x_init = InitialGuess(x_warm, interpolation=InterpolationType.EACH_FRAME)
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add(
[tau_min] * biorbd_model.nbGeneralizedTorque() + [muscle_min] * biorbd_model.nbMuscleTotal(),
[tau_max] * biorbd_model.nbGeneralizedTorque() + [muscle_max] * biorbd_model.nbMuscleTotal(),
)
u_init = InitialGuessList()
u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque() + [muscle_init] * biorbd_model.nbMuscleTotal())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
use_sx=use_sx,
n_threads=n_threads,
)
def test_initial_guess_each_frame():
n_elements = 6
n_shoot = 10
init_val = np.random.random((n_elements, n_shoot + 1))
init = InitialGuess(init_val, interpolation=InterpolationType.EACH_FRAME)
init.check_and_adjust_dimensions(n_elements, n_shoot)
for i in range(n_shoot + 1):
expected_val = init_val[:, i]
np.testing.assert_almost_equal(init.init.evaluate_at(i), expected_val)
def test_initial_guess_linear():
nb_elements = 6
nb_shoot = 10
init_val = np.random.random((nb_elements, 2))
init = InitialGuess(init_val, interpolation=InterpolationType.LINEAR)
init.check_and_adjust_dimensions(nb_elements, nb_shoot)
for i in range(nb_shoot + 1):
expected_val = init_val[:, 0] + (init_val[:, 1] - init_val[:, 0]) * i / nb_shoot
np.testing.assert_almost_equal(init.init.evaluate_at(i), expected_val)
def main():
# --- Load pendulum --- #
ocp = pendulum.prepare_ocp(
biorbd_model_path="models/pendulum.bioMod",
final_time=2,
n_shooting=10,
)
# Simulation the Initial Guess
# Interpolation: Constant
X = InitialGuess([0, 0, 0, 0])
U = InitialGuess([-1, 1])
sol_from_initial_guess = Solution(ocp, [X, U])
s = sol_from_initial_guess.integrate(
shooting_type=Shooting.SINGLE_CONTINUOUS)
print(
f"Final position of q from single shooting of initial guess = {s.states['q'][:, -1]}"
)
# Uncomment the next line to animate the integration
# s.animate()
# Interpolation: Each frame (for instance, values from a previous optimization or from measured data)
U = np.random.rand(2, 11)
U = InitialGuess(U, interpolation=InterpolationType.EACH_FRAME)
sol_from_initial_guess = Solution(ocp, [X, U])
s = sol_from_initial_guess.integrate(
shooting_type=Shooting.SINGLE_CONTINUOUS)
print(
f"Final position of q from single shooting of initial guess = {s.states['q'][:, -1]}"
)
# Uncomment the next line to animate the integration
# s.animate()
# Uncomment the following lines to graph the solution from initial guesses
# sol_from_initial_guess.graphs(shooting_type=Shooting.SINGLE_CONTINUOUS)
# sol_from_initial_guess.graphs(shooting_type=Shooting.MULTIPLE)
# Simulation of the solution. It is not the graph of the solution,
# it is the graph of a Runge Kutta from the solution
sol = ocp.solve()
s_single = sol.integrate(shooting_type=Shooting.SINGLE_CONTINUOUS)
# Uncomment the next line to animate the integration
# s_single.animate()
print(
f"Final position of q from single shooting of the solution = {s_single.states['q'][:, -1]}"
)
s_multiple = sol.integrate(shooting_type=Shooting.MULTIPLE,
keep_intermediate_points=True)
print(
f"Final position of q from multiple shooting of the solution = {s_multiple.states['q'][:, -1]}"
)
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 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 test_initial_guess_update():
# Load pendulum
bioptim_folder = TestUtils.bioptim_folder()
pendulum = TestUtils.load_module(
bioptim_folder +
"/examples/optimal_time_ocp/pendulum_min_time_Mayer.py")
ocp = pendulum.prepare_ocp(
biorbd_model_path=bioptim_folder +
"/examples/optimal_time_ocp/models/pendulum.bioMod",
final_time=2,
n_shooting=10,
)
np.testing.assert_almost_equal(ocp.nlp[0].x_init.init, np.zeros((4, 1)))
np.testing.assert_almost_equal(ocp.nlp[0].u_init.init, np.zeros((2, 1)))
idx = ocp.v.parameters_in_list.index("time")
np.testing.assert_almost_equal(
ocp.v.parameters_in_list[idx].initial_guess.init[0, 0], 2)
np.testing.assert_almost_equal(
ocp.v.init.init, np.concatenate((np.zeros(
(4 * 11 + 2 * 10, 1)), [[2]])))
wrong_new_x_init = InitialGuess([1] * 6)
new_x_init = InitialGuess([1] * 4)
wrong_new_u_init = InitialGuess([3] * 4)
new_u_init = InitialGuess([3] * 2)
new_time_init = InitialGuess([4])
# No name for the param
with pytest.raises(
ValueError,
match="update_initial_guess must specify a name for the parameters"
):
ocp.update_initial_guess(new_x_init, new_u_init, new_time_init)
new_time_init.name = "dumb name"
with pytest.raises(
ValueError,
match="update_initial_guess cannot declare new parameters"):
ocp.update_initial_guess(new_x_init, new_u_init, new_time_init)
new_time_init.name = "time"
with pytest.raises(RuntimeError):
ocp.update_initial_guess(new_x_init, wrong_new_u_init, new_time_init)
with pytest.raises(RuntimeError):
ocp.update_initial_guess(wrong_new_x_init, wrong_new_u_init,
new_time_init)
ocp.update_initial_guess(new_x_init, new_u_init, new_time_init)
np.testing.assert_almost_equal(ocp.nlp[0].x_init.init, np.ones((4, 1)))
np.testing.assert_almost_equal(ocp.nlp[0].u_init.init, np.ones((2, 1)) * 3)
idx = ocp.v.parameters_in_list.index("time")
np.testing.assert_almost_equal(
ocp.v.parameters_in_list[idx].initial_guess.init[0, 0], 4)
np.testing.assert_almost_equal(
ocp.v.init.init,
np.array([[1, 1, 1, 1] * 11 + [3, 3] * 10 + [4]]).T)
def _set_initial_guess(self, init_file):
if init_file is None:
x_init = np.zeros((self.n_q * 2, 1))
x_init[:self.n_q, 0] = self.violin.q(self.bow_starting)
u_init = np.zeros((self.n_tau + self.n_mus, 1))
self.x_init = InitialGuess(x_init)
self.u_init = InitialGuess(u_init)
else:
_, sol = ViolinOcp.load(init_file)
self.x_init = InitialGuess(
sol.states["all"], interpolation=InterpolationType.EACH_FRAME)
self.u_init = InitialGuess(
sol.controls["all"][:, :-1],
interpolation=InterpolationType.EACH_FRAME)
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: 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 test_initial_guess_constant_with_first_and_last_different():
nb_elements = 6
nb_shoot = 10
init_val = np.random.random((nb_elements, 3))
init = InitialGuess(init_val, interpolation=InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT)
init.check_and_adjust_dimensions(nb_elements, nb_shoot)
for i in range(nb_shoot + 1):
if i == 0:
expected_val = init_val[:, 0]
elif i == nb_shoot:
expected_val = init_val[:, 2]
else:
expected_val = init_val[:, 1]
np.testing.assert_almost_equal(init.init.evaluate_at(i), expected_val)
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 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_short_ocp(biorbd_model: biorbd.Model, final_time: float, n_shooting: int):
"""
Prepare to build a blank short ocp to use single shooting bioptim function
Parameters
----------
biorbd_model: biorbd.Model
biorbd model build with the bioMod
final_time: float
The time at the final node
n_shooting: int
The number of shooting points
Returns
-------
The blank OptimalControlProgram
"""
# Add objective functions
objective_functions = ObjectiveList()
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.MUSCLE_ACTIVATIONS_DRIVEN)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([0] * biorbd_model.nbMuscles(), [1] * biorbd_model.nbMuscles())
x_init = InitialGuess([0] * biorbd_model.nbQ() * 2)
u_init = InitialGuess([0] * biorbd_model.nbMuscles())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
use_sx=True,
)
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 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_single_shooting(
biorbd_model_path: str,
n_shooting: int,
final_time: float,
ode_solver: OdeSolver,
n_threads: int = 1,
use_sx: bool = False,
) -> OptimalControlProgram:
"""
Prepare the ss
Returns
-------
The OptimalControlProgram ready to be solved
"""
biorbd_model = biorbd.Model(biorbd_model_path)
# Dynamics
dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN,
implicit_dynamics=False,
implicit_soft_contacts=False)
# Initial guess
x_init = InitialGuess([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()))
# Problem parameters
tau_min, tau_max, tau_init = -100, 100, 0
u_init = InitialGuess([tau_init] * biorbd_model.nbGeneralizedTorque())
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
ode_solver=ode_solver,
use_sx=use_sx,
n_threads=n_threads,
)
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 test_initial_guess_custom():
nb_elements = 6
nb_shoot = 10
def custom_bound_func(current_shooting, val, total_shooting):
# Linear interpolation created with custom bound function
return val[:, 0] + (val[:, 1] - val[:, 0]) * current_shooting / total_shooting
init_val = np.random.random((nb_elements, 2))
init = InitialGuess(
custom_bound_func,
interpolation=InterpolationType.CUSTOM,
val=init_val,
total_shooting=nb_shoot,
)
init.check_and_adjust_dimensions(nb_elements, nb_shoot)
for i in range(nb_shoot + 1):
expected_val = init_val[:, 0] + (init_val[:, 1] - init_val[:, 0]) * i / nb_shoot
np.testing.assert_almost_equal(init.init.evaluate_at(i), expected_val)
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,
)
