def prepare_ocp(
biorbd_model: biorbd.Model,
final_time: float,
n_shooting: int,
markers_ref: np.ndarray,
excitations_ref: np.ndarray,
q_ref: np.ndarray,
use_residual_torque: bool,
kin_data_to_track: str = "markers",
ode_solver: OdeSolver = OdeSolver.COLLOCATION(),
) -> OptimalControlProgram:
"""
Prepare the ocp to solve
Parameters
----------
biorbd_model: biorbd.Model
The loaded biorbd model
final_time: float
The time at final node
n_shooting: int
The number of shooting points
markers_ref: np.ndarray
The marker to track if 'markers' is chosen in kin_data_to_track
excitations_ref: np.ndarray
The muscle excitation (EMG) to track
q_ref: np.ndarray
The state to track if 'q' is chosen in kin_data_to_track
kin_data_to_track: str
The type of kin data to track ('markers' or 'q')
use_residual_torque: bool
If residual torque are present or not in the dynamics
ode_solver: OdeSolver
The ode solver to use
Returns
-------
The OptimalControlProgram ready to solve
"""
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.TRACK_CONTROL,
key="muscles",
target=excitations_ref)
if use_residual_torque:
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL,
key="tau")
if kin_data_to_track == "markers":
objective_functions.add(ObjectiveFcn.Lagrange.TRACK_MARKERS,
node=Node.ALL,
weight=100,
target=markers_ref)
elif kin_data_to_track == "q":
objective_functions.add(
ObjectiveFcn.Lagrange.TRACK_STATE,
key="q",
weight=100,
node=Node.ALL,
target=q_ref,
index=range(biorbd_model.nbQ()),
)
else:
raise RuntimeError("Wrong choice of kin_data_to_track")
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.MUSCLE_DRIVEN,
with_excitations=True,
with_residual_torque=use_residual_torque)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
# Due to unpredictable movement of the forward dynamics that generated the movement, the bound must be larger
x_bounds[0].min[[0, 1], :] = -2 * np.pi
x_bounds[0].max[[0, 1], :] = 2 * np.pi
# Add muscle to the bounds
activation_min, activation_max, activation_init = 0, 1, 0.5
x_bounds[0].concatenate(
Bounds([activation_min] * biorbd_model.nbMuscles(),
[activation_max] * biorbd_model.nbMuscles()))
x_bounds[0][(biorbd_model.nbQ() + biorbd_model.nbQdot()):,
0] = excitations_ref[:, 0]
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()) +
[0] * biorbd_model.nbMuscles())
# Define control path constraint
excitation_min, excitation_max, excitation_init = 0, 1, 0.5
u_bounds = BoundsList()
u_init = InitialGuessList()
if use_residual_torque:
tau_min, tau_max, tau_init = -100, 100, 0
u_bounds.add(
[tau_min] * biorbd_model.nbGeneralizedTorque() +
[excitation_min] * biorbd_model.nbMuscles(),
[tau_max] * biorbd_model.nbGeneralizedTorque() +
[excitation_max] * biorbd_model.nbMuscles(),
)
u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque() +
[excitation_init] * biorbd_model.nbMuscles())
else:
u_bounds.add([excitation_min] * biorbd_model.nbMuscles(),
[excitation_max] * biorbd_model.nbMuscles())
u_init.add([excitation_init] * biorbd_model.nbMuscles())
# ------------- #
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,
final_time: float,
n_shooting: int,
fatigue_type: str,
ode_solver: OdeSolver = OdeSolver.COLLOCATION(),
torque_level: int = 0,
) -> OptimalControlProgram:
"""
Prepare the ocp
Parameters
----------
biorbd_model_path: str
The path to the bioMod
final_time: float
The time at the final node
n_shooting: int
The number of shooting points
fatigue_type: str
The type of dynamics to apply ("xia" or "michaud")
ode_solver: OdeSolver
The ode solver to use
torque_level: int
0 no residual torque, 1 with residual torque, 2 with fatigable residual torque
Returns
-------
The OptimalControlProgram ready to be solved
"""
biorbd_model = biorbd.Model(biorbd_model_path)
n_tau = biorbd_model.nbGeneralizedTorque()
n_muscles = biorbd_model.nbMuscleTotal()
tau_min, tau_max = -10, 10
# Define fatigue parameters for each muscle and residual torque
fatigue_dynamics = FatigueList()
for i in range(n_muscles):
if fatigue_type == "xia":
fatigue_dynamics.add(XiaFatigue(LD=10, LR=10, F=0.01, R=0.002), state_only=False)
elif fatigue_type == "michaud":
fatigue_dynamics.add(
MichaudFatigue(
LD=100, LR=100, F=0.005, R=0.005, effort_threshold=0.2, effort_factor=0.001, stabilization_factor=10
),
state_only=True,
)
elif fatigue_type == "effort":
fatigue_dynamics.add(EffortPerception(effort_threshold=0.2, effort_factor=0.001))
else:
raise ValueError("fatigue_type not implemented")
if torque_level >= 2:
for i in range(n_tau):
if fatigue_type == "xia":
fatigue_dynamics.add(
XiaTauFatigue(
XiaFatigue(LD=10, LR=10, F=5, R=10, scaling=tau_min),
XiaFatigue(LD=10, LR=10, F=5, R=10, scaling=tau_max),
),
state_only=False,
)
elif fatigue_type == "michaud":
fatigue_dynamics.add(
MichaudTauFatigue(
MichaudFatigue(
LD=10, LR=10, F=5, R=10, effort_threshold=0.15, effort_factor=0.07, scaling=tau_min
),
MichaudFatigue(
LD=10, LR=10, F=5, R=10, effort_threshold=0.15, effort_factor=0.07, scaling=tau_max
),
),
state_only=False,
)
elif fatigue_type == "effort":
fatigue_dynamics.add(
TauEffortPerception(
EffortPerception(effort_threshold=0.15, effort_factor=0.001, scaling=tau_min),
EffortPerception(effort_threshold=0.15, effort_factor=0.001, scaling=tau_max),
),
state_only=False,
)
else:
raise ValueError("fatigue_type not implemented")
# Dynamics
dynamics = Dynamics(DynamicsFcn.MUSCLE_DRIVEN, expand=False, fatigue=fatigue_dynamics, with_torque=torque_level > 0)
# Add objective functions
objective_functions = ObjectiveList()
if torque_level > 0:
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau")
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="muscles", weight=100)
objective_functions.add(
ObjectiveFcn.Mayer.SUPERIMPOSE_MARKERS, first_marker="target", second_marker="COM_hand", weight=0.01
)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_FATIGUE, key="muscles", weight=1000)
# Constraint
constraint = Constraint(
ConstraintFcn.SUPERIMPOSE_MARKERS,
first_marker="target",
second_marker="COM_hand",
node=Node.END,
axes=[Axis.X, Axis.Y],
)
x_bounds = QAndQDotBounds(biorbd_model)
x_bounds[:, 0] = (0.07, 1.4, 0, 0)
x_bounds.concatenate(FatigueBounds(fatigue_dynamics, fix_first_frame=True))
x_init = InitialGuess([1.57] * biorbd_model.nbQ() + [0] * biorbd_model.nbQdot())
x_init.concatenate(FatigueInitialGuess(fatigue_dynamics))
# Define control path constraint
u_bounds = Bounds([tau_min] * n_tau, [tau_max] * n_tau) if torque_level == 1 else Bounds()
u_bounds.concatenate(FatigueBounds(fatigue_dynamics, variable_type=VariableType.CONTROLS))
u_init = InitialGuess([0] * n_tau) if torque_level == 1 else InitialGuess()
u_init.concatenate(FatigueInitialGuess(fatigue_dynamics, variable_type=VariableType.CONTROLS))
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraint,
ode_solver=ode_solver,
use_sx=False,
n_threads=8,
)
def test_michaud_fatigable_muscles():
bioptim_folder = TestUtils.bioptim_folder()
fatigue = TestUtils.load_module(
f"{bioptim_folder}/examples/fatigue/static_arm_with_fatigue.py")
model_path = f"{bioptim_folder}/examples/fatigue/models/arm26_constant.bioMod"
ocp = fatigue.prepare_ocp(
biorbd_model_path=model_path,
final_time=0.9,
n_shooting=5,
fatigue_type="michaud",
ode_solver=OdeSolver.COLLOCATION(),
torque_level=1,
)
sol = ocp.solve()
# Check objective function value
f = np.array(sol.cost)
np.testing.assert_equal(f.shape, (1, 1))
if platform.system() == "Linux":
np.testing.assert_almost_equal(f[0, 0], 16.32400654587575)
# Check constraints
g = np.array(sol.constraints)
np.testing.assert_equal(g.shape, (702, 1))
np.testing.assert_almost_equal(g, np.zeros((702, 1)))
# Check some of the results
states, controls = sol.states, sol.controls
q, qdot, ma, mr, mf = states["q"], states["qdot"], states[
"muscles_ma"], states["muscles_mr"], states["muscles_mf"]
tau, muscles = controls["tau"], controls["muscles"]
# initial and final position
np.testing.assert_almost_equal(q[:, 0], np.array((0.07, 1.4)))
np.testing.assert_almost_equal(q[:, -1], np.array(
(1.64470726, 2.25033212)))
# initial and final velocities
np.testing.assert_almost_equal(qdot[:, 0], np.array((0, 0)))
np.testing.assert_almost_equal(ma[:, 0], np.array((0, 0, 0, 0, 0, 0)))
np.testing.assert_almost_equal(mr[:, 0], np.array((1, 1, 1, 1, 1, 1)))
np.testing.assert_almost_equal(mf[:, 0], np.array((0, 0, 0, 0, 0, 0)))
np.testing.assert_almost_equal(
mf[:, -1],
np.array((0, 3.59773278e-04, 3.59740895e-04, 0, 0, 0)),
)
if platform.system() == "Linux":
np.testing.assert_almost_equal(qdot[:, -1],
np.array((-3.8913551, 3.68787122)))
np.testing.assert_almost_equal(
ma[:, -1],
np.array((0.03924828, 0.01089071, 0.00208428, 0.05019898,
0.05019898, 0.00058203)))
np.testing.assert_almost_equal(
mr[:, -1],
np.array((0.96071394, 0.98795266, 0.99699829, 0.9496845, 0.9496845,
0.99917771)))
np.testing.assert_almost_equal(tau[:, 0],
np.array((0.96697626, 0.7686893)))
np.testing.assert_almost_equal(tau[:, -2],
np.array((0.59833412, -0.73455049)))
np.testing.assert_almost_equal(
muscles[:, 0],
np.array((1.25202085e-07, 3.21982969e-01, 2.28408549e-01,
3.74330449e-07, 3.74330448e-07, 1.69987512e-01)),
)
np.testing.assert_almost_equal(
muscles[:, -2],
np.array((0.0441982, 0.00474236, 0.0009076, 0.04843388, 0.04843388,
0.00025345)),
)
# save and load
TestUtils.save_and_load(sol, ocp, True)
# simulate
TestUtils.simulate(sol)
def test_xia_fatigable_muscles():
bioptim_folder = TestUtils.bioptim_folder()
fatigue = TestUtils.load_module(
f"{bioptim_folder}/examples/fatigue/static_arm_with_fatigue.py")
model_path = f"{bioptim_folder}/examples/fatigue/models/arm26_constant.bioMod"
ocp = fatigue.prepare_ocp(
biorbd_model_path=model_path,
final_time=0.9,
n_shooting=5,
fatigue_type="xia",
ode_solver=OdeSolver.COLLOCATION(),
torque_level=1,
)
sol = ocp.solve()
# 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], 19.770521758810368)
# Check constraints
g = np.array(sol.constraints)
np.testing.assert_equal(g.shape, (552, 1))
np.testing.assert_almost_equal(g, np.zeros((552, 1)))
# Check some of the results
states, controls = sol.states, sol.controls
q, qdot, ma, mr, mf = states["q"], states["qdot"], states[
"muscles_ma"], states["muscles_mr"], states["muscles_mf"]
tau, muscles = controls["tau"], controls["muscles"]
# initial and final position
np.testing.assert_almost_equal(q[:, 0], np.array((0.07, 1.4)))
np.testing.assert_almost_equal(q[:, -1], np.array(
(1.64470726, 2.25033212)))
# initial and final velocities
np.testing.assert_almost_equal(qdot[:, 0], np.array((0, 0)))
np.testing.assert_almost_equal(qdot[:, -1],
np.array((-2.93853331, 3.00564551)))
# fatigue parameters
np.testing.assert_almost_equal(ma[:, 0], np.array((0, 0, 0, 0, 0, 0)))
np.testing.assert_almost_equal(
ma[:, -1],
np.array((0.00739128, 0.00563555, 0.00159309, 0.02418655, 0.02418655,
0.00041913)))
np.testing.assert_almost_equal(mr[:, 0], np.array((1, 1, 1, 1, 1, 1)))
np.testing.assert_almost_equal(
mr[:, -1],
np.array((0.99260018, 0.99281414, 0.99707397, 0.97566527, 0.97566527,
0.99904065)))
np.testing.assert_almost_equal(mf[:, 0], np.array((0, 0, 0, 0, 0, 0)))
np.testing.assert_almost_equal(
mf[:, -1],
np.array((8.54868154e-06, 1.55030599e-03, 1.33293886e-03,
1.48176210e-04, 1.48176210e-04, 5.40217808e-04)),
)
# initial and final controls
np.testing.assert_almost_equal(tau[:, 0], np.array(
(0.80920008, 1.66855572)))
np.testing.assert_almost_equal(tau[:, -2],
np.array((0.81847388, -0.85234628)))
np.testing.assert_almost_equal(
muscles[:, 0],
np.array((6.22395441e-08, 4.38966513e-01, 3.80781292e-01,
2.80532297e-07, 2.80532297e-07, 2.26601989e-01)),
)
np.testing.assert_almost_equal(
muscles[:, -2],
np.array((8.86069119e-03, 1.17337666e-08, 1.28715148e-08,
2.02340603e-02, 2.02340603e-02, 2.16517945e-088)),
)
# save and load
TestUtils.save_and_load(sol, ocp, True)
# simulate
TestUtils.simulate(sol)
def test_effort_fatigable_muscles():
bioptim_folder = TestUtils.bioptim_folder()
fatigue = TestUtils.load_module(
f"{bioptim_folder}/examples/fatigue/static_arm_with_fatigue.py")
model_path = f"{bioptim_folder}/examples/fatigue/models/arm26_constant.bioMod"
ocp = fatigue.prepare_ocp(
biorbd_model_path=model_path,
final_time=0.9,
n_shooting=5,
fatigue_type="effort",
ode_solver=OdeSolver.COLLOCATION(),
torque_level=1,
)
sol = ocp.solve()
# 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], 15.670921245579846)
# Check constraints
g = np.array(sol.constraints)
np.testing.assert_equal(g.shape, (252, 1))
np.testing.assert_almost_equal(g, np.zeros((252, 1)))
# Check some of the results
states, controls = sol.states, sol.controls
q, qdot, mf = states["q"], states["qdot"], states["muscles_mf"]
tau, muscles = controls["tau"], controls["muscles"]
# initial and final position
np.testing.assert_almost_equal(q[:, 0], np.array((0.07, 1.4)))
np.testing.assert_almost_equal(q[:, -1], np.array(
(1.64470726, 2.25033212)))
# 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.88775204, 3.6333437)))
# fatigue parameters
np.testing.assert_almost_equal(mf[:, 0], np.array((0, 0, 0, 0, 0, 0)))
np.testing.assert_almost_equal(
mf[:, -1],
np.array((0, 3.59773278e-04, 3.59740895e-04, 0, 0, 0)),
)
# initial and final controls
np.testing.assert_almost_equal(tau[:, 0], np.array(
(1.00151725, 0.75680955)))
np.testing.assert_almost_equal(tau[:, -2],
np.array((0.5258675, -0.65113292)))
np.testing.assert_almost_equal(
muscles[:, 0],
np.array((-3.28714919e-09, 3.22449026e-01, 2.29706846e-01,
2.48558352e-08, 2.48558352e-08, 1.68035357e-01)),
)
np.testing.assert_almost_equal(
muscles[:, -2],
np.array((3.86483779e-02, 1.10050544e-09, 2.74222976e-09,
4.25097691e-02, 4.25097691e-02, 6.56233979e-09)),
)
# save and load
TestUtils.save_and_load(sol, ocp, True)
# simulate
TestUtils.simulate(sol)
def prepare_ocp(
biorbd_model_path: str,
final_time: float,
n_shooting: int,
weight: float,
ode_solver: OdeSolver = OdeSolver.COLLOCATION(),
) -> OptimalControlProgram:
"""
Prepare the ocp
Parameters
----------
biorbd_model_path: str
The path to the bioMod
final_time: float
The time at the final node
n_shooting: int
The number of shooting points
weight: float
The weight applied to the SUPERIMPOSE_MARKERS final objective function. The bigger this number is, the greater
the model will try to reach the marker. This is in relation with the other objective functions
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.Lagrange.MINIMIZE_CONTROL, key="tau")
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL,
key="muscles")
objective_functions.add(ObjectiveFcn.Mayer.SUPERIMPOSE_MARKERS,
first_marker="target",
second_marker="COM_hand",
weight=weight)
# 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] = (0.07, 1.4, 0, 0)
# Initial guess
x_init = InitialGuessList()
x_init.add([1.57] * biorbd_model.nbQ() + [0] * biorbd_model.nbQdot())
# Define control path constraint
muscle_min, muscle_max, muscle_init = 0, 1, 0.5
tau_min, tau_max, tau_init = -1, 1, 0
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,
ode_solver=ode_solver,
)