def prepare_ocp(model_path, phase_time, number_shooting_points):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(model_path)
torque_min, torque_max, torque_init = -500, 500, 0
tau_mapping = BidirectionalMapping(Mapping([-1, -1, -1, 0]), Mapping([3]))
# Add objective functions
objective_functions = (
{
"type": Objective.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT,
"weight": -1
},
{
"type": Objective.Lagrange.MINIMIZE_TORQUE,
"weight": 1 / 100
},
)
# Dynamics
problem_type = ProblemType.torque_driven_with_contact
# Constraints
constraints = ()
# Path constraint
nb_q = biorbd_model.nbQ()
nb_qdot = nb_q
pose_at_first_node = [0, 0, -0.5, 0.5]
# Initialize X_bounds
X_bounds = QAndQDotBounds(biorbd_model)
X_bounds.min[:, 0] = pose_at_first_node + [0] * nb_qdot
X_bounds.max[:, 0] = pose_at_first_node + [0] * nb_qdot
# Initial guess
X_init = InitialConditions(pose_at_first_node + [0] * nb_qdot)
# Define control path constraint
U_bounds = Bounds(min_bound=[torque_min] * tau_mapping.reduce.len,
max_bound=[torque_max] * tau_mapping.reduce.len)
U_init = InitialConditions([torque_init] * tau_mapping.reduce.len)
# ------------- #
return OptimalControlProgram(
biorbd_model,
problem_type,
number_shooting_points,
phase_time,
X_init,
U_init,
X_bounds,
U_bounds,
objective_functions,
constraints,
tau_mapping=tau_mapping,
)
def prepare_ocp(biorbd_model_path="cubeSym.bioMod", show_online_optim=False, ode_solver=OdeSolver.RK):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
# Problem parameters
number_shooting_points = 30
final_time = 2
torque_min, torque_max, torque_init = -100, 100, 0
all_generalized_mapping = BidirectionalMapping(Mapping([0, 1, 2, 2], [3]), Mapping([0, 1, 2]))
# Add objective functions
objective_functions = {"type": Objective.Lagrange.MINIMIZE_TORQUE, "weight": 100}
# Dynamics
variable_type = ProblemType.torque_driven
# Constraints
constraints = (
{"type": Constraint.ALIGN_MARKERS, "instant": Instant.START, "first_marker": 0, "second_marker": 1,},
{"type": Constraint.ALIGN_MARKERS, "instant": Instant.END, "first_marker": 0, "second_marker": 2,},
)
# Path constraint
X_bounds = QAndQDotBounds(biorbd_model, all_generalized_mapping)
for i in range(3, 6):
X_bounds.first_node_min[i] = 0
X_bounds.last_node_min[i] = 0
X_bounds.first_node_max[i] = 0
X_bounds.last_node_max[i] = 0
# Initial guess
X_init = InitialConditions([0] * all_generalized_mapping.reduce.len * 2)
# Define control path constraint
U_bounds = Bounds(
[torque_min] * all_generalized_mapping.reduce.len, [torque_max] * all_generalized_mapping.reduce.len,
)
U_init = InitialConditions([torque_init] * all_generalized_mapping.reduce.len)
# ------------- #
return OptimalControlProgram(
biorbd_model,
variable_type,
number_shooting_points,
final_time,
objective_functions,
X_init,
U_init,
X_bounds,
U_bounds,
constraints,
ode_solver=ode_solver,
all_generalized_mapping=all_generalized_mapping,
show_online_optim=show_online_optim,
)
def plot_CoM(x, model_path):
m = biorbd.Model(model_path)
q_mapping = BidirectionalMapping(
Mapping([0, 1, 2, -1, 3, -1, 3, 4, 5, 6, 4, 5, 6], [5]), Mapping([0, 1, 2, 4, 7, 8, 9])
)
q_reduced = x[:7, :]
q_expanded = q_mapping.expand.map(q_reduced)
from casadi import Function, MX
import numpy as np
q_sym = MX.sym("q", m.nbQ(), 1)
CoM_func = Function("Compute_CoM", [q_sym], [m.CoM(q_sym).to_mx()], ["q"], ["CoM"],).expand()
CoM = np.array(CoM_func(q_expanded[:, :]))
return CoM[2]
def CoM_dot_Z_last_node_positivity(ocp, nlp, t, x, u, p):
from casadi import Function, MX
q_reduced = x[0][:nlp["nbQ"]]
qdot_reduced = x[0][nlp["nbQ"]:]
q_mapping = BidirectionalMapping(
Mapping([0, 1, 2, -1, 3, -1, 3, 4, 5, 6, 4, 5, 6], [5]), Mapping([0, 1, 2, 4, 7, 8, 9])
)
q_expanded = q_mapping.expand.map(q_reduced)
qdot_expanded = q_mapping.expand.map(qdot_reduced)
q_sym = MX.sym("q", q_expanded.size()[0], 1)
qdot_sym = MX.sym("q_dot", qdot_expanded.size()[0], 1)
CoM_dot_func = Function(
"Compute_CoM_dot", [q_sym, qdot_sym], [nlp["model"].CoMdot(q_sym, qdot_sym).to_mx()], ["q", "q_dot"], ["CoM_dot"],
).expand()
CoM_dot = CoM_dot_func(q_expanded, qdot_expanded)
return CoM_dot[2]
def plot_CoM_dot(x, model_path):
m = biorbd.Model(model_path)
q_mapping = BidirectionalMapping(
Mapping([0, 1, 2, -1, 3, -1, 3, 4, 5, 6, 4, 5, 6], [5]),
Mapping([0, 1, 2, 4, 7, 8, 9]))
q_reduced = x[:7, :]
qdot_reduced = x[7:, :]
q_expanded = q_mapping.expand.map(q_reduced)
qdot_expanded = q_mapping.expand.map(qdot_reduced)
q_sym = MX.sym("q", m.nbQ(), 1)
qdot_sym = MX.sym("q_dot", m.nbQdot(), 1)
CoM_dot_func = Function(
"Compute_CoM_dot",
[q_sym, qdot_sym],
[m.CoMdot(q_sym, qdot_sym).to_mx()],
["q", "q_dot"],
["CoM_dot"],
).expand()
CoM_dot = np.array(CoM_dot_func(q_expanded[:, :], qdot_expanded[:, :]))
return CoM_dot[2]
def prepare_ocp(model_path, phase_time, number_shooting_points, direction,
boundary):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(model_path)
tau_min, tau_max, tau_ini = -500, 500, 0
tau_mapping = BidirectionalMapping(Mapping([-1, -1, -1, 0]), Mapping([3]))
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT,
weight=-1)
# Dynamics
dynamics = DynamicsTypeList()
dynamics.add(DynamicsType.TORQUE_DRIVEN_WITH_CONTACT)
# Constraints
constraints = ConstraintList()
constraints.add(
Constraint.CONTACT_FORCE_INEQUALITY,
direction=direction,
instant=Instant.ALL,
contact_force_idx=1,
boundary=boundary,
)
constraints.add(
Constraint.CONTACT_FORCE_INEQUALITY,
direction=direction,
instant=Instant.ALL,
contact_force_idx=2,
boundary=boundary,
)
# Path constraint
nb_q = biorbd_model.nbQ()
nb_qdot = nb_q
pose_at_first_node = [0, 0, -0.75, 0.75]
# Initialize X_bounds
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
x_bounds[0].min[:, 0] = pose_at_first_node + [0] * nb_qdot
x_bounds[0].max[:, 0] = pose_at_first_node + [0] * nb_qdot
# Initial guess
x_init = InitialConditionsList()
x_init.add(pose_at_first_node + [0] * nb_qdot)
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([[tau_min] * tau_mapping.reduce.len,
[tau_max] * tau_mapping.reduce.len])
u_init = InitialConditionsList()
u_init.add([tau_ini] * tau_mapping.reduce.len)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
phase_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
tau_mapping=tau_mapping,
)
def generate_data(biorbd_model, final_time, nb_shooting):
# Aliases
nb_q = biorbd_model.nbQ()
nb_qdot = biorbd_model.nbQdot()
nb_tau = biorbd_model.nbGeneralizedTorque()
nb_mus = biorbd_model.nbMuscleTotal()
nb_markers = biorbd_model.nbMarkers()
dt = final_time / nb_shooting
# Casadi related stuff
symbolic_states = MX.sym("x", nb_q + nb_qdot + nb_mus, 1)
symbolic_controls = MX.sym("u", nb_tau + nb_mus, 1)
nlp = {
"model":
biorbd_model,
"nbTau":
nb_tau,
"nbQ":
nb_q,
"nbQdot":
nb_qdot,
"nbMuscle":
nb_mus,
"q_mapping":
BidirectionalMapping(Mapping(range(nb_q)), Mapping(range(nb_q))),
"q_dot_mapping":
BidirectionalMapping(Mapping(range(nb_qdot)), Mapping(range(nb_qdot))),
"tau_mapping":
BidirectionalMapping(Mapping(range(nb_tau)), Mapping(range(nb_tau))),
}
markers_func = []
for i in range(nb_markers):
markers_func.append(
Function(
"ForwardKin",
[symbolic_states],
[biorbd_model.marker(symbolic_states[:nb_q], i).to_mx()],
["q"],
["marker_" + str(i)],
).expand())
dynamics_func = Function(
"ForwardDyn",
[symbolic_states, symbolic_controls],
[
Dynamics.forward_dynamics_muscle_excitations_and_torque_driven(
symbolic_states, symbolic_controls, nlp)
],
["x", "u"],
["xdot"],
).expand()
def dyn_interface(t, x, u):
u = np.concatenate([np.zeros(2), u])
return np.array(dynamics_func(x, u)).squeeze()
# Generate some muscle excitations
U = np.random.rand(nb_shooting, nb_mus)
# Integrate and collect the position of the markers accordingly
X = np.ndarray(
(biorbd_model.nbQ() + biorbd_model.nbQdot() + nb_mus, nb_shooting + 1))
markers = np.ndarray((3, biorbd_model.nbMarkers(), nb_shooting + 1))
def add_to_data(i, q):
X[:, i] = q
for j, mark_func in enumerate(markers_func):
markers[:, j, i] = np.array(mark_func(q)).squeeze()
x_init = np.array([0] * nb_q + [0] * nb_qdot + [0.5] * nb_mus)
add_to_data(0, x_init)
for i, u in enumerate(U):
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, nb_shooting + 1)
return time_interp, markers, X, U
def prepare_ocp(model_path,
phase_time,
number_shooting_points,
use_actuators=False):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(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 = BidirectionalMapping(Mapping([-1, -1, -1, 0]), Mapping([3]))
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT,
weight=-1)
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=1 / 100)
# Dynamics
dynamics = DynamicsTypeList()
if use_actuators:
dynamics.add(DynamicsType.TORQUE_ACTIVATIONS_DRIVEN_WITH_CONTACT)
else:
dynamics.add(DynamicsType.TORQUE_DRIVEN_WITH_CONTACT)
# Path constraint
nb_q = biorbd_model.nbQ()
nb_qdot = nb_q
pose_at_first_node = [0, 0, -0.5, 0.5]
# Initialize X_bounds
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
x_bounds[0].min[:, 0] = pose_at_first_node + [0] * nb_qdot
x_bounds[0].max[:, 0] = pose_at_first_node + [0] * nb_qdot
# Initial guess
x_init = InitialConditionsList()
x_init.add(pose_at_first_node + [0] * nb_qdot)
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([[tau_min] * tau_mapping.reduce.len,
[tau_max] * tau_mapping.reduce.len])
u_init = InitialConditionsList()
u_init.add([tau_init] * tau_mapping.reduce.len)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
phase_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
tau_mapping=tau_mapping,
)
def prepare_ocp(model_path,
phase_time,
number_shooting_points,
use_symmetry=True,
use_actuators=True):
# --- Options --- #
# Model path
biorbd_model = [biorbd.Model(elt) for elt in model_path]
nb_phases = len(biorbd_model)
if use_actuators:
tau_min, tau_max, tau_init = -1, 1, 0
else:
tau_min, tau_max, tau_init = -500, 500, 0
if use_symmetry:
q_mapping = BidirectionalMapping(
Mapping([0, 1, 2, -1, 3, -1, 3, 4, 5, 6, 4, 5, 6], [5]),
Mapping([0, 1, 2, 4, 7, 8, 9]))
q_mapping = q_mapping, q_mapping
tau_mapping = BidirectionalMapping(
Mapping([-1, -1, -1, -1, 0, -1, 0, 1, 2, 3, 1, 2, 3], [5]),
Mapping([4, 7, 8, 9]))
tau_mapping = tau_mapping, tau_mapping
else:
q_mapping = BidirectionalMapping(
Mapping([i for i in range(biorbd_model[0].nbQ())]),
Mapping([i for i in range(biorbd_model[0].nbQ())]))
q_mapping = q_mapping, q_mapping
tau_mapping = q_mapping
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT,
weight=-1,
phase=1)
# Dynamics
dynamics = DynamicsTypeList()
if use_actuators:
dynamics.add(DynamicsType.TORQUE_ACTIVATIONS_DRIVEN_WITH_CONTACT)
dynamics.add(DynamicsType.TORQUE_ACTIVATIONS_DRIVEN_WITH_CONTACT)
else:
dynamics.add(DynamicsType.TORQUE_DRIVEN_WITH_CONTACT)
dynamics.add(DynamicsType.TORQUE_DRIVEN_WITH_CONTACT)
# --- Constraints --- #
constraints = ConstraintList()
# Positivity constraints of the normal component of the reaction forces
contact_axes = (1, 2, 4, 5)
for i in contact_axes:
constraints.add(Constraint.CONTACT_FORCE_INEQUALITY,
phase=0,
direction="GREATER_THAN",
instant=Instant.ALL,
contact_force_idx=i,
boundary=0)
contact_axes = (1, 3)
for i in contact_axes:
constraints.add(Constraint.CONTACT_FORCE_INEQUALITY,
phase=1,
direction="GREATER_THAN",
instant=Instant.ALL,
contact_force_idx=i,
boundary=0)
# Non-slipping constraints
# N.B.: Application on only one of the two feet is sufficient, as the slippage cannot occurs on only one foot.
constraints.add(Constraint.NON_SLIPPING,
phase=0,
instant=Instant.ALL,
normal_component_idx=(1, 2),
tangential_component_idx=0,
static_friction_coefficient=0.5)
constraints.add(Constraint.NON_SLIPPING,
phase=1,
instant=Instant.ALL,
normal_component_idx=1,
tangential_component_idx=0,
static_friction_coefficient=0.5)
# Custom constraints for contact forces at transitions
# constraints_second_phase.append(
# {"type": Constraint.CUSTOM, "function": from_2contacts_to_1, "instant": Instant.START}
# )
# Custom constraints for positivity of CoM_dot on z axis just before the take-off
constraints.add(CoM_dot_Z_last_node_positivity,
phase=1,
instant=Instant.END,
min_bound=0,
max_bound=np.inf)
# TODO: Make it works also with no symmetry (adapt to refactor)
# if not use_symmetry:
# first_dof = (3, 4, 7, 8, 9)
# second_dof = (5, 6, 10, 11, 12)
# coef = (-1, 1, 1, 1, 1)
# for i in range(len(first_dof)):
# for elt in [
# constraints_first_phase,
# constraints_second_phase,
# ]:
# elt.append(
# {
# "type": Constraint.PROPORTIONAL_STATE,
# "instant": Instant.ALL,
# "first_dof": first_dof[i],
# "second_dof": second_dof[i],
# "coef": coef[i],
# }
# )
# constraints = (
# constraints_first_phase,
# constraints_second_phase,
# )
# # Time constraint
# for i in range(nb_phases):
# constraints.add(Constraint.TIME_CONSTRAINT, phase=i, minimum=time_min[i], maximum=time_max[i])
# --- Path constraints --- #
if use_symmetry:
nb_q = q_mapping[0].reduce.len
nb_qdot = nb_q
pose_at_first_node = [0, 0, -0.5336, 1.4, 0.8, -0.9, 0.47]
else:
nb_q = q_mapping[0].reduce.len
nb_qdot = nb_q
pose_at_first_node = [
0, 0, -0.5336, 0, 1.4, 0, 1.4, 0.8, -0.9, 0.47, 0.8, -0.9, 0.47
]
# Initialize x_bounds (Interpolation type is CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT)
x_bounds = BoundsList()
for i in range(nb_phases):
x_bounds.add(
QAndQDotBounds(biorbd_model[i],
all_generalized_mapping=q_mapping[i]))
x_bounds[0].min[:, 0] = pose_at_first_node + [0] * nb_qdot
x_bounds[0].max[:, 0] = pose_at_first_node + [0] * nb_qdot
# # Initial guess for states (Interpolation type is CONSTANT)
# x_init = InitialConditionsList()
# for i in range(nb_phases):
# x_init.add(pose_at_first_node + [0] * nb_qdot)
# Initial guess for states (Interpolation type is CONSTANT for 1st phase and SPLINE with 3 key positions for 2nd phase)
x_init = InitialConditionsList()
x_init.add(pose_at_first_node +
[0] * nb_qdot) # x_init phase 0 type CONSTANT
t_spline = np.hstack((0, 0.34, phase_time[1]))
p0 = np.array([pose_at_first_node + [0] * nb_qdot]).T
p_flex = np.array(
[[-0.12, -0.23, -1.10, 1.85, 2.06, -1.67, 0.55, 0, 0, 0, 0, 0, 0,
0]]).T
p_end = p0
key_positions = np.hstack((p0, p_flex, p_end))
x_init.add(
key_positions, t=t_spline,
interpolation=InterpolationType.SPLINE) # x_init phase 1 type SPLINE
# Define control path constraint
u_bounds = BoundsList()
for tau_m in tau_mapping:
u_bounds.add(
[[tau_min] * tau_m.reduce.len, [tau_max] * tau_m.reduce.len],
interpolation=InterpolationType.CONSTANT
) # This precision of the CONSTANT type is for informative purposes only
u_init = InitialConditionsList()
for tau_m in tau_mapping:
u_init.add(
[tau_init] *
tau_m.reduce.len) # Interpolation type is CONSTANT (default value)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
phase_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
q_mapping=q_mapping,
q_dot_mapping=q_mapping,
tau_mapping=tau_mapping,
nb_threads=2,
)
def prepare_ocp(model_path, phase_time, number_shooting_points, direction,
boundary):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(model_path)
torque_min, torque_max, torque_init = -500, 500, 0
activation_min, activation_max, activation_init = 0, 1, 0.5
tau_mapping = BidirectionalMapping(Mapping([-1, -1, -1, 0]), Mapping([3]))
# Add objective functions
objective_functions = ({
"type": Objective.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT,
"weight": -1
}, )
# Dynamics
problem_type = ProblemType.muscle_excitations_and_torque_driven_with_contact
# Constraints
constraints = (
{
"type": Constraint.CONTACT_FORCE_INEQUALITY,
"direction": direction,
"instant": Instant.ALL,
"contact_force_idx": 1,
"boundary": boundary,
},
{
"type": Constraint.CONTACT_FORCE_INEQUALITY,
"direction": direction,
"instant": Instant.ALL,
"contact_force_idx": 2,
"boundary": boundary,
},
)
# Path constraint
nb_q = biorbd_model.nbQ()
nb_qdot = nb_q
nb_mus = biorbd_model.nbMuscleTotal()
pose_at_first_node = [0, 0, -0.75, 0.75]
# Initialize X_bounds
X_bounds = QAndQDotBounds(biorbd_model)
X_bounds.concatenate(
Bounds([activation_min] * nb_mus, [activation_max] * nb_mus))
X_bounds.min[:, 0] = pose_at_first_node + [0] * nb_qdot + [0.5] * nb_mus
X_bounds.max[:, 0] = pose_at_first_node + [0] * nb_qdot + [0.5] * nb_mus
# Initial guess
X_init = [
InitialConditions(pose_at_first_node + [0] * nb_qdot + [0.5] * nb_mus)
]
# Define control path constraint
U_bounds = [
Bounds(
min_bound=[torque_min] * tau_mapping.reduce.len +
[activation_min] * biorbd_model.nbMuscleTotal(),
max_bound=[torque_max] * tau_mapping.reduce.len +
[activation_max] * biorbd_model.nbMuscleTotal(),
)
]
U_init = [
InitialConditions([torque_init] * tau_mapping.reduce.len +
[activation_init] * biorbd_model.nbMuscleTotal())
]
# ------------- #
return OptimalControlProgram(
biorbd_model,
problem_type,
number_shooting_points,
phase_time,
X_init,
U_init,
X_bounds,
U_bounds,
objective_functions=objective_functions,
constraints=constraints,
tau_mapping=tau_mapping,
)
def prepare_ocp(biorbd_model_path="cubeSym.bioMod", ode_solver=OdeSolver.RK):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
# Problem parameters
number_shooting_points = 30
final_time = 2
tau_min, tau_max, tau_init = -100, 100, 0
all_generalized_mapping = BidirectionalMapping(Mapping([0, 1, 2, 2], [3]),
Mapping([0, 1, 2]))
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=100)
# Dynamics
dynamics = DynamicsTypeList()
dynamics.add(DynamicsType.TORQUE_DRIVEN)
# Constraints
constraints = ConstraintList()
constraints.add(Constraint.ALIGN_MARKERS,
instant=Instant.START,
first_marker_idx=0,
second_marker_idx=1)
constraints.add(Constraint.ALIGN_MARKERS,
instant=Instant.END,
first_marker_idx=0,
second_marker_idx=2)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model, all_generalized_mapping))
x_bounds[0].min[3:6, [0, -1]] = 0
x_bounds[0].max[3:6, [0, -1]] = 0
# Initial guess
x_init = InitialConditionsList()
x_init.add([0] * all_generalized_mapping.reduce.len * 2)
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([[tau_min] * all_generalized_mapping.reduce.len,
[tau_max] * all_generalized_mapping.reduce.len])
u_init = InitialConditionsList()
u_init.add([tau_init] * all_generalized_mapping.reduce.len)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
all_generalized_mapping=all_generalized_mapping,
)
def prepare_ocp(model_path, phase_time, number_shooting_points, mu):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(model_path)
torque_min, torque_max, torque_init = -500, 500, 0
tau_mapping = BidirectionalMapping(Mapping([-1, -1, -1, 0]), Mapping([3]))
# Add objective functions
objective_functions = ({"type": Objective.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT, "weight": -1},)
# Dynamics
problem_type = ProblemType.torque_driven_with_contact
# Constraints
constraints = (
{
"type": Constraint.CONTACT_FORCE_INEQUALITY,
"direction": "GREATER_THAN",
"instant": Instant.ALL,
"contact_force_idx": 1,
"boundary": 0,
},
{
"type": Constraint.CONTACT_FORCE_INEQUALITY,
"direction": "GREATER_THAN",
"instant": Instant.ALL,
"contact_force_idx": 2,
"boundary": 0,
},
{
"type": Constraint.NON_SLIPPING,
"instant": Instant.ALL,
"normal_component_idx": (1, 2),
"tangential_component_idx": 0,
"static_friction_coefficient": mu,
},
)
# Path constraint
nb_q = biorbd_model.nbQ()
nb_qdot = nb_q
pose_at_first_node = [0, 0, -0.5, 0.5]
# Initialize X_bounds
X_bounds = QAndQDotBounds(biorbd_model)
X_bounds.min[:, 0] = pose_at_first_node + [0] * nb_qdot
X_bounds.max[:, 0] = pose_at_first_node + [0] * nb_qdot
# Initial guess
X_init = InitialConditions(pose_at_first_node + [0] * nb_qdot)
# Define control path constraint
U_bounds = Bounds(min_bound=[torque_min] * tau_mapping.reduce.len, max_bound=[torque_max] * tau_mapping.reduce.len)
U_init = InitialConditions([torque_init] * tau_mapping.reduce.len)
# ------------- #
return OptimalControlProgram(
biorbd_model,
problem_type,
number_shooting_points,
phase_time,
X_init,
U_init,
X_bounds,
U_bounds,
objective_functions,
constraints,
tau_mapping=tau_mapping,
)
def prepare_ocp(
show_online_optim=False,
use_symmetry=True,
):
# --- Options --- #
# Model path
biorbd_model = (
biorbd.Model("jumper2contacts.bioMod"),
biorbd.Model("jumper1contacts.bioMod"),
)
nb_phases = len(biorbd_model)
torque_min, torque_max, torque_init = -1000, 1000, 0
# Problem parameters
number_shooting_points = [8, 8]
phase_time = [0.4, 0.2]
if use_symmetry:
q_mapping = BidirectionalMapping(
Mapping([0, 1, 2, -1, 3, -1, 3, 4, 5, 6, 4, 5, 6], [5]),
Mapping([0, 1, 2, 4, 7, 8, 9]))
q_mapping = q_mapping, q_mapping
tau_mapping = BidirectionalMapping(
Mapping([-1, -1, -1, -1, 0, -1, 0, 1, 2, 3, 1, 2, 3], [5]),
Mapping([4, 7, 8, 9]))
tau_mapping = tau_mapping, tau_mapping
else:
q_mapping = BidirectionalMapping(
Mapping([i for i in range(biorbd_model[0].nbQ())]),
Mapping([i for i in range(biorbd_model[0].nbQ())]),
)
q_mapping = q_mapping, q_mapping
tau_mapping = q_mapping
# Add objective functions
objective_functions = (
(),
(
{
"type": Objective.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT,
"weight": -1
},
{
"type": Objective.Lagrange.MINIMIZE_ALL_CONTROLS,
"weight": 1 / 100
},
),
)
# Dynamics
problem_type = (
ProblemType.torque_driven_with_contact,
ProblemType.torque_driven_with_contact,
)
constraints_first_phase = []
constraints_second_phase = []
contact_axes = (1, 2, 4, 5)
for i in contact_axes:
constraints_first_phase.append({
"type": Constraint.CONTACT_FORCE_GREATER_THAN,
"instant": Instant.ALL,
"idx": i,
"boundary": 0,
})
contact_axes = (1, 3)
for i in contact_axes:
constraints_second_phase.append({
"type": Constraint.CONTACT_FORCE_GREATER_THAN,
"instant": Instant.ALL,
"idx": i,
"boundary": 0,
})
constraints_first_phase.append({
"type": Constraint.NON_SLIPPING,
"instant": Instant.ALL,
"normal_component_idx": (1, 2, 4, 5),
"tangential_component_idx": (0, 3),
"static_friction_coefficient": 0.5,
})
constraints_second_phase.append({
"type": Constraint.NON_SLIPPING,
"instant": Instant.ALL,
"normal_component_idx": (1, 3),
"tangential_component_idx": (0, 2),
"static_friction_coefficient": 0.5,
})
if not use_symmetry:
first_dof = (3, 4, 7, 8, 9)
second_dof = (5, 6, 10, 11, 12)
coeff = (-1, 1, 1, 1, 1)
for i in range(len(first_dof)):
constraints_first_phase.append({
"type": Constraint.PROPORTIONAL_STATE,
"instant": Instant.ALL,
"first_dof": first_dof[i],
"second_dof": second_dof[i],
"coef": coeff[i],
})
for i in range(len(first_dof)):
constraints_second_phase.append({
"type": Constraint.PROPORTIONAL_STATE,
"instant": Instant.ALL,
"first_dof": first_dof[i],
"second_dof": second_dof[i],
"coef": coeff[i],
})
constraints = (constraints_first_phase, constraints_second_phase)
# Path constraint
if use_symmetry:
nb_q = q_mapping[0].reduce.len
nb_qdot = nb_q
pose_at_first_node = [0, 0, -0.5336, 1.4, 0.8, -0.9, 0.47]
else:
nb_q = q_mapping[0].reduce.len
nb_qdot = nb_q
pose_at_first_node = [
0,
0,
-0.5336,
0,
1.4,
0,
1.4,
0.8,
-0.9,
0.47,
0.8,
-0.9,
0.47,
]
# Initialize X_bounds
X_bounds = [
QAndQDotBounds(biorbd_model[i], all_generalized_mapping=q_mapping[i])
for i in range(nb_phases)
]
X_bounds[0].first_node_min = pose_at_first_node + [0] * nb_qdot
X_bounds[0].first_node_max = pose_at_first_node + [0] * nb_qdot
# Initial guess
X_init = [
InitialConditions(pose_at_first_node + [0] * nb_qdot),
InitialConditions(pose_at_first_node + [0] * nb_qdot),
]
# Define control path constraint
U_bounds = [
Bounds(min_bound=[torque_min] * tau_m.reduce.len,
max_bound=[torque_max] * tau_m.reduce.len)
for tau_m in tau_mapping
]
U_init = [
InitialConditions([torque_init] * tau_m.reduce.len)
for tau_m in tau_mapping
]
# ------------- #
return OptimalControlProgram(
biorbd_model,
problem_type,
number_shooting_points,
phase_time,
objective_functions,
X_init,
U_init,
X_bounds,
U_bounds,
constraints,
q_mapping=q_mapping,
q_dot_mapping=q_mapping,
tau_mapping=tau_mapping,
show_online_optim=show_online_optim,
)
def generate_data(biorbd_model,
final_time,
nb_shooting,
use_residual_torque=True):
# Aliases
nb_q = biorbd_model.nbQ()
nb_qdot = biorbd_model.nbQdot()
nb_tau = biorbd_model.nbGeneralizedTorque()
nb_mus = biorbd_model.nbMuscleTotal()
nb_markers = biorbd_model.nbMarkers()
dt = final_time / nb_shooting
if use_residual_torque:
nu = nb_tau + nb_mus
else:
nu = nb_mus
# Casadi related stuff
symbolic_states = MX.sym("x", nb_q + nb_qdot, 1)
symbolic_controls = MX.sym("u", nu, 1)
symbolic_parameters = MX.sym("params", 0, 0)
markers_func = []
for i in range(nb_markers):
markers_func.append(
Function(
"ForwardKin",
[symbolic_states],
[biorbd_model.marker(symbolic_states[:nb_q], i).to_mx()],
["q"],
["marker_" + str(i)],
).expand())
nlp = {
"model":
biorbd_model,
"nbMuscle":
nb_mus,
"q_mapping":
BidirectionalMapping(Mapping(range(nb_q)), Mapping(range(nb_q))),
"q_dot_mapping":
BidirectionalMapping(Mapping(range(nb_qdot)), Mapping(range(nb_qdot))),
"parameters_to_optimize": {},
}
if use_residual_torque:
nlp["nbTau"] = nb_tau
nlp["tau_mapping"] = BidirectionalMapping(Mapping(range(nb_tau)),
Mapping(range(nb_tau)))
dyn_func = DynamicsFunctions.forward_dynamics_torque_muscle_driven
else:
dyn_func = DynamicsFunctions.forward_dynamics_muscle_activations_driven
dynamics_func = Function(
"ForwardDyn",
[symbolic_states, symbolic_controls, symbolic_parameters],
[
dyn_func(symbolic_states, symbolic_controls, symbolic_parameters,
nlp)
],
["x", "u", "p"],
["xdot"],
).expand()
def dyn_interface(t, x, u):
if use_residual_torque:
u = np.concatenate([np.zeros(nb_tau), u])
return np.array(dynamics_func(x, u, np.empty((0, 0)))).squeeze()
# Generate some muscle activation
U = np.random.rand(nb_shooting, nb_mus).T
# Integrate and collect the position of the markers accordingly
X = np.ndarray((nb_q + nb_qdot, nb_shooting + 1))
markers = np.ndarray((3, biorbd_model.nbMarkers(), nb_shooting + 1))
def add_to_data(i, q):
X[:, i] = q
for j, mark_func in enumerate(markers_func):
markers[:, j, i] = np.array(mark_func(q)).squeeze()
x_init = np.array([0] * nb_q + [0] * nb_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, nb_shooting + 1)
return time_interp, markers, X, U
