def prepare_ocp(biorbd_model_path, number_shooting_points, final_time, loop_from_constraint, ode_solver=OdeSolver.RK):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
# Problem parameters
tau_min, tau_max, tau_init = -100, 100, 0
# Add objective functions
objective_functions = ObjectiveOption(Objective.Lagrange.MINIMIZE_TORQUE, weight=100)
# Dynamics
dynamics = DynamicsTypeOption(DynamicsType.TORQUE_DRIVEN)
# Constraints
constraints = ConstraintList()
constraints.add(Constraint.ALIGN_MARKERS, instant=Instant.MID, first_marker_idx=0, second_marker_idx=2)
constraints.add(Constraint.TRACK_STATE, instant=Instant.MID, states_idx=2)
constraints.add(Constraint.ALIGN_MARKERS, instant=Instant.END, first_marker_idx=0, second_marker_idx=1)
# Path constraint
x_bounds = BoundsOption(QAndQDotBounds(biorbd_model))
x_bounds.min[2:6, -1] = [1.57, 0, 0, 0]
x_bounds.max[2:6, -1] = [1.57, 0, 0, 0]
# Initial guess
x_init = InitialConditionsOption([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()))
# Define control path constraint
u_bounds = BoundsOption(
[[tau_min] * biorbd_model.nbGeneralizedTorque(), [tau_max] * biorbd_model.nbGeneralizedTorque()]
)
u_init = InitialConditionsOption([tau_init] * biorbd_model.nbGeneralizedTorque())
# ------------- #
# A state transition loop constraint is treated as
# hard penalty (constraint) if weight is <= 0 [or if no weight is provided], or
# as a soft penalty (objective) otherwise
state_transitions = StateTransitionList()
if loop_from_constraint:
state_transitions.add(StateTransition.CYCLIC, weight=0)
else:
state_transitions.add(StateTransition.CYCLIC, weight=10000)
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,
state_transitions=state_transitions,
)
def prepare_ocp(biorbd_model_path, final_time, number_shooting_points,
time_min, time_max):
# --- Options --- #
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()
# Add objective functions
objective_functions = ObjectiveOption(Objective.Lagrange.MINIMIZE_TORQUE)
# Dynamics
dynamics = DynamicsTypeOption(DynamicsType.TORQUE_DRIVEN)
# Constraints
constraints = ConstraintOption(Constraint.TIME_CONSTRAINT,
instant=Instant.END,
minimum=time_min,
maximum=time_max)
# Path constraint
x_bounds = BoundsOption(QAndQDotBounds(biorbd_model))
x_bounds.min[:, [0, -1]] = 0
x_bounds.max[:, [0, -1]] = 0
x_bounds.min[n_q - 1, -1] = 3.14
x_bounds.max[n_q - 1, -1] = 3.14
# Initial guess
x_init = InitialConditionsOption([0] * (n_q + n_qdot))
# Define control path constraint
u_bounds = BoundsOption([[tau_min] * n_tau, [tau_max] * n_tau])
u_bounds.min[n_tau - 1, :] = 0
u_bounds.max[n_tau - 1, :] = 0
u_init = InitialConditionsOption([tau_init] * n_tau)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
)
def prepare_ocp(biorbd_model_path, 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
# Add objective functions
objective_functions = ObjectiveOption(Objective.Lagrange.MINIMIZE_TORQUE,
weight=100)
# Dynamics
dynamics = DynamicsTypeOption(DynamicsType.TORQUE_DRIVEN)
# Constraints
constraints = ConstraintList()
constraints.add(custom_func_align_markers,
instant=Instant.START,
first_marker_idx=0,
second_marker_idx=1)
constraints.add(custom_func_align_markers,
instant=Instant.END,
first_marker_idx=0,
second_marker_idx=2)
# Path constraint
x_bounds = BoundsOption(QAndQDotBounds(biorbd_model))
x_bounds.min[1:6, [0, -1]] = 0
x_bounds.max[1:6, [0, -1]] = 0
x_bounds.min[2, -1] = 1.57
x_bounds.max[2, -1] = 1.57
# Initial guess
x_init = InitialConditionsOption(
[0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()))
# Define control path constraint
u_bounds = BoundsOption([[tau_min] * biorbd_model.nbGeneralizedTorque(),
[tau_max] * biorbd_model.nbGeneralizedTorque()])
u_init = InitialConditionsOption([tau_init] *
biorbd_model.nbGeneralizedTorque())
# ------------- #
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,
)
def prepare_ocp(
biorbd_model_path,
number_shooting_points,
final_time,
initial_guess=InterpolationType.CONSTANT,
):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
nq = biorbd_model.nbQ()
nqdot = biorbd_model.nbQdot()
ntau = biorbd_model.nbGeneralizedTorque()
tau_min, tau_max, tau_init = -100, 100, 0
# Add objective functions
objective_functions = ObjectiveOption(Objective.Lagrange.MINIMIZE_TORQUE,
weight=100)
# Dynamics
dynamics = DynamicsTypeOption(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 constraints
x_bounds = BoundsOption(QAndQDotBounds(biorbd_model))
x_bounds.min[1:6, [0, -1]] = 0
x_bounds.max[1:6, [0, -1]] = 0
u_bounds = BoundsOption([[tau_min] * ntau, [tau_max] * ntau])
# Initial guesses
t = None
extra_params_x = {}
extra_params_u = {}
if initial_guess == InterpolationType.CONSTANT:
x = [0] * (nq + nqdot)
u = [tau_init] * ntau
elif initial_guess == InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT:
x = np.array([[1.0, 0.0, 0.0, 0, 0, 0], [1.5, 0.0, 0.785, 0, 0, 0],
[2.0, 0.0, 1.57, 0, 0, 0]]).T
u = np.array([[1.45, 9.81, 2.28], [0, 9.81, 0], [-1.45, 9.81,
-2.28]]).T
elif initial_guess == InterpolationType.LINEAR:
x = np.array([[1.0, 0.0, 0.0, 0, 0, 0], [2.0, 0.0, 1.57, 0, 0, 0]]).T
u = np.array([[1.45, 9.81, 2.28], [-1.45, 9.81, -2.28]]).T
elif initial_guess == InterpolationType.EACH_FRAME:
x = np.random.random((nq + nqdot, number_shooting_points + 1))
u = np.random.random((ntau, number_shooting_points))
elif initial_guess == InterpolationType.SPLINE:
# Bound spline assume the first and last point are 0 and final respectively
t = np.hstack((0, np.sort(np.random.random(
(3, )) * final_time), final_time))
x = np.random.random((nq + nqdot, 5))
u = np.random.random((ntau, 5))
elif initial_guess == InterpolationType.CUSTOM:
# The custom function refers to the one at the beginning of the file. It emulates a Linear interpolation
x = custom_init_func
u = custom_init_func
extra_params_x = {
"my_values": np.random.random((nq + nqdot, 2)),
"nb_shooting": number_shooting_points
}
extra_params_u = {
"my_values": np.random.random((ntau, 2)),
"nb_shooting": number_shooting_points
}
else:
raise RuntimeError("Initial guess not implemented yet")
x_init = InitialConditionsOption(x,
t=t,
interpolation=initial_guess,
**extra_params_x)
u_init = InitialConditionsOption(u,
t=t,
interpolation=initial_guess,
**extra_params_u)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
)
def prepare_ocp(biorbd_model_path, final_time, number_shooting_points, min_g,
max_g, target_g):
# --- Options --- #
biorbd_model = biorbd.Model(biorbd_model_path)
tau_min, tau_max, tau_init = -30, 30, 0
n_q = biorbd_model.nbQ()
n_qdot = biorbd_model.nbQdot()
n_tau = biorbd_model.nbGeneralizedTorque()
# Add objective functions
objective_functions = ObjectiveOption(Objective.Lagrange.MINIMIZE_TORQUE,
weight=10)
# Dynamics
dynamics = DynamicsTypeOption(DynamicsType.TORQUE_DRIVEN)
# Path constraint
x_bounds = BoundsOption(QAndQDotBounds(biorbd_model))
x_bounds.min[:, [0, -1]] = 0
x_bounds.max[:, [0, -1]] = 0
x_bounds.min[1, -1] = 3.14
x_bounds.max[1, -1] = 3.14
# Initial guess
x_init = InitialConditionsOption([0] * (n_q + n_qdot))
# Define control path constraint
u_bounds = BoundsOption([[tau_min] * n_tau, [tau_max] * n_tau])
u_bounds.min[1, :] = 0
u_bounds.max[1, :] = 0
u_init = InitialConditionsOption([tau_init] * n_tau)
# Define the parameter to optimize
# Give the parameter some min and max bounds
parameters = ParameterList()
bound_gravity = Bounds(min_bound=min_g,
max_bound=max_g,
interpolation=InterpolationType.CONSTANT)
# and an initial condition
initial_gravity = InitialConditions((min_g + max_g) / 2)
parameter_objective_functions = ObjectiveOption(
my_target_function,
weight=10,
quadratic=True,
custom_type=Objective.Parameter,
target_value=target_g)
parameters.add(
"gravity_z", # The name of the parameter
my_parameter_function, # The function that modifies the biorbd model
initial_gravity, # The initial guess
bound_gravity, # The bounds
size=1, # The number of elements this particular parameter vector has
penalty_list=
parameter_objective_functions, # Objective of constraint for this particular parameter
extra_value=1, # You can define as many extra arguments as you want
)
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
parameters=parameters,
)
def prepare_ocp(
biorbd_model_path,
number_shooting_points,
final_time,
interpolation_type=InterpolationType.
CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT,
):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
nq = biorbd_model.nbQ()
nqdot = biorbd_model.nbQdot()
ntau = biorbd_model.nbGeneralizedTorque()
tau_min, tau_max, tau_init = -100, 100, 0
# Add objective functions
objective_functions = ObjectiveOption(Objective.Lagrange.MINIMIZE_TORQUE)
# Dynamics
dynamics = DynamicsTypeOption(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 constraints
if interpolation_type == InterpolationType.CONSTANT:
x_min = [-100] * (nq + nqdot)
x_max = [100] * (nq + nqdot)
x_bounds = BoundsOption([x_min, x_max],
interpolation=InterpolationType.CONSTANT)
u_min = [tau_min] * ntau
u_max = [tau_max] * ntau
u_bounds = BoundsOption([u_min, u_max],
interpolation=InterpolationType.CONSTANT)
elif interpolation_type == InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT:
x_min = np.random.random((6, 3)) * (-10) - 5
x_max = np.random.random((6, 3)) * 10 + 5
x_bounds = BoundsOption([x_min, x_max],
interpolation=InterpolationType.
CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT)
u_min = np.random.random((3, 3)) * tau_min + tau_min / 2
u_max = np.random.random((3, 3)) * tau_max + tau_max / 2
u_bounds = BoundsOption([u_min, u_max],
interpolation=InterpolationType.
CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT)
elif interpolation_type == InterpolationType.LINEAR:
x_min = np.random.random((6, 2)) * (-10) - 5
x_max = np.random.random((6, 2)) * 10 + 5
x_bounds = BoundsOption([x_min, x_max],
interpolation=InterpolationType.LINEAR)
u_min = np.random.random((3, 2)) * tau_min + tau_min / 2
u_max = np.random.random((3, 2)) * tau_max + tau_max / 2
u_bounds = BoundsOption([u_min, u_max],
interpolation=InterpolationType.LINEAR)
elif interpolation_type == InterpolationType.EACH_FRAME:
x_min = np.random.random(
(nq + nqdot, number_shooting_points + 1)) * (-10) - 5
x_max = np.random.random(
(nq + nqdot, number_shooting_points + 1)) * 10 + 5
x_bounds = BoundsOption([x_min, x_max],
interpolation=InterpolationType.EACH_FRAME)
u_min = np.random.random(
(ntau, number_shooting_points)) * tau_min + tau_min / 2
u_max = np.random.random(
(ntau, number_shooting_points)) * tau_max + tau_max / 2
u_bounds = BoundsOption([u_min, u_max],
interpolation=InterpolationType.EACH_FRAME)
elif interpolation_type == InterpolationType.SPLINE:
spline_time = np.hstack(
(0, np.sort(np.random.random((3, )) * final_time), final_time))
x_min = np.random.random((nq + nqdot, 5)) * (-10) - 5
x_max = np.random.random((nq + nqdot, 5)) * 10 + 5
u_min = np.random.random((ntau, 5)) * tau_min + tau_min / 2
u_max = np.random.random((ntau, 5)) * tau_max + tau_max / 2
x_bounds = BoundsOption([x_min, x_max],
interpolation=InterpolationType.SPLINE,
t=spline_time)
u_bounds = BoundsOption([u_min, u_max],
interpolation=InterpolationType.SPLINE,
t=spline_time)
elif interpolation_type == InterpolationType.CUSTOM:
# The custom functions refer to the ones at the beginning of the file. As for instance, they emulate a Linear interpolation
extra_params_x = {
"n_elements": nq + nqdot,
"nb_shooting": number_shooting_points
}
extra_params_u = {
"n_elements": ntau,
"nb_shooting": number_shooting_points
}
x_bounds = BoundsOption([custom_x_bounds_min, custom_x_bounds_max],
interpolation=InterpolationType.CUSTOM,
**extra_params_x)
u_bounds = BoundsOption([custom_u_bounds_min, custom_u_bounds_max],
interpolation=InterpolationType.CUSTOM,
**extra_params_u)
else:
raise NotImplementedError("Not implemented yet")
# Initial guess
x_init = InitialConditionsOption([0] * (nq + nqdot))
u_init = InitialConditionsOption([tau_init] * ntau)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
)