def test_bidirectional_mapping():
mapping = BiMapping([0, 1, 2], [3, 4, 5])
np.testing.assert_almost_equal(mapping.to_first.len, 3)
np.testing.assert_almost_equal(mapping.to_first.map_idx, [3, 4, 5])
np.testing.assert_almost_equal(mapping.to_second.map_idx, [0, 1, 2])
with pytest.raises(RuntimeError,
match="to_second must be a Mapping class"):
BiMapping(1, [3, 4, 5])
with pytest.raises(RuntimeError, match="to_first must be a Mapping class"):
BiMapping([0, 1, 2], 3)
def _set_dimensions_and_mapping(self):
q_mapping = BiMapping([0, 1, 2, None, 3, None, 3, 4, 5, 6, 4, 5, 6],
[0, 1, 2, 4, 7, 8, 9])
self.q_mapping = [q_mapping for _ in range(self.n_phases)]
self.qdot_mapping = [q_mapping for _ in range(self.n_phases)]
tau_mapping = BiMapping(
[None, None, None, None, 0, None, 0, 1, 2, 3, 1, 2, 3],
[4, 7, 8, 9])
self.tau_mapping = [tau_mapping for _ in range(self.n_phases)]
self.n_q = q_mapping.to_first.len
self.n_qdot = self.n_q
self.n_tau = tau_mapping.to_first.len
def __init__(self, m):
self.model = m
self.q = MX.sym("q", m.nbQ(), 1)
self.casadi_func = {}
self.mapping = {
"q": BiMapping(range(self.model.nbQ()),
range(self.model.nbQ()))
}
def prepare_ocp(biorbd_model_path,
phase_time,
n_shooting,
min_bound,
max_bound,
mu,
ode_solver=OdeSolver.RK4()):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
tau_min, tau_max, tau_init = -500, 500, 0
tau_mapping = BiMapping([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.TORQUE_DRIVEN_WITH_CONTACT)
# Constraints
constraints = ConstraintList()
constraints.add(
ConstraintFcn.CONTACT_FORCE,
min_bound=min_bound,
max_bound=max_bound,
node=Node.ALL,
contact_force_idx=1,
)
constraints.add(
ConstraintFcn.CONTACT_FORCE,
min_bound=min_bound,
max_bound=max_bound,
node=Node.ALL,
contact_force_idx=2,
)
constraints.add(
ConstraintFcn.NON_SLIPPING,
node=Node.ALL,
normal_component_idx=(1, 2),
tangential_component_idx=0,
static_friction_coefficient=mu,
)
# 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] * tau_mapping.to_first.len,
[tau_max] * tau_mapping.to_first.len)
u_init = InitialGuessList()
u_init.add([tau_init] * tau_mapping.to_first.len)
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
phase_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
tau_mapping=tau_mapping,
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,
)
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
# Casadi related stuff
symbolic_q = MX.sym("q", n_q, 1)
symbolic_qdot = MX.sym("qdot", n_qdot, 1)
symbolic_mus_states = MX.sym("mus", n_mus, 1)
symbolic_tau = MX.sym("tau", n_tau, 1)
symbolic_mus_controls = MX.sym("mus", n_mus, 1)
symbolic_states = vertcat(*(symbolic_q, symbolic_qdot,
symbolic_mus_states))
symbolic_controls = vertcat(*(symbolic_tau, symbolic_mus_controls))
symbolic_parameters = MX.sym("u", 0, 0)
nlp = NonLinearProgram()
nlp.model = biorbd_model
nlp.variable_mappings = {
"q": BiMapping(range(n_q), range(n_q)),
"qdot": BiMapping(range(n_qdot), range(n_qdot)),
"tau": BiMapping(range(n_tau), range(n_tau)),
"muscles": BiMapping(range(n_mus), range(n_mus)),
}
markers_func = biorbd.to_casadi_func("ForwardKin", biorbd_model.markers,
symbolic_q)
nlp.states.append("q", [symbolic_q, symbolic_q], symbolic_q,
nlp.variable_mappings["q"])
nlp.states.append("qdot", [symbolic_qdot, symbolic_qdot], symbolic_qdot,
nlp.variable_mappings["qdot"])
nlp.states.append("muscles", [symbolic_mus_states, symbolic_mus_states],
symbolic_mus_states, nlp.variable_mappings["muscles"])
nlp.controls.append("tau", [symbolic_tau, symbolic_tau], symbolic_tau,
nlp.variable_mappings["tau"])
nlp.controls.append(
"muscles",
[symbolic_mus_controls, symbolic_mus_controls],
symbolic_mus_controls,
nlp.variable_mappings["muscles"],
)
dynamics_func = biorbd.to_casadi_func(
"ForwardDyn",
DynamicsFunctions.muscles_driven,
symbolic_states,
symbolic_controls,
symbolic_parameters,
nlp,
False,
)
def dyn_interface(t, x, u):
u = np.concatenate([np.zeros(n_tau), u])
return np.array(dynamics_func(x, u, np.empty((0, 0)))).squeeze()
# Generate some muscle excitations
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_mus, 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[:n_q])
x_init = np.array([0] * n_q + [0] * n_qdot + [0.5] * n_mus)
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_path: str,
phase_time: float,
n_shooting: int,
use_actuators: bool = False,
ode_solver: OdeSolver = OdeSolver.RK4(),
objective_name: str = "MINIMIZE_PREDICTED_COM_HEIGHT",
com_constraints: bool = False,
) -> OptimalControlProgram:
"""
Prepare the ocp
Parameters
----------
biorbd_model_path: str
The path to the bioMod file
phase_time: float
The time at the final node
n_shooting: int
The number of shooting points
use_actuators: bool
If torque or torque activation should be used for the dynamics
ode_solver: OdeSolver
The ode solver to use
objective_name: str
The objective function to run ('MINIMIZE_PREDICTED_COM_HEIGHT',
'MINIMIZE_COM_POSITION' or 'MINIMIZE_COM_VELOCITY')
com_constraints: bool
If a constraint on the COM should be applied
Returns
-------
The OptimalControlProgram ready to be solved
"""
biorbd_model = biorbd.Model(biorbd_model_path)
if use_actuators:
tau_min, tau_max, tau_init = -1, 1, 0
else:
tau_min, tau_max, tau_init = -500, 500, 0
tau_mapping = BiMapping([None, None, None, 0], [3])
# Add objective functions
objective_functions = ObjectiveList()
if objective_name == "MINIMIZE_PREDICTED_COM_HEIGHT":
objective_functions.add(
ObjectiveFcn.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT, weight=-1)
elif objective_name == "MINIMIZE_COM_POSITION":
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_COM_POSITION,
axis=Axis.Z,
weight=-1)
elif objective_name == "MINIMIZE_COM_VELOCITY":
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_COM_VELOCITY,
axis=Axis.Z,
weight=-1)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE,
weight=1 / 100)
# Dynamics
dynamics = DynamicsList()
if use_actuators:
dynamics.add(DynamicsFcn.TORQUE_ACTIVATIONS_DRIVEN_WITH_CONTACT)
else:
dynamics.add(DynamicsFcn.TORQUE_DRIVEN_WITH_CONTACT)
# Constraints
constraints = ConstraintList()
if com_constraints:
constraints.add(
ConstraintFcn.TRACK_COM_VELOCITY,
node=Node.ALL,
min_bound=np.array([-100, -100, -100]),
max_bound=np.array([100, 100, 100]),
)
constraints.add(
ConstraintFcn.TRACK_COM_POSITION,
node=Node.ALL,
min_bound=np.array([-1, -1, -1]),
max_bound=np.array([1, 1, 1]),
)
# Path constraint
n_q = biorbd_model.nbQ()
n_qdot = n_q
pose_at_first_node = [0, 0, -0.5, 0.5]
# 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] * tau_mapping.to_first.len,
[tau_max] * tau_mapping.to_first.len)
u_init = InitialGuessList()
u_init.add([tau_init] * tau_mapping.to_first.len)
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
phase_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints=constraints,
tau_mapping=tau_mapping,
ode_solver=ode_solver,
)
def prepare_ocp(
biorbd_model_path: str = "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
all_generalized_mapping = BiMapping([0, 1, 2, -2], [0, 1, 2])
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
# Constraints
constraints = ConstraintList()
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.START,
first_marker="m0",
second_marker="m1")
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.END,
first_marker="m0",
second_marker="m2")
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model, all_generalized_mapping))
x_bounds[0][3:6, [0, -1]] = 0
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * all_generalized_mapping.to_first.len * 2)
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([tau_min] * all_generalized_mapping.to_first.len,
[tau_max] * all_generalized_mapping.to_first.len)
u_init = InitialGuessList()
u_init.add([tau_init] * all_generalized_mapping.to_first.len)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
all_generalized_mapping=all_generalized_mapping,
)
def prepare_ocp(biorbd_model_path, phase_time, n_shooting, min_bound,
max_bound):
# --- Options --- #
# 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
tau_mapping = BiMapping([None, None, None, 0], [3])
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN_WITH_CONTACT)
# Constraints
constraints = ConstraintList()
constraints.add(
ConstraintFcn.CONTACT_FORCE,
min_bound=min_bound,
max_bound=max_bound,
node=Node.ALL,
contact_force_idx=1,
)
constraints.add(
ConstraintFcn.CONTACT_FORCE,
min_bound=min_bound,
max_bound=max_bound,
node=Node.ALL,
contact_force_idx=2,
)
# 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] * tau_mapping.to_first.len +
[activation_min] * biorbd_model.nbMuscleTotal(),
[tau_max] * tau_mapping.to_first.len +
[activation_max] * biorbd_model.nbMuscleTotal(),
)
u_init = InitialGuessList()
u_init.add([tau_init] * tau_mapping.to_first.len +
[activation_init] * biorbd_model.nbMuscleTotal())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
phase_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
tau_mapping=tau_mapping,
)
def prepare_ocp(biorbd_model_path: str, final_time: float, n_shooting: int,
n_threads: int) -> OptimalControlProgram:
"""
Prepare the Euler version of the ocp
Parameters
----------
biorbd_model_path: str
The path to the bioMod
final_time: float
The initial guess for the time at the final node
n_shooting: int
The number of shooting points
Returns
-------
The OptimalControlProgram ready to be solved
"""
# --- Options --- #
np.random.seed(0)
biorbd_model = biorbd.Model(biorbd_model_path)
tau_min, tau_max, tau_init = -100, 100, 0
n_q = biorbd_model.nbQ()
n_qdot = biorbd_model.nbQdot()
n_tau = biorbd_model.nbGeneralizedTorque() - biorbd_model.nbRoot()
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE,
index=n_q + 5,
weight=-1)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=1e-6)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
# Initial guesses
vz0 = 6.0
x = np.vstack((np.zeros(
(n_q, n_shooting + 1)), np.ones((n_qdot, n_shooting + 1))))
x[2, :] = (vz0 * np.linspace(0, final_time, n_shooting + 1) +
-9.81 / 2 * np.linspace(0, final_time, n_shooting + 1)**2)
x[3, :] = np.linspace(0, 2 * np.pi, n_shooting + 1)
x[5, :] = np.linspace(0, 2 * np.pi, n_shooting + 1)
x[6, :] = np.random.random((1, n_shooting + 1)) * np.pi - np.pi
x[7, :] = np.random.random((1, n_shooting + 1)) * np.pi
x[n_q + 2, :] = vz0 - 9.81 * np.linspace(0, final_time, n_shooting + 1)
x[n_q + 3, :] = 2 * np.pi / final_time
x[n_q + 5, :] = 2 * np.pi / final_time
x_init = InitialGuessList()
x_init.add(x, interpolation=InterpolationType.EACH_FRAME)
# Path constraint
x_bounds = BoundsList()
x_min = np.zeros((n_q + n_qdot, 3))
x_max = np.zeros((n_q + n_qdot, 3))
x_min[:, 0] = [
0, 0, 0, 0, 0, 0, -2.8, 2.8, -1, -1, 7, x[n_q + 3, 0], 0, x[n_q + 5,
0], 0, 0
]
x_max[:, 0] = [
0, 0, 0, 0, 0, 0, -2.8, 2.8, 1, 1, 10, x[n_q + 3, 0], 0, x[n_q + 5, 0],
0, 0
]
x_min[:, 1] = [
-1,
-1,
-0.001,
-0.001,
-np.pi / 4,
-np.pi,
-np.pi,
0,
-100,
-100,
-100,
-100,
-100,
-100,
-100,
-100,
]
x_max[:, 1] = [
1, 1, 5, 2 * np.pi + 0.001, np.pi / 4, 50, 0, np.pi, 100, 100, 100,
100, 100, 100, 100, 100
]
x_min[:, 2] = [
-0.1,
-0.1,
-0.1,
2 * np.pi - 0.1,
-15 * np.pi / 180,
2 * np.pi,
-np.pi,
0,
-100,
-100,
-100,
-100,
-100,
-100,
-100,
-100,
]
x_max[:, 2] = [
0.1,
0.1,
0.1,
2 * np.pi + 0.1,
15 * np.pi / 180,
20 * np.pi,
0,
np.pi,
100,
100,
100,
100,
100,
100,
100,
100,
]
x_bounds.add(bounds=Bounds(x_min,
x_max,
interpolation=InterpolationType.
CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT))
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([tau_min] * n_tau, [tau_max] * n_tau)
u_mapping = BiMapping([None, None, None, None, None, None, 0, 1], [6, 7])
u_init = InitialGuessList()
u_init.add([tau_init] * n_tau)
# Set time as a variable
constraints = ConstraintList()
constraints.add(ConstraintFcn.TIME_CONSTRAINT,
node=Node.END,
min_bound=0.5,
max_bound=1.5)
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
n_threads=n_threads,
tau_mapping=u_mapping,
ode_solver=OdeSolver.RK4(),
)
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,
)
class Jumper:
model_files = (
"jumper2contacts.bioMod",
"jumper1contacts.bioMod",
"jumper1contacts.bioMod",
"jumper1contacts.bioMod",
"jumper2contacts.bioMod",
)
time_min = 0.2, 0.05, 0.6, 0.05, 0.1
time_max = 0.5, 0.5, 2.0, 0.5, 0.5
phase_time = 0.3, 0.2, 0.6, 0.2, 0.2
n_shoot = 30, 15, 20, 30, 30
q_mapping = BiMapping([0, 1, 2, 3, 3, 4, 5, 6, 4, 5, 6],
[0, 1, 2, 3, 5, 6, 7])
tau_mapping = BiMapping([None, None, None, 0, 0, 1, 2, 3, 1, 2, 3],
[3, 5, 6, 7])
initial_states = []
body_at_first_node = [0, 0, 0, 2.10, 1.15, 0.80, 0.20]
initial_velocity = [0, 0, 0, 0, 0, 0, 0]
tau_min = 20 # Tau minimal bound despite the torque activation
arm_dof = 3
heel_dof = 6
heel_marker_idx = 85
toe_marker_idx = 86
floor_z = 0.0
flat_foot_phases = 0, 4 # The indices of flat foot phases
toe_only_phases = 1, 3 # The indices of toe only phases
flatfoot_contact_x_idx = (
) # Contacts indices of heel and toe in bioMod 2 contacts
flatfoot_contact_y_idx = (
1, 4) # Contacts indices of heel and toe in bioMod 2 contacts
flatfoot_contact_z_idx = (
0, 2, 3, 5) # Contacts indices of heel and toe in bioMod 2 contacts
flatfoot_non_slipping = ((1, ), (0, 2)) # (X-Y components), Z components
toe_contact_x_idx = () # Contacts indices of toe in bioMod 1 contact
toe_contact_y_idx = (0, 2) # Contacts indices of toe in bioMod 1 contact
toe_contact_z_idx = (1, 2) # Contacts indices of toe in bioMod 1 contact
toe_non_slipping = ((0, ), 1) # (X-Y components), Z components
static_friction_coefficient = 0.5
def __init__(self, path_to_models):
self.models = [
biorbd.Model(path_to_models + "/" + elt)
for elt in self.model_files
]
def find_initial_root_pose(self):
# This method finds a root pose such that the body of a given pose has its CoM centered to the feet
model = self.models[0]
n_root = model.nbRoot()
body_pose_no_root = self.q_mapping.to_second.map(
self.body_at_first_node)[n_root:, 0]
bimap = BiMapping(
list(range(n_root)) + [None] * body_pose_no_root.shape[0],
list(range(n_root)))
bound_min = []
bound_max = []
for i in range(model.nbSegment()):
seg = model.segment(i)
for r in seg.QRanges():
bound_min.append(r.min())
bound_max.append(r.max())
bound_min = bimap.to_first.map(np.array(bound_min)[:, np.newaxis])
bound_max = bimap.to_first.map(np.array(bound_max)[:, np.newaxis])
root_bounds = (list(bound_min[:, 0]), list(bound_max[:, 0]))
q_sym = MX.sym("Q", model.nbQ(), 1)
com_func = biorbd.to_casadi_func("com", model.CoM, q_sym)
contacts_func = biorbd.to_casadi_func("contacts",
model.constraintsInGlobal, q_sym,
True)
shoulder_jcs_func = biorbd.to_casadi_func("shoulder_jcs",
model.globalJCS, q_sym, 3)
hand_marker_func = biorbd.to_casadi_func("hand_marker", model.marker,
q_sym, 32)
def objective_function(q_root, *args, **kwargs):
# Center of mass
q = bimap.to_second.map(q_root[:, np.newaxis])[:, 0]
q[model.nbRoot():] = body_pose_no_root
com = np.array(com_func(q))
contacts = np.array(contacts_func(q))
mean_contacts = np.mean(contacts, axis=1)
shoulder_jcs = np.array(shoulder_jcs_func(q))
hand = np.array(hand_marker_func(q))
# Prepare output
out = np.ndarray((0, ))
# The center of contact points should be at 0
out = np.concatenate((out, mean_contacts[0:2]))
out = np.concatenate((out, contacts[2,
self.flatfoot_contact_z_idx]))
# The projection of the center of mass should be at 0 and at 0.95 meter high
out = np.concatenate((out, (com + np.array([[0, 0, -0.95]]).T)[:,
0]))
# Keep the arms horizontal
out = np.concatenate((out, (shoulder_jcs[2, 3] - hand[2])))
return out
q_root0 = np.mean(root_bounds, axis=0)
pos = optimize.least_squares(objective_function,
x0=q_root0,
bounds=root_bounds)
root = np.zeros(n_root)
root[bimap.to_first.map_idx] = pos.x
return root
def show(self, q):
import bioviz
b = bioviz.Viz(self.models[0].path().absolutePath().to_string())
b.set_q(q if len(q.shape) == 1 else q[:, 0])
b.exec()
def find_initial_root_pose(self):
# This method finds a root pose such that the body of a given pose has its CoM centered to the feet
model = self.models[0]
n_root = model.nbRoot()
body_pose_no_root = self.q_mapping.to_second.map(
self.body_at_first_node)[n_root:, 0]
bimap = BiMapping(
list(range(n_root)) + [None] * body_pose_no_root.shape[0],
list(range(n_root)))
bound_min = []
bound_max = []
for i in range(model.nbSegment()):
seg = model.segment(i)
for r in seg.QRanges():
bound_min.append(r.min())
bound_max.append(r.max())
bound_min = bimap.to_first.map(np.array(bound_min)[:, np.newaxis])
bound_max = bimap.to_first.map(np.array(bound_max)[:, np.newaxis])
root_bounds = (list(bound_min[:, 0]), list(bound_max[:, 0]))
q_sym = MX.sym("Q", model.nbQ(), 1)
com_func = biorbd.to_casadi_func("com", model.CoM, q_sym)
contacts_func = biorbd.to_casadi_func("contacts",
model.constraintsInGlobal, q_sym,
True)
shoulder_jcs_func = biorbd.to_casadi_func("shoulder_jcs",
model.globalJCS, q_sym, 3)
hand_marker_func = biorbd.to_casadi_func("hand_marker", model.marker,
q_sym, 32)
def objective_function(q_root, *args, **kwargs):
# Center of mass
q = bimap.to_second.map(q_root[:, np.newaxis])[:, 0]
q[model.nbRoot():] = body_pose_no_root
com = np.array(com_func(q))
contacts = np.array(contacts_func(q))
mean_contacts = np.mean(contacts, axis=1)
shoulder_jcs = np.array(shoulder_jcs_func(q))
hand = np.array(hand_marker_func(q))
# Prepare output
out = np.ndarray((0, ))
# The center of contact points should be at 0
out = np.concatenate((out, mean_contacts[0:2]))
out = np.concatenate((out, contacts[2,
self.flatfoot_contact_z_idx]))
# The projection of the center of mass should be at 0 and at 0.95 meter high
out = np.concatenate((out, (com + np.array([[0, 0, -0.95]]).T)[:,
0]))
# Keep the arms horizontal
out = np.concatenate((out, (shoulder_jcs[2, 3] - hand[2])))
return out
q_root0 = np.mean(root_bounds, axis=0)
pos = optimize.least_squares(objective_function,
x0=q_root0,
bounds=root_bounds)
root = np.zeros(n_root)
root[bimap.to_first.map_idx] = pos.x
return root
def prepare_ocp_quaternion(biorbd_model_path, final_time, n_shooting):
"""
Prepare the quaternion version of the ocp
Parameters
----------
biorbd_model_path: str
The path to the bioMod
final_time: float
The initial guess for the time at the final node
n_shooting: int
The number of shooting points
Returns
-------
The OptimalControlProgram ready to be solved
"""
# --- Options --- #
np.random.seed(0)
biorbd_model = biorbd.Model(biorbd_model_path)
tau_min, tau_max, tau_init = -100, 100, 0
n_q = biorbd_model.nbQ()
n_qdot = biorbd_model.nbQdot()
n_tau = biorbd_model.nbGeneralizedTorque() - 6
# Add objective functions
objective_functions = ObjectiveList()
states_mx = cas.MX.sym("states_mx", n_q + n_qdot)
states_to_euler_rate_func = states_to_euler_rate(states_mx)
states_to_euler_func = states_to_euler(states_mx)
objective_functions.add(
max_twist_quaternion,
states_to_euler_rate_func=states_to_euler_rate_func,
custom_type=ObjectiveFcn.Lagrange,
weight=-1,
)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=1e-6)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
# Initial guesses
vz0 = 6.0
x = np.zeros((n_q + n_qdot, n_shooting + 1))
x[2, :] = (vz0 * np.linspace(0, final_time, n_shooting + 1) +
-9.81 / 2 * np.linspace(0, final_time, n_shooting + 1)**2)
euler_mx = cas.MX.sym("euler_mx", 3)
quaternion_mx = cas.MX.sym("quaternion_mx", 4)
euler_rate_mx = cas.MX.sym("euler_rate_mx", 3)
euler_to_quaternion_func = euler_to_quaternion(euler_mx)
euler_rate_to_body_velocities_func = euler_rate_to_body_velocities(
quaternion_mx, euler_rate_mx, euler_mx)
root_euler = np.zeros((3, n_shooting + 1))
root_euler_rate = np.zeros((3, n_shooting + 1))
root_euler[0, :] = np.linspace(0.01, 2 * np.pi, n_shooting + 1)
root_euler[2, :] = np.linspace(0.01, 2 * np.pi, n_shooting + 1)
root_euler_rate[0, :] = 2 * np.pi / final_time
root_euler_rate[2, :] = 2 * np.pi / final_time
for i in range(n_shooting + 1):
root_quaternion = euler_to_quaternion_func(root_euler[:, i])
x[3:6, i] = np.reshape(root_quaternion[1:], 3)
x[8, i] = np.reshape(root_quaternion[0], 1)
root_omega = euler_rate_to_body_velocities_func(
root_quaternion, root_euler_rate[:, i], root_euler[:, i])
x[12:15, i] = np.reshape(root_omega, 3)
x[6, :] = np.random.random((1, n_shooting + 1)) * np.pi - np.pi
x[7, :] = np.random.random((1, n_shooting + 1)) * np.pi
x[n_q + 2, :] = vz0 - 9.81 * np.linspace(0, final_time, n_shooting + 1)
x_init = InitialGuessList()
x_init.add(x, interpolation=InterpolationType.EACH_FRAME)
# Path constraint
x_bounds = BoundsList()
x_min = np.zeros((n_q + n_qdot, 3))
x_max = np.zeros((n_q + n_qdot, 3))
x_min[:, 0] = [
0, 0, 0, x[3, 0], x[4, 0], x[5, 0], -2.8, 2.8, x[8, 0], -1, -1, 4,
x[12, 0], x[13, 0], x[14, 0], 0, 0
]
x_max[:, 0] = [
0, 0, 0, x[3, 0], x[4, 0], x[5, 0], -2.8, 2.8, x[8, 0], 1, 1, 10,
x[12, 0], x[13, 0], x[14, 0], 0, 0
]
x_min[:, 1] = [
-1,
-1,
-0.001,
-1.05,
-1.05,
-1.05,
-np.pi,
0,
-1.05,
-100,
-100,
-100,
-100,
-100,
-100,
-100,
-100,
]
x_max[:, 1] = [
1, 1, 5, 1.05, 1.05, 1.05, 0, np.pi, 1.05, 100, 100, 100, 100, 100,
100, 100, 100
]
x_min[:, 2] = [
-0.1,
-0.1,
-0.1,
x[3, 0],
-1.05,
-1.05,
-np.pi,
0,
-1.05,
-100,
-100,
-100,
-100,
-100,
-100,
-100,
-100,
]
x_max[:, 2] = [
0.1, 0.1, 0.1, x[3, 0], 1.05, 1.05, 0, np.pi, 1.05, 100, 100, 100, 100,
100, 100, 100, 100
]
x_bounds.add(bounds=Bounds(x_min,
x_max,
interpolation=InterpolationType.
CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT))
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([tau_min] * n_tau, [tau_max] * n_tau)
u_mapping = BiMapping([None, None, None, None, None, None, 0, 1], [6, 7])
u_init = InitialGuessList()
u_init.add([tau_init] * n_tau)
# Set time as a variable
constraints = ConstraintList()
constraints.add(ConstraintFcn.TIME_CONSTRAINT,
node=Node.END,
min_bound=0.5,
max_bound=1.5)
constraints.add(
final_position_quaternion,
states_to_euler_func=states_to_euler_func,
node=Node.END,
min_bound=-15 * np.pi / 180,
max_bound=15 * np.pi / 180,
)
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
n_threads=8,
tau_mapping=u_mapping,
ode_solver=OdeSolver.RK4(),
)