def muscle_forces(q: MX, qdot: MX, a: MX, controls: MX, model: biorbd.Model, use_activation=True):
"""
Compute muscle force
Parameters
----------
q: MX
Symbolic value of joint angle
qdot: MX
Symbolic value of joint velocity
a: MX
Symbolic activation
controls: int
Symbolic value of activations
model: biorbd.Model
biorbd model build with the bioMod
use_activation: bool
True if activation drive False if excitation driven
Returns
-------
List of muscle forces
"""
muscle_states = biorbd.VecBiorbdMuscleState(model.nbMuscles())
for k in range(model.nbMuscles()):
if use_activation:
muscle_states[k].setActivation(controls[k])
else:
muscle_states[k].setActivation(a[k])
muscle_states[k].setExcitation(controls[k])
return model.muscleForces(muscle_states, q, qdot).to_mx()
def set_mass(biorbd_model: biorbd.Model, value: MX):
"""
The pre dynamics function is called right before defining the dynamics of the system. If one wants to
modify the dynamics (e.g. optimize the gravity in this case), then this function is the proper way to do it.
Parameters
----------
biorbd_model: biorbd.Model
The model to modify by the parameters
value: MX
The CasADi variables to modify the model
"""
biorbd_model.segment(0).characteristics().setMass(value)
def my_parameter_function(biorbd_model: biorbd.Model, value: MX, extra_value: Any):
"""
The pre dynamics function is called right before defining the dynamics of the system. If one wants to
modify the dynamics (e.g. optimize the gravity in this case), then this function is the proper way to do it.
Parameters
----------
biorbd_model: biorbd.Model
The model to modify by the parameters
value: MX
The CasADi variables to modify the model
extra_value: Any
Any parameters required by the user. The name(s) of the extra_value must match those used in parameter.add
"""
biorbd_model.setGravity(biorbd.Vector3d(0, 0, value * extra_value))
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 define_objective(
q: np.array, iteration: int, rt_ratio: int, ns_mhe: int, biorbd_model: biorbd.Model, use_noise=True
):
"""
Define the objective function for the ocp
Parameters
----------
q: np.array
State to track
iteration: int
Current iteration
rt_ratio: int
Value of the reference data ratio to send to the estimator
ns_mhe: int
Size of the windows
biorbd_model: biorbd.Model
biorbd model build with the bioMod
use_noise: bool
True if noisy reference data
Returns
---------
The objective function
"""
objectives = ObjectiveList()
if use_noise is not True:
weight = {"track_state": 1000000, "min_act": 1000, "min_dq": 10, "min_q": 10}
else:
weight = {"track_state": 1000, "min_act": 100, "min_dq": 100, "min_q": 10}
objectives.add(ObjectiveFcn.Lagrange.MINIMIZE_MUSCLES_CONTROL, weight=weight["min_act"])
objectives.add(
ObjectiveFcn.Lagrange.MINIMIZE_STATE,
weight=weight["min_dq"],
index=np.array(range(biorbd_model.nbQ(), biorbd_model.nbQ() * 2)),
)
objectives.add(
ObjectiveFcn.Lagrange.MINIMIZE_STATE,
weight=weight["min_q"],
index=np.array(range(biorbd_model.nbQ())),
)
q = q[:, ::rt_ratio]
objectives.add(
ObjectiveFcn.Lagrange.TRACK_STATE,
weight=weight["track_state"],
target=q[:, iteration : (ns_mhe + 1 + iteration)],
index=range(biorbd_model.nbQ()),
)
return objectives
def force_func(biorbd_model: biorbd.Model, use_activation=True):
"""
Define Casadi function to use muscle_forces
Parameters
----------
biorbd_model: biorbd.Model
biorbd model build with the bioMod
use_activation: bool
True if activation drive False if excitation driven
"""
q_mx = MX.sym("qMX", biorbd_model.nbQ(), 1)
dq_mx = MX.sym("dq_mx", biorbd_model.nbQ(), 1)
a_mx = MX.sym("a_mx", biorbd_model.nbMuscles(), 1)
u_mx = MX.sym("u_mx", biorbd_model.nbMuscles(), 1)
return Function(
"MuscleForce",
[q_mx, dq_mx, a_mx, u_mx],
[muscle_forces(q_mx, dq_mx, a_mx, u_mx, biorbd_model, use_activation=use_activation)],
["qMX", "dq_mx", "a_mx", "u_mx"],
["Force"],
).expand()
def set_mass(biorbd_model: biorbd.Model, value: MX):
biorbd_model.segment(0).characteristics().setMass(value)
def my_parameter_function(biorbd_model: biorbd.Model, value: MX,
extra_value: Any):
value[2] *= extra_value
biorbd_model.setGravity(value)
def prepare_ocp(
biorbd_model: biorbd.Model,
final_time: float,
n_shooting: int,
markers_ref: np.ndarray,
tau_ref: np.ndarray,
ode_solver: OdeSolver = OdeSolver.RK4(),
) -> OptimalControlProgram:
"""
Prepare the ocp
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 markers to track
tau_ref: np.ndarray
The generalized forces to track
ode_solver: OdeSolver
The ode solver to use
Returns
-------
The OptimalControlProgram ready to be solved
"""
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.TRACK_MARKERS,
axis_to_track=[Axis.Y, Axis.Z],
weight=100,
target=markers_ref)
objective_functions.add(ObjectiveFcn.Lagrange.TRACK_TORQUE, target=tau_ref)
# 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
n_q = biorbd_model.nbQ()
n_qdot = biorbd_model.nbQdot()
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_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 generate_data(biorbd_model: biorbd.Model,
final_time: float,
n_shooting: int,
use_residual_torque: bool = True) -> tuple:
"""
Generate random data. If np.random.seed is defined before, it will always return the same results
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
use_residual_torque: bool
If residual torque are present or not in the dynamics
Returns
-------
The time, marker, states and controls of the program. The ocp will try to track these
"""
# Aliases
n_q = biorbd_model.nbQ()
n_qdot = biorbd_model.nbQdot()
n_tau = biorbd_model.nbGeneralizedTorque()
n_mus = biorbd_model.nbMuscleTotal()
dt = final_time / n_shooting
if use_residual_torque:
nu = n_tau + n_mus
else:
nu = n_mus
# Casadi related stuff
symbolic_q = MX.sym("q", n_q, 1)
symbolic_qdot = MX.sym("qdot", n_qdot, 1)
symbolic_controls = MX.sym("u", nu, 1)
symbolic_parameters = MX.sym("params", 0, 0)
markers_func = biorbd.to_casadi_func("ForwardKin", biorbd_model.markers,
symbolic_q)
nlp = NonLinearProgram()
nlp.model = biorbd_model
nlp.shape = {"muscle": n_mus}
nlp.mapping = {
"q": BiMapping(range(n_q), range(n_q)),
"qdot": BiMapping(range(n_qdot), range(n_qdot)),
}
if use_residual_torque:
nlp.shape["tau"] = n_tau
nlp.mapping["tau"] = BiMapping(range(n_tau), range(n_tau))
dyn_func = DynamicsFunctions.forward_dynamics_muscle_activations_and_torque_driven
else:
dyn_func = DynamicsFunctions.forward_dynamics_muscle_activations_driven
symbolic_states = vertcat(*(symbolic_q, symbolic_qdot))
dynamics_func = biorbd.to_casadi_func("ForwardDyn", dyn_func,
symbolic_states, symbolic_controls,
symbolic_parameters, nlp)
def dyn_interface(t, x, u):
if use_residual_torque:
u = np.concatenate([np.zeros(n_tau), u])
return np.array(dynamics_func(x, u, np.empty((0, 0)))).squeeze()
# Generate some muscle activation
U = np.random.rand(n_shooting, n_mus).T
# Integrate and collect the position of the markers accordingly
X = np.ndarray((n_q + n_qdot, n_shooting + 1))
markers = np.ndarray((3, biorbd_model.nbMarkers(), n_shooting + 1))
def add_to_data(i, q):
X[:, i] = q
markers[:, :, i] = markers_func(q[0:n_q])
x_init = np.array([0] * n_q + [0] * n_qdot)
add_to_data(0, x_init)
for i, u in enumerate(U.T):
sol = solve_ivp(dyn_interface, (0, dt),
x_init,
method="RK45",
args=(u, ))
x_init = sol["y"][:, -1]
add_to_data(i + 1, x_init)
time_interp = np.linspace(0, final_time, n_shooting + 1)
return time_interp, markers, X, U
def prepare_ocp(
biorbd_model: biorbd.Model,
final_time: float,
n_shooting: int,
markers_ref: np.ndarray,
activations_ref: np.ndarray,
q_ref: np.ndarray,
kin_data_to_track: str = "markers",
use_residual_torque: bool = True,
ode_solver: OdeSolver = OdeSolver.RK4(),
) -> 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
activations_ref: np.ndarray
The muscle activation 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_MUSCLES_CONTROL,
target=activations_ref)
if use_residual_torque:
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE)
if kin_data_to_track == "markers":
objective_functions.add(ObjectiveFcn.Lagrange.TRACK_MARKERS,
weight=100,
target=markers_ref)
elif kin_data_to_track == "q":
objective_functions.add(ObjectiveFcn.Lagrange.TRACK_STATE,
weight=100,
target=q_ref,
index=range(biorbd_model.nbQ()))
else:
raise RuntimeError("Wrong choice of kin_data_to_track")
# Dynamics
dynamics = DynamicsList()
if use_residual_torque:
dynamics.add(DynamicsFcn.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN)
else:
dynamics.add(DynamicsFcn.MUSCLE_ACTIVATIONS_DRIVEN)
# 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
nq = biorbd_model.nbQ()
x_bounds[0].min[:nq, :] = -2 * np.pi
x_bounds[0].max[:nq, :] = 2 * np.pi
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()))
# Define control path constraint
activation_min, activation_max, activation_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() +
[activation_min] * biorbd_model.nbMuscleTotal(),
[tau_max] * biorbd_model.nbGeneralizedTorque() +
[activation_max] * biorbd_model.nbMuscleTotal(),
)
u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque() +
[activation_init] * biorbd_model.nbMuscleTotal())
else:
u_bounds.add([activation_min] * biorbd_model.nbMuscleTotal(),
[activation_max] * biorbd_model.nbMuscleTotal())
u_init.add([activation_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,
)
def prepare_ocp(
biorbd_model: biorbd.Model,
final_time: float,
n_shooting: int,
use_sx: bool,
weights: np.ndarray,
) -> OptimalControlProgram:
"""
Prepare the ocp
Parameters
----------
biorbd_model: str
The path to the bioMod
final_time: float
The time at the final node
n_shooting: int
The number of shooting points
use_sx: bool
If SX should be used or not
weights: 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
Returns
-------
The OptimalControlProgram ready to be solved
"""
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE,
weight=weights[0])
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE,
weight=weights[1])
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_MUSCLES_CONTROL,
weight=weights[2])
objective_functions.add(ObjectiveFcn.Mayer.SUPERIMPOSE_MARKERS,
first_marker_idx=0,
second_marker_idx=1,
weight=weights[3])
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
# Force initial position
if use_sx:
x_bounds[0][:, 0] = [1.24, 1.55, 0, 0]
else:
x_bounds[0][:, 0] = [1.0, 1.3, 0, 0]
# Initial guess
x_init = InitialGuessList()
init_state = [1.57] * biorbd_model.nbQ() + [0] * biorbd_model.nbQdot()
x_init.add(init_state)
# Define control path constraint
muscle_min, muscle_max, muscle_init = 0, 1, 0.5
tau_min, tau_max, tau_init = -10, 10, 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_bounds[0][:, 0] = [0] * biorbd_model.nbGeneralizedTorque() + [0] * 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,
n_threads=8,
use_sx=use_sx,
)