def prepare_ocp(model_path,
phase_time,
number_shooting_points,
min_bound,
ode_solver=OdeSolver.RK):
# --- 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 = ObjectiveList()
objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT,
weight=-1)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.MUSCLE_EXCITATIONS_AND_TORQUE_DRIVEN_WITH_CONTACT)
# Constraints
constraints = ConstraintList()
constraints.add(
ConstraintFcn.CONTACT_FORCE,
min_bound=min_bound,
max_bound=np.inf,
node=Node.ALL,
contact_force_idx=1,
)
constraints.add(
ConstraintFcn.CONTACT_FORCE,
min_bound=min_bound,
max_bound=np.inf,
node=Node.ALL,
contact_force_idx=2,
)
# 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 = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
x_bounds[0].concatenate(
Bounds([activation_min] * nb_mus, [activation_max] * nb_mus))
x_bounds[0][:, 0] = pose_at_first_node + [0] * nb_qdot + [0.5] * nb_mus
# Initial guess
x_init = InitialGuessList()
x_init.add(pose_at_first_node + [0] * nb_qdot + [0.5] * nb_mus)
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add(
[torque_min] * tau_mapping.reduce.len +
[activation_min] * biorbd_model.nbMuscleTotal(),
[torque_max] * tau_mapping.reduce.len +
[activation_max] * biorbd_model.nbMuscleTotal(),
)
u_init = InitialGuessList()
u_init.add([torque_init] * tau_mapping.reduce.len +
[activation_init] * biorbd_model.nbMuscleTotal())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
phase_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions=objective_functions,
constraints=constraints,
tau_mapping=tau_mapping,
ode_solver=ode_solver,
)
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_q = MX.sym("q", nb_q, 1)
symbolic_qdot = MX.sym("q_dot", nb_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(
model=biorbd_model,
shape={"muscle": nb_mus},
mapping={
"q":
BidirectionalMapping(Mapping(range(nb_q)), Mapping(range(nb_q))),
"q_dot":
BidirectionalMapping(Mapping(range(nb_qdot)),
Mapping(range(nb_qdot))),
},
)
if use_residual_torque:
nlp.shape["tau"] = nb_tau
nlp.mapping["tau"] = 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
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(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
markers[:, :, i] = markers_func(q[0:nb_q])
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
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(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
# Constraints
constraints = ConstraintList()
constraints.add(ConstraintFcn.ALIGN_MARKERS,
node=Node.START,
first_marker_idx=0,
second_marker_idx=1)
constraints.add(ConstraintFcn.ALIGN_MARKERS,
node=Node.END,
first_marker_idx=0,
second_marker_idx=2)
# 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.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 = InitialGuessList()
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,
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][:, 0] = pose_at_first_node + [0] * nb_qdot
# Initial guess
x_init = InitialGuessList()
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 = InitialGuessList()
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, ns, time_min, time_max, init):
# --- Options --- #
# Model path
biorbd_model = [biorbd.Model(elt) for elt in model_path]
nb_phases = len(biorbd_model)
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, 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, tau_mapping, tau_mapping, tau_mapping
nq = len(q_mapping[0].reduce.map_idx)
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT,
weight=-100,
phase=1)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN_WITH_CONTACT)
dynamics.add(DynamicsFcn.TORQUE_DRIVEN_WITH_CONTACT)
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
dynamics.add(DynamicsFcn.TORQUE_DRIVEN_WITH_CONTACT)
dynamics.add(DynamicsFcn.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(ConstraintFcn.CONTACT_FORCE,
phase=0,
node=Node.ALL,
contact_force_idx=i,
max_bound=np.inf)
constraints.add(ConstraintFcn.CONTACT_FORCE,
phase=4,
node=Node.ALL,
contact_force_idx=i,
max_bound=np.inf)
contact_axes = (1, 3)
for i in contact_axes:
constraints.add(ConstraintFcn.CONTACT_FORCE,
phase=1,
node=Node.ALL,
contact_force_idx=i,
max_bound=np.inf)
constraints.add(ConstraintFcn.CONTACT_FORCE,
phase=3,
node=Node.ALL,
contact_force_idx=i,
max_bound=np.inf)
# 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(
ConstraintFcn.NON_SLIPPING,
phase=0,
node=Node.ALL,
normal_component_idx=(1, 2),
tangential_component_idx=0,
static_friction_coefficient=0.5,
)
constraints.add(
ConstraintFcn.NON_SLIPPING,
phase=1,
node=Node.ALL,
normal_component_idx=1,
tangential_component_idx=0,
static_friction_coefficient=0.5,
)
constraints.add(
ConstraintFcn.NON_SLIPPING,
phase=3,
node=Node.ALL,
normal_component_idx=1,
tangential_component_idx=0,
static_friction_coefficient=0.5,
)
constraints.add(
ConstraintFcn.NON_SLIPPING,
phase=4,
node=Node.ALL,
normal_component_idx=(1, 2),
tangential_component_idx=0,
static_friction_coefficient=0.5,
)
# Custom constraints for contact forces at transitions
constraints.add(utils.toe_on_floor,
phase=3,
node=Node.START,
min_bound=-0.0001,
max_bound=0.0001)
constraints.add(utils.heel_on_floor,
phase=4,
node=Node.START,
min_bound=-0.0001,
max_bound=0.0001)
# Custom constraints for positivity of CoM_dot on z axis just before the take-off
constraints.add(utils.com_dot_z,
phase=1,
node=Node.END,
min_bound=0,
max_bound=np.inf)
# Constraint arm positivity
constraints.add(ConstraintFcn.TRACK_STATE,
phase=1,
node=Node.END,
index=3,
min_bound=1.0,
max_bound=np.inf)
# Constraint foot positivity
constraints.add(utils.heel_on_floor,
phase=1,
node=Node.ALL,
min_bound=-0.0001,
max_bound=np.inf)
constraints.add(utils.heel_on_floor,
phase=2,
node=Node.ALL,
min_bound=-0.0001,
max_bound=np.inf)
constraints.add(utils.toe_on_floor,
phase=2,
node=Node.ALL,
min_bound=-0.0001,
max_bound=np.inf)
constraints.add(utils.heel_on_floor,
phase=3,
node=Node.ALL,
min_bound=-0.0001,
max_bound=np.inf)
# Torque constraint + minimize_state
for i in range(nb_phases):
constraints.add(utils.tau_actuator_constraints,
phase=i,
node=Node.ALL,
minimal_tau=20)
objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_TIME,
weight=0.0001,
phase=i,
min_bound=time_min[i],
max_bound=time_max[i])
# State transitions
state_transitions = StateTransitionList()
state_transitions.add(StateTransition.IMPACT, phase_pre_idx=2)
state_transitions.add(StateTransition.IMPACT, phase_pre_idx=3)
# Path constraint
nb_q = q_mapping[0].reduce.len
nb_q_dot = nb_q
pose_at_first_node = [0, 0, -0.5336, 1.4, 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(bounds=QAndQDotBounds(
biorbd_model[i], all_generalized_mapping=q_mapping[i]))
x_bounds[0][:, 0] = pose_at_first_node + [0] * nb_q_dot
x_bounds[3].min[13, 0] = -1000
x_bounds[3].max[13, 0] = 1000
x_bounds[4].min[13, 0] = -1000
x_bounds[4].max[13, 0] = 1000
x_bounds[4][2:, -1] = pose_at_first_node[2:] + [0] * nb_q_dot
# Initial guess for states (Interpolation type is CONSTANT)
x_init = InitialGuessList()
for i in range(nb_phases):
x_init.add(pose_at_first_node + [0] * nb_q_dot)
# Define control path constraint
u_bounds = BoundsList()
for i in range(nb_phases):
u_bounds.add([-500] * tau_mapping[i].reduce.len,
[500] * tau_mapping[i].reduce.len)
# Define initial guess for controls
u_init = InitialGuessList()
for i in range(nb_phases):
if init is not None:
u_init.add(init)
else:
u_init.add([0] * tau_mapping[i].reduce.len)
# ------------- #
ocp = OptimalControlProgram(
biorbd_model,
dynamics,
ns,
phase_time,
x_init=x_init,
x_bounds=x_bounds,
u_init=u_init,
u_bounds=u_bounds,
objective_functions=objective_functions,
constraints=constraints,
q_mapping=q_mapping,
q_dot_mapping=q_mapping,
tau_mapping=tau_mapping,
state_transitions=state_transitions,
nb_threads=2,
use_SX=False,
)
return utils.add_custom_plots(ocp, nb_phases, x_bounds, nq, minimal_tau=20)
def prepare_ocp(model_path, phase_time, number_shooting_points, mu, ode_solver=OdeSolver.RK):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(model_path)
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(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,
max_bound=np.inf,
node=Node.ALL,
contact_force_idx=1,
)
constraints.add(
ConstraintFcn.CONTACT_FORCE,
max_bound=np.inf,
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
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(bounds=QAndQDotBounds(biorbd_model))
x_bounds[0][:, 0] = pose_at_first_node + [0] * nb_qdot
# Initial guess
x_init = InitialGuessList()
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 = InitialGuessList()
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,
constraints,
tau_mapping=tau_mapping,
ode_solver=ode_solver,
)
def prepare_ocp(model_path, phase_time, number_shooting_points, min_bound, max_bound):
# --- 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,
min_bound=min_bound,
max_bound=max_bound,
node=Node.ALL,
contact_force_idx=1,
)
constraints.add(
Constraint.CONTACT_FORCE,
min_bound=min_bound,
max_bound=max_bound,
node=Node.ALL,
contact_force_idx=2,
)
# 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][:, 0] = pose_at_first_node + [0] * nb_qdot
# Initial guess
x_init = InitialGuessList()
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 = InitialGuessList()
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,
)