def test_track_marker_2D_pendulum(ode_solver):
# Load muscle_activations_contact_tracker
from bioptim.examples.torque_driven_ocp import track_markers_2D_pendulum as ocp_module
bioptim_folder = os.path.dirname(ocp_module.__file__)
ode_solver = ode_solver()
# Define the problem
model_path = bioptim_folder + "/models/pendulum.bioMod"
biorbd_model = biorbd.Model(model_path)
final_time = 2
n_shooting = 30
# Generate data to fit
np.random.seed(42)
markers_ref = np.random.rand(3, 2, n_shooting + 1)
tau_ref = np.random.rand(2, n_shooting)
if isinstance(ode_solver, OdeSolver.IRK):
tau_ref = tau_ref * 5
ocp = ocp_module.prepare_ocp(biorbd_model, final_time, n_shooting, markers_ref, tau_ref, ode_solver=ode_solver)
sol = ocp.solve()
# Check constraints
g = np.array(sol.constraints)
np.testing.assert_equal(g.shape, (n_shooting * 4, 1))
np.testing.assert_almost_equal(g, np.zeros((n_shooting * 4, 1)))
# Check some of the results
q, qdot, tau = sol.states["q"], sol.states["qdot"], sol.controls["tau"]
if isinstance(ode_solver, OdeSolver.IRK):
# Check objective function value
f = np.array(sol.cost)
np.testing.assert_equal(f.shape, (1, 1))
np.testing.assert_almost_equal(f[0, 0], 290.6751231)
# initial and final position
np.testing.assert_almost_equal(q[:, 0], np.array((0, 0)))
np.testing.assert_almost_equal(q[:, -1], np.array((0.64142484, 2.85371719)))
# initial and final velocities
np.testing.assert_almost_equal(qdot[:, 0], np.array((0, 0)))
np.testing.assert_almost_equal(qdot[:, -1], np.array((3.46921861, 3.24168308)))
# initial and final controls
np.testing.assert_almost_equal(tau[:, 0], np.array((9.11770196, -13.83677175)))
np.testing.assert_almost_equal(tau[:, -2], np.array((1.16836132, 4.77230548)))
elif isinstance(ode_solver, OdeSolver.RK8):
pass
else:
# Check objective function value
f = np.array(sol.cost)
np.testing.assert_equal(f.shape, (1, 1))
np.testing.assert_almost_equal(f[0, 0], 281.8560713312711)
# initial and final position
np.testing.assert_almost_equal(q[:, 0], np.array((0, 0)))
np.testing.assert_almost_equal(q[:, -1], np.array((0.8367364, 3.37533055)))
# initial and final velocities
np.testing.assert_almost_equal(qdot[:, 0], np.array((0, 0)))
np.testing.assert_almost_equal(qdot[:, -1], np.array((3.2688391, 3.88242643)))
# initial and final controls
np.testing.assert_almost_equal(tau[:, 0], np.array((6.93890241, -12.76433504)))
np.testing.assert_almost_equal(tau[:, -2], np.array((0.13156876, 0.93749913)))
# save and load
TestUtils.save_and_load(sol, ocp, False)
# simulate
TestUtils.simulate(sol)
def prepare_ocp(
biorbd_model_path: str,
final_time: float,
n_shooting: int,
ode_solver: OdeSolver = OdeSolver.RK4(),
weight: float = 1,
) -> OptimalControlProgram:
"""
Prepare the optimal control program
Parameters
----------
biorbd_model_path: str
The path to the bioMod
final_time: float
The initial guess for the final time
n_shooting: int
The number of shooting points
ode_solver: OdeSolver
The ode solver to use
weight: float
The weighting of the minimize time objective function
Returns
-------
The OptimalControlProgram ready to be solved
"""
biorbd_model = biorbd.Model(biorbd_model_path)
# Add objective functions
objective_functions = ObjectiveList()
# A weight of -1 will maximize time
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TIME, weight=weight)
# Dynamics
dynamics = DynamicsList()
expand = False if isinstance(ode_solver, OdeSolver.IRK) else True
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand)
# Path constraint
n_q = biorbd_model.nbQ()
n_qdot = biorbd_model.nbQdot()
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
x_bounds[0][:, [0, -1]] = 0
x_bounds[0][n_q - 1, -1] = 3.14
# Initial guess
x_init = InitialGuessList()
x_init.add([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 = BoundsList()
u_bounds.add([tau_min] * n_tau, [tau_max] * n_tau)
u_bounds[0][n_tau - 1, :] = 0
u_init = InitialGuessList()
u_init.add([tau_init] * n_tau)
# ------------- #
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(
final_time: list,
time_min: list,
time_max: list,
n_shooting: list,
biorbd_model_path: str = "cube.bioMod",
ode_solver: OdeSolver = OdeSolver.RK4(),
) -> OptimalControlProgram:
"""
Prepare the optimal control program. This example can be called as a normal single phase (all list len equals to 1)
or as a three phases (all list len equals to 3)
Parameters
----------
final_time: list
The initial guess for the final time of each phase
time_min: list
The minimal time for each phase
time_max: list
The maximal time for each phase
n_shooting: list
The number of shooting points for each phase
biorbd_model_path: str
The path to the bioMod
ode_solver: OdeSolver
The ode solver to use
Returns
-------
The multiphase OptimalControlProgram ready to be solved
"""
# --- Options --- #
n_phases = len(n_shooting)
if n_phases != 1 and n_phases != 3:
raise RuntimeError("Number of phases must be 1 to 3")
# 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_CONTROL, key="tau", weight=100, phase=0)
if n_phases == 3:
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, phase=1)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, phase=2)
# Dynamics
dynamics = DynamicsList()
expand = False if isinstance(ode_solver, OdeSolver.IRK) else True
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, phase=0, expand=expand)
if n_phases == 3:
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, phase=1, expand=expand)
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, phase=2, expand=expand)
# 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.TIME_CONSTRAINT, node=Node.END, min_bound=time_min[0], max_bound=time_max[0], phase=0)
if n_phases == 3:
constraints.add(
ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m1", phase=1
)
constraints.add(
ConstraintFcn.TIME_CONSTRAINT, node=Node.END, min_bound=time_min[1], max_bound=time_max[1], 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, min_bound=time_min[2], max_bound=time_max[2], phase=2
)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) # Phase 0
if n_phases == 3:
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
if n_phases == 3:
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()))
if n_phases == 3:
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())
if n_phases == 3:
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())
if n_phases == 3:
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
# ------------- #
return OptimalControlProgram(
biorbd_model[:n_phases],
dynamics,
n_shooting,
final_time[:n_phases],
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
)
def test_torque_derivative_driven_implicit(with_contact, cx):
# Prepare the program
nlp = NonLinearProgram()
nlp.model = biorbd.Model(
TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod"
)
nlp.ns = 5
nlp.cx = cx
nlp.phase_idx = 0
nlp.x_bounds = np.zeros((nlp.model.nbQ() * 4, 1))
nlp.u_bounds = np.zeros((nlp.model.nbQ(), 2))
ocp = OptimalControlProgram(nlp)
nlp.control_type = ControlType.CONSTANT
NonLinearProgram.add(
ocp,
"dynamics_type",
Dynamics(DynamicsFcn.TORQUE_DERIVATIVE_DRIVEN, with_contact=with_contact, implicit_dynamics=True),
False,
)
# Prepare the dynamics
ConfigureProblem.initialize(ocp, nlp)
# Test the results
np.random.seed(42)
states = np.random.rand(nlp.states.shape, nlp.ns)
controls = np.random.rand(nlp.controls.shape, nlp.ns)
params = np.random.rand(nlp.parameters.shape, nlp.ns)
x_out = np.array(nlp.dynamics_func(states, controls, params))
if with_contact:
contact_out = np.array(nlp.contact_forces_func(states, controls, params))
np.testing.assert_almost_equal(
x_out[:, 0],
[
0.6118529,
0.785176,
0.6075449,
0.8083973,
0.3886773,
0.5426961,
0.7722448,
0.7290072,
0.8631034,
0.3251833,
0.1195942,
0.4937956,
0.0314292,
0.2492922,
0.2897515,
0.8714606,
],
)
np.testing.assert_almost_equal(contact_out[:, 0], [-2.444071, 128.8816865, 2.7245124])
else:
np.testing.assert_almost_equal(
x_out[:, 0],
[
0.6118529,
0.785176,
0.6075449,
0.8083973,
0.3886773,
0.5426961,
0.7722448,
0.7290072,
0.8631034,
0.3251833,
0.1195942,
0.4937956,
0.0314292,
0.2492922,
0.2897515,
0.8714606,
],
)
def test_torque_activation_driven(with_contact, with_external_force, cx):
# Prepare the program
nlp = NonLinearProgram()
nlp.model = biorbd.Model(
TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod"
)
nlp.ns = 5
nlp.cx = cx
nlp.x_bounds = np.zeros((nlp.model.nbQ() * 2, 1))
nlp.u_bounds = np.zeros((nlp.model.nbQ(), 1))
ocp = OptimalControlProgram(nlp)
nlp.control_type = ControlType.CONSTANT
NonLinearProgram.add(
ocp, "dynamics_type", Dynamics(DynamicsFcn.TORQUE_ACTIVATIONS_DRIVEN, with_contact=with_contact), False
)
np.random.seed(42)
if with_external_force:
external_forces = [np.random.rand(6, nlp.model.nbSegment(), nlp.ns)]
nlp.external_forces = BiorbdInterface.convert_array_to_external_forces(external_forces)[0]
# Prepare the dynamics
ConfigureProblem.initialize(ocp, nlp)
# Test the results
states = np.random.rand(nlp.states.shape, nlp.ns)
controls = np.random.rand(nlp.controls.shape, nlp.ns)
params = np.random.rand(nlp.parameters.shape, nlp.ns)
x_out = np.array(nlp.dynamics_func(states, controls, params))
if with_contact:
contact_out = np.array(nlp.contact_forces_func(states, controls, params))
if with_external_force:
np.testing.assert_almost_equal(
x_out[:, 0],
[0.8631, 0.32518, 0.11959, 0.4938, 19.01887, 18.51503, -53.08574, 58.48719],
decimal=5,
)
np.testing.assert_almost_equal(contact_out[:, 0], [109.8086936, 3790.3932439, -3571.7858574])
else:
np.testing.assert_almost_equal(
x_out[:, 0],
[0.61185289, 0.78517596, 0.60754485, 0.80839735, 0.78455384, -0.16844256, -1.56184114, 1.97658587],
decimal=5,
)
np.testing.assert_almost_equal(contact_out[:, 0], [-7.88958997, 329.70828173, -263.55516549])
else:
if with_external_force:
np.testing.assert_almost_equal(
x_out[:, 0],
[
8.63103426e-01,
3.25183322e-01,
1.19594246e-01,
4.93795596e-01,
1.73558072e01,
-4.69891264e01,
1.81396922e02,
3.61170139e03,
],
decimal=5,
)
else:
np.testing.assert_almost_equal(
x_out[:, 0],
[
6.11852895e-01,
7.85175961e-01,
6.07544852e-01,
8.08397348e-01,
-2.38262975e01,
-5.82033454e01,
1.27439020e02,
3.66531163e03,
],
decimal=5,
)
def prepare_ocp(
biorbd_model_path: str,
n_shooting: int,
final_time: float,
ode_solver: OdeSolver = OdeSolver.RK4()
) -> OptimalControlProgram:
"""
Prepare the ocp
Parameters
----------
biorbd_model_path: str
The path to the bioMod file
n_shooting: int
The number of shooting points
final_time: float
The time at the final node
ode_solver: OdeSolver
The ode solver to use
Returns
-------
The OptimalControlProgram ready to be solved
"""
biorbd_model = biorbd.Model(biorbd_model_path)
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_MARKERS,
marker_index=1,
weight=-1)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL,
key="tau",
node=Node.ALL_SHOOTING,
weight=100)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
# Define control path constraint
n_tau = biorbd_model.nbGeneralizedTorque(
) # biorbd_model.nbGeneralizedTorque()
tau_min, tau_max, tau_init = -100, 100, 0
u_bounds = BoundsList()
u_bounds.add([tau_min] * n_tau, [tau_max] * n_tau)
# Initial guesses
# TODO put this in a function defined before and explain what it does, and what are the variables
x = np.vstack((np.zeros(
(biorbd_model.nbQ(), 2)), np.ones((biorbd_model.nbQdot(), 2))))
Arm_init_D = np.zeros((3, 2))
Arm_init_D[1, 0] = 0
Arm_init_D[1, 1] = -np.pi + 0.01
Arm_init_G = np.zeros((3, 2))
Arm_init_G[1, 0] = 0
Arm_init_G[1, 1] = np.pi - 0.01
for i in range(2):
Arm_Quat_D = eul2quat(Arm_init_D[:, i])
Arm_Quat_G = eul2quat(Arm_init_G[:, i])
x[6:9, i] = Arm_Quat_D[1:]
x[12, i] = Arm_Quat_D[0]
x[9:12, i] = Arm_Quat_G[1:]
x[13, i] = Arm_Quat_G[0]
x_init = InitialGuessList()
x_init.add(x, interpolation=InterpolationType.LINEAR)
u_init = InitialGuessList()
u_init.add([tau_init] * n_tau)
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(
biorbd_model_path: str,
n_shooting: int,
final_time: float,
initial_guess: InterpolationType = InterpolationType.CONSTANT,
ode_solver=OdeSolver.RK4(),
) -> OptimalControlProgram:
"""
Prepare the program
Parameters
----------
biorbd_model_path: str
The path of the biorbd model
n_shooting: int
The number of shooting points
final_time: float
The time at the final node
initial_guess: InterpolationType
The type of interpolation to use for the initial guesses
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)
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",
weight=100)
# Dynamics
expand = False if isinstance(ode_solver, OdeSolver.IRK) else True
dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN, expand=expand)
# 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 constraint and control path constraints
x_bounds = QAndQDotBounds(biorbd_model)
x_bounds[1:6, [0, -1]] = 0
x_bounds[2, -1] = 1.57
u_bounds = Bounds([tau_min] * ntau, [tau_max] * ntau)
# Initial guesses
t = None
extra_params_x = {}
extra_params_u = {}
if initial_guess == InterpolationType.CONSTANT:
x = [0] * (nq + nqdot)
u = [tau_init] * ntau
elif initial_guess == InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT:
x = np.array([[1.0, 0.0, 0.0, 0, 0, 0], [1.5, 0.0, 0.785, 0, 0, 0],
[2.0, 0.0, 1.57, 0, 0, 0]]).T
u = np.array([[1.45, 9.81, 2.28], [0, 9.81, 0], [-1.45, 9.81,
-2.28]]).T
elif initial_guess == InterpolationType.LINEAR:
x = np.array([[1.0, 0.0, 0.0, 0, 0, 0], [2.0, 0.0, 1.57, 0, 0, 0]]).T
u = np.array([[1.45, 9.81, 2.28], [-1.45, 9.81, -2.28]]).T
elif initial_guess == InterpolationType.EACH_FRAME:
x = np.random.random((nq + nqdot, n_shooting + 1))
u = np.random.random((ntau, n_shooting))
elif initial_guess == InterpolationType.SPLINE:
# Bound spline assume the first and last point are 0 and final respectively
t = np.hstack((0, np.sort(np.random.random(
(3, )) * final_time), final_time))
x = np.random.random((nq + nqdot, 5))
u = np.random.random((ntau, 5))
elif initial_guess == InterpolationType.CUSTOM:
# The custom function refers to the one at the beginning of the file. It emulates a Linear interpolation
x = custom_init_func
u = custom_init_func
extra_params_x = {
"my_values": np.random.random((nq + nqdot, 2)),
"n_shooting": n_shooting
}
extra_params_u = {
"my_values": np.random.random((ntau, 2)),
"n_shooting": n_shooting
}
else:
raise RuntimeError("Initial guess not implemented yet")
x_init = InitialGuess(x,
t=t,
interpolation=initial_guess,
**extra_params_x)
u_init = InitialGuess(u,
t=t,
interpolation=initial_guess,
**extra_params_u)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
)
def prepare_ocp(biorbd_model_path: str = "cube.bioMod",
ode_solver: OdeSolver = OdeSolver.RK4(),
long_optim: bool = False) -> OptimalControlProgram:
"""
Prepare the ocp
Parameters
----------
biorbd_model_path: str
The path to the bioMod
ode_solver: OdeSolver
The ode solve to use
long_optim: bool
If the solver should solve the precise optimization (500 shooting points) or the approximate (50 points)
Returns
-------
The OptimalControlProgram ready to be solved
"""
biorbd_model = (biorbd.Model(biorbd_model_path),
biorbd.Model(biorbd_model_path),
biorbd.Model(biorbd_model_path))
# Problem parameters
if long_optim:
n_shooting = (100, 300, 100)
else:
n_shooting = (20, 30, 20)
final_time = (2, 5, 4)
tau_min, tau_max, tau_init = -100, 100, 0
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL,
key="tau",
weight=100,
phase=0)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL,
key="tau",
weight=100,
phase=1)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL,
key="tau",
weight=100,
phase=2)
objective_functions.add(
minimize_difference,
custom_type=ObjectiveFcn.Mayer,
node=Node.TRANSITION,
weight=100,
phase=1,
quadratic=True,
)
# Dynamics
dynamics = DynamicsList()
expand = False if isinstance(ode_solver, OdeSolver.IRK) else True
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand)
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand)
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand)
# 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)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0]))
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0]))
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0]))
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())
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
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,
marker_velocity_or_displacement: str,
marker_in_first_coordinates_system: bool,
control_type: ControlType,
ode_solver: OdeSolver = OdeSolver.RK4(),
) -> OptimalControlProgram:
"""
Prepare an ocp that targets some marker velocities, either by finite differences or by jacobian
Parameters
----------
biorbd_model_path: str
The path to the bioMod file
final_time: float
The time of the final node
n_shooting: int
The number of shooting points
marker_velocity_or_displacement: str
which type of tracking: finite difference ('disp') or by jacobian ('velo')
marker_in_first_coordinates_system: bool
If the marker to track should be expressed in the global or local reference frame
control_type: ControlType
The type of controls
ode_solver: OdeSolver
The ode solver to use
Returns
-------
The OptimalControlProgram ready to be solved
"""
biorbd_model = biorbd.Model(biorbd_model_path)
# Add objective functions
if marker_in_first_coordinates_system:
# Marker should follow this segment (0 velocity when compare to this one)
coordinates_system_idx = 0
else:
# Marker should be static in global reference frame
coordinates_system_idx = None
objective_functions = ObjectiveList()
if marker_velocity_or_displacement == "disp":
objective_functions.add(
ObjectiveFcn.Lagrange.MINIMIZE_MARKERS,
derivative=True,
reference_jcs=coordinates_system_idx,
marker_index=6,
weight=1000,
)
elif marker_velocity_or_displacement == "velo":
objective_functions.add(
ObjectiveFcn.Lagrange.MINIMIZE_MARKERS_VELOCITY, node=Node.ALL, marker_index=6, weight=1000
)
else:
raise RuntimeError(
f"Wrong choice of marker_velocity_or_displacement, actual value is "
f"{marker_velocity_or_displacement}, should be 'velo' or 'disp'."
)
# Make sure the segments actually moves (in order to test the relative speed objective)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE, key="qdot", node=Node.ALL, index=[2, 3], weight=-1)
# Dynamics
dynamics = DynamicsList()
expand = False if isinstance(ode_solver, OdeSolver.IRK) else True
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand)
# Path constraint
nq = biorbd_model.nbQ()
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
x_bounds[0].min[nq:, :] = -10
x_bounds[0].max[nq:, :] = 10
# Initial guess
x_init = InitialGuessList()
x_init.add([1.5, 1.5, 0.0, 0.0, 0.7, 0.7, 0.6, 0.6])
# Define control path constraint
tau_min, tau_max, tau_init = -100, 100, 0
u_bounds = BoundsList()
u_bounds.add([tau_min] * biorbd_model.nbGeneralizedTorque(), [tau_max] * biorbd_model.nbGeneralizedTorque())
u_init = InitialGuessList()
u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque())
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
control_type=control_type,
ode_solver=ode_solver,
)
def prepare_ocp(
biorbd_model_path: str,
final_time: float,
n_shooting: int,
time_min: float,
time_max: float,
ode_solver: OdeSolver = OdeSolver.RK4(),
) -> OptimalControlProgram:
"""
Prepare the optimal control program
Parameters
----------
biorbd_model_path: str
The path to the bioMod
final_time: float
The initial guess for the final time
n_shooting: int
The number of shooting points
time_min: float
The minimal time the phase can have
time_max: float
The maximal time the phase can have
ode_solver: OdeSolver
The ode solver to use
Returns
-------
The OptimalControlProgram ready to be solved
"""
biorbd_model = biorbd.Model(biorbd_model_path)
# Add objective functions
objective_functions = Objective(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL,
key="tau")
# Dynamics
expand = False if isinstance(ode_solver, OdeSolver.IRK) else True
dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN, expand=expand)
# Constraints
constraints = Constraint(ConstraintFcn.TIME_CONSTRAINT,
node=Node.END,
min_bound=time_min,
max_bound=time_max)
# Path constraint
n_q = biorbd_model.nbQ()
n_qdot = biorbd_model.nbQdot()
x_bounds = QAndQDotBounds(biorbd_model)
x_bounds[:, [0, -1]] = 0
x_bounds[n_q - 1, -1] = 3.14
# Initial guess
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,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
)
def main():
biorbd_model_path = "models/cart_pendulum.bioMod"
biorbd_model = biorbd.Model(biorbd_model_path)
solver = Solver.ACADOS() # or Solver.IPOPT()
final_time = 5
n_shoot_per_second = 100
window_len = 10
window_duration = 1 / n_shoot_per_second * window_len
n_frames_total = final_time * n_shoot_per_second - window_len - 1
x0 = np.array([0, np.pi / 2, 0, 0])
noise_std = 0.05 # STD of noise added to measurements
torque_max = 2 # Max torque applied to the model
states, markers, markers_noised, controls = generate_data(
biorbd_model,
final_time,
x0,
torque_max,
n_shoot_per_second * final_time,
noise_std,
show_plots=False)
x_init = np.zeros((biorbd_model.nbQ() * 2, window_len + 1))
u_init = np.zeros((biorbd_model.nbQ(), window_len))
torque_max = 5 # Give a bit of slack on the max torque
biorbd_model = biorbd.Model(biorbd_model_path)
mhe = prepare_mhe(
biorbd_model,
window_len=window_len,
window_duration=window_duration,
max_torque=torque_max,
x_init=x_init,
u_init=u_init,
)
def update_functions(mhe, t, _):
def target(i: int):
return markers_noised[:, :, i:i + window_len + 1]
mhe.update_objectives_target(target=target(t), list_index=0)
return t < n_frames_total # True if there are still some frames to reconstruct
# Solve the program
sol = mhe.solve(update_functions, **get_solver_options(solver))
print("ACADOS with Bioptim")
print(f"Window size of MHE : {window_duration} s.")
print(f"New measurement every : {1 / n_shoot_per_second} s.")
print(
f"Average time per iteration of MHE : {sol.solver_time_to_optimize / (n_frames_total - 1)} s."
)
print(
f"Average real time per iteration of MHE : {sol.real_time_to_optimize / (n_frames_total - 1)} s."
)
print(
f"Norm of the error on state = {np.linalg.norm(states[:, :n_frames_total] - sol.states['all'])}"
)
markers_estimated = states_to_markers(biorbd_model, sol.states["all"])
plt.plot(
markers_noised[1, :, :n_frames_total].T,
markers_noised[2, :, :n_frames_total].T,
"x",
label="Noised markers trajectory",
)
plt.gca().set_prop_cycle(None)
plt.plot(markers[1, :, :n_frames_total].T,
markers[2, :, :n_frames_total].T,
label="True markers trajectory")
plt.gca().set_prop_cycle(None)
plt.plot(markers_estimated[1, :, :].T,
markers_estimated[2, :, :].T,
"o",
label="Estimated marker trajectory")
plt.legend()
plt.figure()
plt.plot(sol.states["all"].T, "--", label="States estimate")
plt.plot(states[:, :n_frames_total].T, label="State truth")
plt.legend()
plt.show()
sol.animate()
def prepare_ocp(biorbd_model_path,
phase_time,
n_shooting,
muscle_activations_ref,
contact_forces_ref,
ode_solver=OdeSolver.RK4()):
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
tau_min, tau_max, tau_init = -500, 500, 0
activation_min, activation_max, activation_init = 0, 1, 0.5
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.TRACK_CONTROL,
key="muscles",
target=muscle_activations_ref)
objective_functions.add(ObjectiveFcn.Lagrange.TRACK_CONTACT_FORCES,
target=contact_forces_ref)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE,
key="qdot",
weight=0.001)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE,
key="q",
weight=0.001)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL,
key="muscles",
weight=0.001)
# objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="torque", weight=0.001)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.MUSCLE_DRIVEN,
with_residual_torque=True,
with_contact=True)
# Path constraint
n_q = biorbd_model.nbQ()
n_qdot = n_q
pose_at_first_node = [0, 0, -0.75, 0.75]
# Initialize x_bounds
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
x_bounds[0][:, 0] = pose_at_first_node + [0] * n_qdot
# Initial guess
x_init = InitialGuessList()
x_init.add(pose_at_first_node + [0] * n_qdot)
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add(
[tau_min] * biorbd_model.nbGeneralizedTorque() +
[activation_min] * biorbd_model.nbMuscleTotal(),
[tau_max] * biorbd_model.nbGeneralizedTorque() +
[activation_max] * biorbd_model.nbMuscleTotal(),
)
u_init = InitialGuessList()
u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque() +
[activation_init] * biorbd_model.nbMuscleTotal())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
phase_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions=objective_functions,
ode_solver=ode_solver,
)
def prepare_ocp(
biorbd_model_path: str, final_time: float, n_shooting: int, ode_solver: OdeSolver = OdeSolver.RK4()
) -> OptimalControlProgram:
"""
Prepare the ocp
Parameters
----------
biorbd_model_path: str
The path to the model
final_time: float
The time of the final node
n_shooting: int
The number of shooting points
ode_solver:
The ode solver to use
Returns
-------
The OptimalControlProgram ready to be solved
"""
biorbd_model = biorbd.Model(biorbd_model_path)
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100)
# Dynamics
dynamics = DynamicsList()
expand = False if isinstance(ode_solver, OdeSolver.IRK) else True
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand)
# Constraints
constraints = ConstraintList()
constraints.add(ConstraintFcn.TRACK_SEGMENT_WITH_CUSTOM_RT, node=Node.ALL, segment="seg_rt", rt=0)
# Path constraint
nq = biorbd_model.nbQ()
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
x_bounds[0][2, [0, -1]] = [-1.57, 1.57]
x_bounds[0][nq:, [0, -1]] = 0
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()))
# Define control path constraint
tau_min, tau_max, tau_init = -100, 100, 0
u_bounds = BoundsList()
u_bounds.add([tau_min] * biorbd_model.nbGeneralizedTorque(), [tau_max] * biorbd_model.nbGeneralizedTorque())
u_init = InitialGuessList()
u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
)
def prepare_ocp(
biorbd_model_path: str = "models/cube.bioMod",
ode_solver: OdeSolver = OdeSolver.RK4()
) -> OptimalControlProgram:
"""
Parameters
----------
biorbd_model_path: str
The path to the bioMod
ode_solver: OdeSolver
The type of ode solver used
Returns
-------
The ocp ready to be solved
"""
# Model path
biorbd_model = (
biorbd.Model(biorbd_model_path),
biorbd.Model(biorbd_model_path),
biorbd.Model(biorbd_model_path),
biorbd.Model(biorbd_model_path),
)
# Problem parameters
n_shooting = (20, 20, 20, 20)
final_time = (2, 5, 4, 2)
tau_min, tau_max, tau_init = -100, 100, 0
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL,
key="tau",
weight=100,
phase=0)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL,
key="tau",
weight=100,
phase=1)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL,
key="tau",
weight=100,
phase=2)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL,
key="tau",
weight=100,
phase=3)
# Dynamics
dynamics = DynamicsList()
expand = False if isinstance(ode_solver, OdeSolver.IRK) else True
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand)
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand)
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand)
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand)
# 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.SUPERIMPOSE_MARKERS,
node=Node.END,
first_marker="m0",
second_marker="m1",
phase=3)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0]))
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0]))
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0]))
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0]))
x_bounds[0][[1, 3, 4, 5], 0] = 0
x_bounds[-1][[1, 3, 4, 5], -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()))
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_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())
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
"""
By default, all phase transitions (here phase 0 to phase 1, phase 1 to phase 2 and phase 2 to phase 3)
are continuous. In the event that one (or more) phase transition(s) is desired to be discontinuous,
as for example IMPACT or CUSTOM can be used as below.
"phase_pre_idx" corresponds to the index of the phase preceding the transition.
IMPACT will cause an impact related discontinuity when defining one or more contact points in the model.
CUSTOM will allow to call the custom function previously presented in order to have its own phase transition.
Finally, if you want a phase transition (continuous or not) between the last and the first phase (cyclicity)
you can use the dedicated PhaseTransitionFcn.Cyclic or use a continuous set at the last phase_pre_idx.
If for some reason, you don't want the phase transition to be hard constraint, you can specify a weight higher than
zero. It will thereafter be treated as a Mayer objective function with the specified weight.
"""
phase_transitions = PhaseTransitionList()
phase_transitions.add(PhaseTransitionFcn.CONTINUOUS,
phase_pre_idx=0,
states_mapping=BiMapping(range(3), range(3)))
phase_transitions.add(PhaseTransitionFcn.IMPACT, phase_pre_idx=1)
phase_transitions.add(custom_phase_transition, phase_pre_idx=2, coef=0.5)
phase_transitions.add(PhaseTransitionFcn.CYCLIC)
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,
)
import biorbd_casadi as biorbd
import numpy as np
m = biorbd.Model("../models/jumper2contacts.bioMod")
q0 = np.array([0, 0, -0.5336, 0, 1.4, 0, 1.4, 0.8, -0.9, 0.47, 0.8, -0.9, 0.47])
q1 = np.array([-0.12, -0.23, -1.10, 0, 1.85, 0, 1.85, 2.06, -1.67, 0.55, 2.06, -1.67, 0.55])
q_mat = [q0, q1]
torque_act = np.array([0, 0, 0, 0, 1, 0, 1, -1, 1, -1, -1, 1, -1])
qdot_null = np.zeros((13,))
qdot0 = np.array(
[
-0.69722964,
-1.33958085,
-1.8479633,
0.0,
-0.64892519,
-0.0,
-0.64892519,
6.15902598,
-5.95116095,
1.63984244,
6.15902598,
-5.95116095,
1.63984244,
]
)
def prepare_ocp(biorbd_model_path,
phase_time,
n_shooting,
min_bound,
ode_solver=OdeSolver.RK4()):
biorbd_model = biorbd.Model(biorbd_model_path)
torque_min, torque_max, torque_init = -500, 500, 0
activation_min, activation_max, activation_init = 0, 1, 0.5
dof_mapping = BiMappingList()
dof_mapping.add("tau", [None, None, None, 0], [3])
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT,
weight=-1)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.MUSCLE_DRIVEN,
with_excitations=True,
with_torque=True,
with_contact=True)
# Constraints
constraints = ConstraintList()
constraints.add(
ConstraintFcn.TRACK_CONTACT_FORCES,
min_bound=min_bound,
max_bound=np.inf,
node=Node.ALL_SHOOTING,
contact_index=1,
)
constraints.add(
ConstraintFcn.TRACK_CONTACT_FORCES,
min_bound=min_bound,
max_bound=np.inf,
node=Node.ALL_SHOOTING,
contact_index=2,
)
# Path constraint
n_q = biorbd_model.nbQ()
n_qdot = n_q
n_mus = biorbd_model.nbMuscleTotal()
pose_at_first_node = [0, 0, -0.75, 0.75]
# Initialize x_bounds
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
x_bounds[0].concatenate(
Bounds([activation_min] * n_mus, [activation_max] * n_mus))
x_bounds[0][:, 0] = pose_at_first_node + [0] * n_qdot + [0.5] * n_mus
# Initial guess
x_init = InitialGuessList()
x_init.add(pose_at_first_node + [0] * n_qdot + [0.5] * n_mus)
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add(
[torque_min] * len(dof_mapping["tau"].to_first) +
[activation_min] * biorbd_model.nbMuscleTotal(),
[torque_max] * len(dof_mapping["tau"].to_first) +
[activation_max] * biorbd_model.nbMuscleTotal(),
)
u_init = InitialGuessList()
u_init.add([torque_init] * len(dof_mapping["tau"].to_first) +
[activation_init] * biorbd_model.nbMuscleTotal())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
phase_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions=objective_functions,
constraints=constraints,
variable_mappings=dof_mapping,
ode_solver=ode_solver,
)
def partial_ocp_parameters(n_phases):
if n_phases != 1 and n_phases != 3:
raise RuntimeError("n_phases should be 1 or 3")
biorbd_model_path = TestUtils.bioptim_folder(
) + "/examples/optimal_time_ocp/models/cube.bioMod"
biorbd_model = biorbd.Model(biorbd_model_path), biorbd.Model(
biorbd_model_path), biorbd.Model(biorbd_model_path)
n_shooting = (2, 2, 2)
final_time = (2, 5, 4)
time_min = [1, 3, 0.1]
time_max = [2, 4, 0.8]
tau_min, tau_max, tau_init = -100, 100, 0
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
if n_phases > 1:
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0]))
if n_phases > 1:
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0]))
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0]))
for bounds in x_bounds:
for i in [1, 3, 4, 5]:
bounds.min[i, [0, -1]] = 0
bounds.max[i, [0, -1]] = 0
x_bounds[0].min[2, 0] = 0.0
x_bounds[0].max[2, 0] = 0.0
if n_phases > 1:
x_bounds[2].min[2, [0, -1]] = [0.0, 1.57]
x_bounds[2].max[2, [0, -1]] = [0.0, 1.57]
x_init = InitialGuessList()
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
if n_phases > 1:
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
u_bounds = BoundsList()
u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(),
[tau_max] * biorbd_model[0].nbGeneralizedTorque())
if n_phases > 1:
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())
if n_phases > 1:
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
return (
biorbd_model[:n_phases],
n_shooting[:n_phases],
final_time[:n_phases],
time_min[:n_phases],
time_max[:n_phases],
tau_min,
tau_max,
tau_init,
dynamics,
x_bounds,
x_init,
u_bounds,
u_init,
)
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_CONTROL,
key="tau",
weight=100)
# Dynamics
expand = False if isinstance(ode_solver, OdeSolver.IRK) else True
dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN, expand=expand)
# Constraints
constraints = ConstraintList()
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.MID,
first_marker="m0",
second_marker="m2")
constraints.add(ConstraintFcn.TRACK_STATE, key="q", node=Node.MID, index=2)
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.END,
first_marker="m0",
second_marker="m1")
# 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(
biorbd_model_path: str = "models/double_pendulum.bioMod",
biorbd_model_path_withTranslations: str = "models/double_pendulum_with_translations.bioMod",
) -> OptimalControlProgram:
biorbd_model = (biorbd.Model(biorbd_model_path), biorbd.Model(biorbd_model_path_withTranslations))
# Problem parameters
n_shooting = (40, 40)
final_time = (1.5, 2.5)
tau_min, tau_max, tau_init = -200, 200, 0
# Mapping
tau_mappings = BiMappingList()
tau_mappings.add("tau", [None, 0], [1], phase=0)
tau_mappings.add("tau", [None, None, None, 0], [3], phase=1)
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=1, phase=0)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=1, phase=1)
objective_functions.add(
ObjectiveFcn.Mayer.MINIMIZE_COM_POSITION, node=Node.END, weight=-1000, axes=Axis.Z, phase=1, quadratic=False
)
objective_functions.add(
ObjectiveFcn.Mayer.MINIMIZE_STATE, key="q", index=2, node=Node.END, weight=-100, phase=1, quadratic=False
)
# Constraints
constraints = ConstraintList()
constraints.add(ConstraintFcn.TIME_CONSTRAINT, node=Node.END, min_bound=0.3, max_bound=3, phase=0)
constraints.add(ConstraintFcn.TIME_CONSTRAINT, node=Node.END, min_bound=0.3, max_bound=3, phase=1)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, with_contact=False)
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, with_contact=False)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0]))
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[1]))
# Phase 0
x_bounds[0][1, 0] = 0
x_bounds[0][0, 0] = 3.14
x_bounds[0].min[0, -1] = 2 * 3.14
# Phase 1
x_bounds[1][[0, 1, 4, 5], 0] = 0
x_bounds[1].min[2, -1] = 3 * 3.14
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
x_init.add([1] * (biorbd_model[1].nbQ() + biorbd_model[1].nbQdot()))
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([tau_min] * len(tau_mappings[0]["tau"].to_first), [tau_max] * len(tau_mappings[0]["tau"].to_first))
u_bounds.add([tau_min] * len(tau_mappings[1]["tau"].to_first), [tau_max] * len(tau_mappings[1]["tau"].to_first))
# Control initial guess
u_init = InitialGuessList()
u_init.add([tau_init] * len(tau_mappings[0]["tau"].to_first))
u_init.add([tau_init] * len(tau_mappings[1]["tau"].to_first))
phase_transitions = PhaseTransitionList()
phase_transitions.add(
PhaseTransitionFcn.CONTINUOUS, phase_pre_idx=0, states_mapping=BiMapping([0, 1, 2, 3], [2, 3, 6, 7])
)
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,
constraints=constraints,
variable_mappings=tau_mappings,
phase_transitions=phase_transitions,
)
def prepare_ocp(
biorbd_model_path: str,
final_time: float,
n_shooting: int,
n_threads: int,
use_sx: bool = False,
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
n_threads: int
The number of threads to use while using multithreading
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
"""
biorbd_model = biorbd.Model(biorbd_model_path)
# Add objective functions
objective_functions = Objective(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL,
key="tau",
derivative=True)
# Dynamics
expand = False if isinstance(ode_solver, OdeSolver.IRK) else True
dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN, expand=expand)
# 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,
u_init,
x_bounds,
u_bounds,
objective_functions=objective_functions,
n_threads=n_threads,
use_sx=use_sx,
ode_solver=ode_solver,
)
def test_torque_derivative_driven(with_contact, with_external_force, cx):
# Prepare the program
nlp = NonLinearProgram()
nlp.model = biorbd.Model(
TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod"
)
nlp.ns = 5
nlp.cx = cx
nlp.x_bounds = np.zeros((nlp.model.nbQ() * 3, 1))
nlp.u_bounds = np.zeros((nlp.model.nbQ(), 1))
ocp = OptimalControlProgram(nlp)
nlp.control_type = ControlType.CONSTANT
nlp.phase_idx = 0
NonLinearProgram.add(
ocp, "dynamics_type", Dynamics(DynamicsFcn.TORQUE_DERIVATIVE_DRIVEN, with_contact=with_contact), False
)
np.random.seed(42)
if with_external_force:
external_forces = [np.random.rand(6, nlp.model.nbSegment(), nlp.ns)]
nlp.external_forces = BiorbdInterface.convert_array_to_external_forces(external_forces)[0]
# Prepare the dynamics
ConfigureProblem.initialize(ocp, nlp)
# Test the results
states = np.random.rand(nlp.states.shape, nlp.ns)
controls = np.random.rand(nlp.controls.shape, nlp.ns)
params = np.random.rand(nlp.parameters.shape, nlp.ns)
x_out = np.array(nlp.dynamics_func(states, controls, params))
if with_contact:
contact_out = np.array(nlp.contact_forces_func(states, controls, params))
if with_external_force:
np.testing.assert_almost_equal(
x_out[:, 0],
[
0.8631034,
0.3251833,
0.1195942,
0.4937956,
-7.7700092,
-7.5782306,
21.7073786,
-16.3059315,
0.8074402,
0.4271078,
0.417411,
0.3232029,
],
)
np.testing.assert_almost_equal(contact_out[:, 0], [-47.8131136, 111.1726516, -24.4449121])
else:
np.testing.assert_almost_equal(
x_out[:, 0],
[
0.61185289,
0.78517596,
0.60754485,
0.80839735,
-0.32149054,
-0.19121314,
0.65071636,
-0.23597164,
0.38867729,
0.54269608,
0.77224477,
0.72900717,
],
)
np.testing.assert_almost_equal(contact_out[:, 0], [-2.444071, 128.8816865, 2.7245124])
else:
if with_external_force:
np.testing.assert_almost_equal(
x_out[:, 0],
[
0.86310343,
0.32518332,
0.11959425,
0.4937956,
0.30731739,
-9.97912778,
1.15263778,
36.02430956,
0.80744016,
0.42710779,
0.417411,
0.32320293,
],
)
else:
np.testing.assert_almost_equal(
x_out[:, 0],
[
0.61185289,
0.78517596,
0.60754485,
0.80839735,
-0.30241366,
-10.38503791,
1.60445173,
35.80238642,
0.38867729,
0.54269608,
0.77224477,
0.72900717,
],
)
def prepare_ocp(
biorbd_model_path: str,
final_time: float,
n_shooting: int,
initialize_near_solution: bool,
ode_solver: OdeSolver = OdeSolver.RK4(),
constr: bool = True,
use_sx: bool = False,
) -> OptimalControlProgram:
"""
Prepare the ocp
Parameters
----------
biorbd_model_path: str
The path to the bioMod file
final_time: float
The time at the final node
n_shooting: int
The number of shooting points
initialize_near_solution: bool
If the initial guess should be almost the solution (this is merely to reduce the time of the tests)
ode_solver: OdeSolver
The ode solver to use
constr: bool
If the constraint should be applied (this is merely to reduce the time of the tests)
use_sx: bool
If SX CasADi variables should be used
Returns
-------
The OptimalControlProgram ready to be solved
"""
biorbd_model = biorbd.Model(biorbd_model_path)
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL,
key="tau",
weight=100,
multi_thread=False)
# Dynamics
dynamics = DynamicsList()
expand = False if isinstance(ode_solver, OdeSolver.IRK) else True
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand)
# Constraints
if constr:
constraints = ConstraintList()
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.START,
first_marker="m0",
second_marker="m4")
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.END,
first_marker="m0",
second_marker="m5")
constraints.add(ConstraintFcn.TRACK_MARKER_WITH_SEGMENT_AXIS,
node=Node.ALL,
marker="m1",
segment="seg_rt",
axis=Axis.X)
else:
constraints = ConstraintList()
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
for i in range(1, 8):
if i != 3:
x_bounds[0][i, [0, -1]] = 0
x_bounds[0][2, -1] = 1.57
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()))
if initialize_near_solution:
for i in range(2):
x_init[0].init[i] = 1.5
for i in range(4, 6):
x_init[0].init[i] = 0.7
for i in range(6, 8):
x_init[0].init[i] = 0.6
# Define control path constraint
tau_min, tau_max, tau_init = -100, 100, 0
u_bounds = BoundsList()
u_bounds.add([tau_min] * biorbd_model.nbGeneralizedTorque(),
[tau_max] * biorbd_model.nbGeneralizedTorque())
u_init = InitialGuessList()
u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
use_sx=use_sx,
)
def test_torque_derivative_driven_implicit_soft_contacts(with_contact, cx, implicit_contact):
# Prepare the program
nlp = NonLinearProgram()
nlp.model = biorbd.Model(
TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod"
)
nlp.ns = 5
nlp.cx = cx
nlp.x_bounds = np.zeros((nlp.model.nbQ() * (2 + 3), 1))
nlp.u_bounds = np.zeros((nlp.model.nbQ() * 4, 1))
ocp = OptimalControlProgram(nlp)
nlp.control_type = ControlType.CONSTANT
NonLinearProgram.add(
ocp,
"dynamics_type",
Dynamics(
DynamicsFcn.TORQUE_DERIVATIVE_DRIVEN, with_contact=with_contact, implicit_soft_contacts=implicit_contact
),
False,
)
# Prepare the dynamics
ConfigureProblem.initialize(ocp, nlp)
# Test the results
np.random.seed(42)
states = np.random.rand(nlp.states.shape, nlp.ns)
controls = np.random.rand(nlp.controls.shape, nlp.ns)
params = np.random.rand(nlp.parameters.shape, nlp.ns)
x_out = np.array(nlp.dynamics_func(states, controls, params))
if with_contact:
contact_out = np.array(nlp.contact_forces_func(states, controls, params))
np.testing.assert_almost_equal(
x_out[:, 0],
[
0.6118529,
0.785176,
0.6075449,
0.8083973,
-0.3214905,
-0.1912131,
0.6507164,
-0.2359716,
0.3886773,
0.5426961,
0.7722448,
0.7290072,
],
)
np.testing.assert_almost_equal(contact_out[:, 0], [-2.444071, 128.8816865, 2.7245124])
else:
np.testing.assert_almost_equal(
x_out[:, 0],
[
0.6118529,
0.785176,
0.6075449,
0.8083973,
-0.3024137,
-10.3850379,
1.6044517,
35.8023864,
0.3886773,
0.5426961,
0.7722448,
0.7290072,
],
)
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_STATE,
key="q",
weight=10,
multi_thread=False)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE,
key="qdot",
weight=10,
multi_thread=False)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL,
key="tau",
weight=10,
multi_thread=False)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL,
key="muscles",
weight=10,
multi_thread=False)
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_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_muscle_driven(with_excitations, with_contact, with_torque, with_external_force, cx):
# Prepare the program
nlp = NonLinearProgram()
nlp.model = biorbd.Model(
TestUtils.bioptim_folder() + "/examples/muscle_driven_ocp/models/arm26_with_contact.bioMod"
)
nlp.ns = 5
nlp.cx = cx
nlp.x_bounds = np.zeros((nlp.model.nbQ() * 2 + nlp.model.nbMuscles(), 1))
nlp.u_bounds = np.zeros((nlp.model.nbMuscles(), 1))
ocp = OptimalControlProgram(nlp)
nlp.control_type = ControlType.CONSTANT
NonLinearProgram.add(
ocp,
"dynamics_type",
Dynamics(
DynamicsFcn.MUSCLE_DRIVEN,
with_torque=with_torque,
with_excitations=with_excitations,
with_contact=with_contact,
),
False,
)
np.random.seed(42)
if with_external_force:
external_forces = [np.random.rand(6, nlp.model.nbSegment(), nlp.ns)]
nlp.external_forces = BiorbdInterface.convert_array_to_external_forces(external_forces)[0]
# Prepare the dynamics
ConfigureProblem.initialize(ocp, nlp)
# Test the results
states = np.random.rand(nlp.states.shape, nlp.ns)
controls = np.random.rand(nlp.controls.shape, nlp.ns)
params = np.random.rand(nlp.parameters.shape, nlp.ns)
x_out = np.array(nlp.dynamics_func(states, controls, params))
if with_contact: # Warning this test is a bit bogus, there since the model does not have contacts
if with_torque:
if with_excitations:
if with_external_force:
np.testing.assert_almost_equal(
x_out[:, 0],
[
0.6158501,
0.50313626,
0.64241928,
1.46421499,
-45.27535002,
73.61890834,
46.87928022,
-1.80189035,
53.3914525,
48.30056919,
63.69373374,
-28.15700995,
],
)
else:
np.testing.assert_almost_equal(
x_out[:, 0],
[
1.83404510e-01,
6.11852895e-01,
7.85175961e-01,
-9.06144782e00,
2.93915658e02,
-9.24229516e02,
8.60630831e00,
3.19433638e00,
2.97405608e01,
-2.02754226e01,
-2.32467778e01,
-4.19135012e01,
],
decimal=6,
)
else:
if with_external_force:
np.testing.assert_almost_equal(
x_out[:, 0],
[6.15850098e-01, 5.03136259e-01, 6.42419278e-01, -7.67236491e00, 2.30765930e02, -7.34713354e02],
decimal=6,
)
else:
np.testing.assert_almost_equal(
x_out[:, 0],
[1.83404510e-01, 6.11852895e-01, 7.85175961e-01, -3.57374110e00, 1.13519647e02, -4.07165959e02],
decimal=6,
)
else:
if with_excitations:
if with_external_force:
np.testing.assert_almost_equal(
x_out[:, 0],
[
0.6158501,
0.50313626,
0.64241928,
1.31194581,
-50.56193318,
82.71912199,
55.65557816,
50.47052688,
0.36025589,
58.92377491,
29.70094194,
-15.13534937,
],
)
else:
np.testing.assert_almost_equal(
x_out[:, 0],
[
1.83404510e-01,
6.11852895e-01,
7.85175961e-01,
-9.49194254e00,
3.03909766e02,
-9.56600268e02,
-7.72228930e00,
-1.13759732e01,
9.51906209e01,
4.45077128e00,
-5.20261014e00,
-2.80864106e01,
],
decimal=6,
)
else:
if with_external_force:
np.testing.assert_almost_equal(
x_out[:, 0],
[0.6158501, 0.50313626, 0.64241928, 1.31194581, -50.56193318, 82.71912199],
)
else:
np.testing.assert_almost_equal(
x_out[:, 0],
[1.83404510e-01, 6.11852895e-01, 7.85175961e-01, -9.49194254e00, 3.03909766e02, -9.56600268e02],
decimal=6,
)
else:
if with_torque:
if with_excitations:
if with_external_force:
np.testing.assert_almost_equal(
x_out[:, 0],
[
0.6158501,
0.50313626,
0.64241928,
1.46421499,
-45.27535002,
73.61890834,
46.87928022,
-1.80189035,
53.3914525,
48.30056919,
63.69373374,
-28.15700995,
],
)
else:
np.testing.assert_almost_equal(
x_out[:, 0],
[
1.83404510e-01,
6.11852895e-01,
7.85175961e-01,
-9.06144782e00,
2.93915658e02,
-9.24229516e02,
8.60630831e00,
3.19433638e00,
2.97405608e01,
-2.02754226e01,
-2.32467778e01,
-4.19135012e01,
],
decimal=6,
)
else:
if with_external_force:
np.testing.assert_almost_equal(
x_out[:, 0],
[6.15850098e-01, 5.03136259e-01, 6.42419278e-01, -7.67236491e00, 2.30765930e02, -7.34713354e02],
decimal=6,
)
else:
np.testing.assert_almost_equal(
x_out[:, 0],
[1.83404510e-01, 6.11852895e-01, 7.85175961e-01, -3.57374110e00, 1.13519647e02, -4.07165959e02],
decimal=6,
)
else:
if with_excitations:
if with_external_force:
np.testing.assert_almost_equal(
x_out[:, 0],
[
0.6158501,
0.50313626,
0.64241928,
1.31194581,
-50.56193318,
82.71912199,
55.65557816,
50.47052688,
0.36025589,
58.92377491,
29.70094194,
-15.13534937,
],
)
else:
np.testing.assert_almost_equal(
x_out[:, 0],
[
1.83404510e-01,
6.11852895e-01,
7.85175961e-01,
-9.49194254e00,
3.03909766e02,
-9.56600268e02,
-7.72228930e00,
-1.13759732e01,
9.51906209e01,
4.45077128e00,
-5.20261014e00,
-2.80864106e01,
],
decimal=6,
)
else:
if with_external_force:
np.testing.assert_almost_equal(
x_out[:, 0],
[0.6158501, 0.50313626, 0.64241928, 1.31194581, -50.56193318, 82.71912199],
)
else:
np.testing.assert_almost_equal(
x_out[:, 0],
[1.83404510e-01, 6.11852895e-01, 7.85175961e-01, -9.49194254e00, 3.03909766e02, -9.56600268e02],
decimal=6,
)
def prepare_ocp(
biorbd_model_path: str,
final_time: float,
n_shooting: int,
fatigue_type: str,
split_controls: bool,
use_sx: bool = True,
) -> 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
fatigue_type: str
The type of dynamics to apply ("xia" or "michaud")
split_controls: bool
If the tau should be split into minus and plus or a if_else should be used
use_sx: bool
If the program should be built from SX (True) or MX (False)
Returns
-------
The OptimalControlProgram ready to be solved
"""
biorbd_model = biorbd.Model(biorbd_model_path)
n_tau = biorbd_model.nbGeneralizedTorque()
tau_min, tau_max, tau_init = -100, 100, 0
# Add objective functions
objective_functions = Objective(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", expand=True)
# Fatigue parameters
fatigue_dynamics = FatigueList()
for i in range(n_tau):
if fatigue_type == "xia":
fatigue_dynamics.add(
XiaTauFatigue(
XiaFatigue(LD=100, LR=100, F=5, R=10, scaling=tau_min),
XiaFatigue(LD=100, LR=100, F=5, R=10, scaling=tau_max),
state_only=False,
split_controls=split_controls,
),
)
elif fatigue_type == "xia_stabilized":
fatigue_dynamics.add(
XiaTauFatigue(
XiaFatigueStabilized(LD=100, LR=100, F=5, R=10, stabilization_factor=10, scaling=tau_min),
XiaFatigueStabilized(LD=100, LR=100, F=5, R=10, stabilization_factor=10, scaling=tau_max),
state_only=False,
split_controls=split_controls,
),
)
elif fatigue_type == "michaud":
fatigue_dynamics.add(
MichaudTauFatigue(
MichaudFatigue(
LD=100,
LR=100,
F=0.005,
R=0.005,
effort_threshold=0.2,
effort_factor=0.001,
stabilization_factor=10,
scaling=tau_min,
),
MichaudFatigue(
LD=100,
LR=100,
F=0.005,
R=0.005,
effort_threshold=0.2,
effort_factor=0.001,
stabilization_factor=10,
scaling=tau_max,
),
state_only=False,
split_controls=split_controls,
),
)
elif fatigue_type == "effort":
fatigue_dynamics.add(
TauEffortPerception(
EffortPerception(effort_threshold=0.2, effort_factor=0.001, scaling=tau_min),
EffortPerception(effort_threshold=0.2, effort_factor=0.001, scaling=tau_max),
split_controls=split_controls,
)
)
else:
raise ValueError("fatigue_type not implemented")
# Dynamics
dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN, fatigue=fatigue_dynamics, expand=True)
# Path constraint
x_bounds = QAndQDotBounds(biorbd_model)
x_bounds[:, [0, -1]] = 0
x_bounds[1, -1] = 3.14
x_bounds.concatenate(FatigueBounds(fatigue_dynamics, fix_first_frame=True))
if fatigue_type != "effort":
x_bounds[[5, 11], 0] = 0 # The rotation dof is passive (fatigue_ma = 0)
if fatigue_type == "xia":
x_bounds[[7, 13], 0] = 1 # The rotation dof is passive (fatigue_mr = 1)
# Initial guess
n_q = biorbd_model.nbQ()
n_qdot = biorbd_model.nbQdot()
x_init = InitialGuess([0] * (n_q + n_qdot))
x_init.concatenate(FatigueInitialGuess(fatigue_dynamics))
# Define control path constraint
u_bounds = FatigueBounds(fatigue_dynamics, variable_type=VariableType.CONTROLS)
if split_controls:
u_bounds[[1, 3], :] = 0 # The rotation dof is passive
else:
u_bounds[1, :] = 0 # The rotation dof is passive
u_init = FatigueInitialGuess(fatigue_dynamics, variable_type=VariableType.CONTROLS)
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,
use_sx=use_sx,
)
def prepare_ocp(
biorbd_model_path: str,
problem_type_custom: bool = True,
ode_solver: OdeSolver = OdeSolver.RK4(),
use_sx: bool = False,
) -> OptimalControlProgram:
"""
Prepare the program
Parameters
----------
biorbd_model_path: str
The path of the biorbd model
problem_type_custom: bool
If the preparation should be done using the user-defined dynamic function or the normal TORQUE_DRIVEN.
They should return the same results
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 --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
# Problem parameters
n_shooting = 30
final_time = 2
# Add objective functions
objective_functions = Objective(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL,
key="tau",
weight=100,
multi_thread=False)
# Dynamics
dynamics = DynamicsList()
expand = False if isinstance(ode_solver, OdeSolver.IRK) else True
if problem_type_custom:
dynamics.add(custom_configure,
dynamic_function=custom_dynamic,
my_additional_factor=1,
expand=expand)
else:
dynamics.add(DynamicsFcn.TORQUE_DRIVEN,
dynamic_function=custom_dynamic,
expand=expand)
# 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 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
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,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
use_sx=use_sx,
)
def prepare_ocp(
biorbd_model_path: str, final_time: float, n_shooting: int, use_sx: bool = True
) -> 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
use_sx: bool
If the ocp should be built with SX. Please note that ACADOS requires SX
Returns
-------
The OptimalControlProgram ready to be solved
"""
biorbd_model = biorbd.Model(biorbd_model_path)
nq = biorbd_model.nbQ()
nqdot = biorbd_model.nbQdot()
target = np.zeros((nq + nqdot, 1))
target[1, 0] = 3.14
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100.0, multi_thread=False)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE, key="q", weight=1.0, multi_thread=False)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE, key="qdot", weight=1.0, multi_thread=False)
objective_functions.add(
ObjectiveFcn.Mayer.MINIMIZE_STATE, weight=50000, key="q", target=target[:nq, :], multi_thread=False
)
objective_functions.add(
ObjectiveFcn.Mayer.MINIMIZE_STATE, weight=50000, key="qdot", target=target[nq:, :], multi_thread=False
)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
x_bounds[0][:, 0] = 0
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * (nq + nqdot))
# Define control path constraint
n_tau = biorbd_model.nbGeneralizedTorque()
torque_min, torque_max, torque_init = -100, 100, 0
u_bounds = BoundsList()
u_bounds.add([torque_min] * n_tau, [torque_max] * n_tau)
u_bounds[0][n_tau - 1, :] = 0
u_init = InitialGuessList()
u_init.add([torque_init] * n_tau)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
use_sx=use_sx,
)
def main():
"""
Generate random data, then create a tracking problem, and finally solve it and plot some relevant information
"""
# Define the problem
biorbd_model = biorbd.Model("models/arm26.bioMod")
final_time = 0.5
n_shooting_points = 50
use_residual_torque = True
# Generate random data to fit
t, markers_ref, x_ref, muscle_activations_ref = generate_data(
biorbd_model,
final_time,
n_shooting_points,
use_residual_torque=use_residual_torque)
# Track these data
biorbd_model = biorbd.Model(
"models/arm26.bioMod"
) # To allow for non free variable, the model must be reloaded
ocp = prepare_ocp(
biorbd_model,
final_time,
n_shooting_points,
markers_ref,
muscle_activations_ref,
x_ref[:biorbd_model.nbQ(), :],
kin_data_to_track="q",
use_residual_torque=use_residual_torque,
)
# --- Solve the program --- #
sol = ocp.solve(show_online_optim=True)
# --- Show the results --- #
q = sol.states["q"]
n_q = ocp.nlp[0].model.nbQ()
n_mark = ocp.nlp[0].model.nbMarkers()
n_frames = q.shape[1]
markers = np.ndarray((3, n_mark, q.shape[1]))
symbolic_states = MX.sym("x", n_q, 1)
markers_func = biorbd.to_casadi_func("ForwardKin", biorbd_model.markers,
symbolic_states)
for i in range(n_frames):
markers[:, :, i] = markers_func(q[:, i])
plt.figure("Markers")
n_steps_ode = ocp.nlp[0].ode_solver.steps + 1 if ocp.nlp[
0].ode_solver.is_direct_collocation else 1
for i in range(markers.shape[1]):
plt.plot(np.linspace(0, final_time, n_shooting_points + 1),
markers_ref[:, i, :].T, "k")
plt.plot(
np.linspace(0, final_time, n_shooting_points * n_steps_ode + 1),
markers[:, i, :].T, "r--")
# --- Plot --- #
plt.show()
def prepare_ocp(
biorbd_model_path: str = "models/cubeSym.bioMod",
ode_solver: OdeSolver = OdeSolver.RK4()
) -> OptimalControlProgram:
"""
Prepare the ocp
Parameters
----------
biorbd_model_path: str
Path to the bioMod
ode_solver: OdeSolver
The ode solver to use
Returns
-------
The OptimalControlProgram ready to be solved
"""
biorbd_model = biorbd.Model(biorbd_model_path)
# Problem parameters
n_shooting = 30
final_time = 2
tau_min, tau_max, tau_init = -100, 100, 0
dof_mappings = BiMappingList()
dof_mappings.add("q", [0, 1, None, 2, 2], [0, 1, 3], 4)
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL,
key="tau",
weight=100)
# Dynamics
dynamics = DynamicsList()
expand = False if isinstance(ode_solver, OdeSolver.IRK) else True
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand)
# 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 constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model, dof_mappings[0]))
x_bounds[0][3:6, [0, -1]] = 0
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * len(dof_mappings["q"].to_first) * 2)
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([tau_min] * len(dof_mappings["q"].to_first),
[tau_max] * len(dof_mappings["q"].to_first))
u_init = InitialGuessList()
u_init.add([tau_init] * len(dof_mappings["q"].to_first))
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
variable_mappings=dof_mappings,
)
