def test_acados_custom_dynamics(problem_type_custom):
if platform == "win32":
print("Test for ACADOS on Windows is skipped")
return
from bioptim.examples.getting_started import custom_dynamics as ocp_module
bioptim_folder = os.path.dirname(ocp_module.__file__)
ocp = ocp_module.prepare_ocp(
biorbd_model_path=bioptim_folder + "/models/cube.bioMod",
problem_type_custom=problem_type_custom,
ode_solver=OdeSolver.RK4(),
use_sx=True,
)
constraints = ConstraintList()
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m2")
ocp.update_constraints(constraints)
sol = ocp.solve(solver=Solver.ACADOS())
# Check some of the results
q, qdot, tau = sol.states["q"], sol.states["qdot"], sol.controls["tau"]
# initial and final position
np.testing.assert_almost_equal(q[:, 0], np.array((2, 0, 0)), decimal=6)
np.testing.assert_almost_equal(q[:, -1], np.array((2, 0, 1.57)))
# initial and final velocities
np.testing.assert_almost_equal(qdot[:, 0], np.array((0, 0, 0)))
np.testing.assert_almost_equal(qdot[:, -1], np.array((0, 0, 0)))
# initial and final controls
np.testing.assert_almost_equal(tau[:, 0], np.array((0, 9.81, 2.27903226)))
np.testing.assert_almost_equal(tau[:, -2], np.array((0, 9.81, -2.27903226)))
def test_acados_custom_dynamics(problem_type_custom):
bioptim_folder = TestUtils.bioptim_folder()
cube = TestUtils.load_module(
bioptim_folder + "/examples/getting_started/custom_dynamics.py")
ocp = cube.prepare_ocp(
biorbd_model_path=bioptim_folder +
"/examples/getting_started/cube.bioMod",
problem_type_custom=problem_type_custom,
ode_solver=OdeSolver.RK4(),
use_sx=True,
)
constraints = ConstraintList()
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.END,
first_marker_idx=0,
second_marker_idx=2)
ocp.update_constraints(constraints)
sol = ocp.solve(solver=Solver.ACADOS)
# Check some of the results
q, qdot, tau = sol.states["q"], sol.states["qdot"], sol.controls["tau"]
# initial and final position
np.testing.assert_almost_equal(q[:, 0], np.array((2, 0, 0)), decimal=6)
np.testing.assert_almost_equal(q[:, -1], np.array((2, 0, 1.57)))
# initial and final velocities
np.testing.assert_almost_equal(qdot[:, 0], np.array((0, 0, 0)))
np.testing.assert_almost_equal(qdot[:, -1], np.array((0, 0, 0)))
# initial and final controls
np.testing.assert_almost_equal(tau[:, 0], np.array((0, 9.81, 2.27903226)))
np.testing.assert_almost_equal(tau[:, -1], np.array(
(0, 9.81, -2.27903226)))
def prepare_ocp(biorbd_model_path,
final_time,
number_shooting_points,
ode_solver=OdeSolver.RK):
# --- Options --- #nq
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
nq = biorbd_model.nbQ()
# Problem parameters
tau_min, tau_max, tau_init = -100, 100, 0
# 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_SEGMENT_WITH_CUSTOM_RT,
node=Node.ALL,
segment_idx=2,
rt_idx=0)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
x_bounds[0][2, [0, -1]] = [-1.57, 1.57]
x_bounds[0][nq:, [0, -1]] = 0
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()))
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([tau_min] * biorbd_model.nbGeneralizedTorque(),
[tau_max] * biorbd_model.nbGeneralizedTorque())
u_init = InitialGuessList()
u_init.add([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 test_acados_constraints_all():
PROJECT_FOLDER = Path(__file__).parent / ".."
spec = importlib.util.spec_from_file_location(
"constraint",
str(PROJECT_FOLDER) + "/examples/align/align_marker_on_segment.py",
)
constraint = importlib.util.module_from_spec(spec)
spec.loader.exec_module(constraint)
ocp = constraint.prepare_ocp(
biorbd_model_path=str(PROJECT_FOLDER) +
"/examples/align/cube_and_line.bioMod",
number_shooting_points=30,
final_time=2,
initialize_near_solution=True,
constr=False,
use_SX=True,
)
constraints = ConstraintList()
constraints.add(ConstraintFcn.ALIGN_MARKER_WITH_SEGMENT_AXIS,
node=Node.ALL,
marker_idx=1,
segment_idx=2,
axis=(Axe.X))
ocp.update_constraints(constraints)
sol = ocp.solve(solver=Solver.ACADOS, solver_options={"print_level": 0})
# Check some of the results
states, controls = Data.get_data(ocp, sol["x"])
q, qdot, tau = states["q"], states["q_dot"], controls["tau"]
# final position
np.testing.assert_almost_equal(q[:, 0],
np.array([0.8385190835, 0, 0,
-0.212027938]),
decimal=6)
np.testing.assert_almost_equal(q[:, -1],
np.array(
(0.8385190835, 0, 1.57, -0.212027938)),
decimal=6)
np.testing.assert_almost_equal(qdot[:, 0],
np.array([0, 0, 0, 0]),
decimal=6)
np.testing.assert_almost_equal(qdot[:, -1],
np.array([0, 0, 0, 0]),
decimal=6)
# initial and final controls
np.testing.assert_almost_equal(tau[:, 0],
np.array((0, 9.81, 2.27903226, 0)),
decimal=6)
np.testing.assert_almost_equal(tau[:, -1],
np.array((0, 9.81, -2.27903226, 0)),
decimal=6)
def __init__(
self,
model_path: str,
violin: Violin,
bow: Bow,
n_cycles: int,
bow_starting: BowPosition.TIP,
init_file: str = None,
use_muscles: bool = True,
time_per_cycle: float = 1,
n_shooting_per_cycle: int = 30,
solver: Solver = Solver.IPOPT,
n_threads: int = 8,
):
self.model_path = model_path
self.model = biorbd.Model(self.model_path)
self.n_q = self.model.nbQ()
self.n_tau = self.model.nbGeneralizedTorque()
self.use_muscles = use_muscles
self.n_mus = self.model.nbMuscles() if self.use_muscles else 0
self.violin = violin
self.bow = bow
self.bow_starting = bow_starting
self.n_cycles = n_cycles
self.n_shooting_per_cycle = n_shooting_per_cycle
self.n_shooting = self.n_shooting_per_cycle * self.n_cycles
self.time_per_cycle = time_per_cycle
self.time = self.time_per_cycle * self.n_cycles
self.solver = solver
self.n_threads = n_threads
if self.use_muscles:
self.dynamics = Dynamics(
DynamicsFcn.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN)
else:
self.dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN)
self.x_bounds = Bounds()
self.u_bounds = Bounds()
self._set_bounds()
self.x_init = InitialGuess()
self.u_init = InitialGuess()
self._set_initial_guess(init_file)
self.objective_functions = ObjectiveList()
self._set_generic_objective_functions()
self.constraints = ConstraintList()
self._set_generic_constraints()
self._set_generic_ocp()
if use_muscles:
online_muscle_torque(self.ocp)
def test_acados_constraints_all():
if platform == "win32":
print("Test for ACADOS on Windows is skipped")
return
bioptim_folder = TestUtils.bioptim_folder()
track = TestUtils.load_module(bioptim_folder +
"/examples/track/track_marker_on_segment.py")
ocp = track.prepare_ocp(
biorbd_model_path=bioptim_folder +
"/examples/track/models/cube_and_line.bioMod",
n_shooting=30,
final_time=2,
initialize_near_solution=True,
constr=False,
use_sx=True,
)
constraints = ConstraintList()
constraints.add(ConstraintFcn.TRACK_MARKER_WITH_SEGMENT_AXIS,
node=Node.ALL,
marker="m1",
segment="seg_rt",
axis=Axis.X)
ocp.update_constraints(constraints)
sol = ocp.solve(solver=Solver.ACADOS)
# Check some of the results
q, qdot, tau = sol.states["q"], sol.states["qdot"], sol.controls["tau"]
# final position
np.testing.assert_almost_equal(q[:, 0],
np.array([2.28988221, 0, 0,
2.95087911e-01]),
decimal=6)
np.testing.assert_almost_equal(q[:, -1],
np.array(
(2.28215749, 0, 1.57, 6.62470772e-01)),
decimal=6)
np.testing.assert_almost_equal(qdot[:, 0],
np.array([0, 0, 0, 0]),
decimal=6)
np.testing.assert_almost_equal(qdot[:, -1],
np.array([0, 0, 0, 0]),
decimal=6)
# initial and final controls
np.testing.assert_almost_equal(tau[:, 0],
np.array((0.04483914, 9.90739842,
2.24951691, 0.78496612)),
decimal=6)
np.testing.assert_almost_equal(tau[:, -2],
np.array((0.15945561, 10.03978178,
-2.36075327, 0.07267697)),
decimal=6)
def test_acados_one_end_constraints():
if platform == "win32":
print("Test for ACADOS on Windows is skipped")
return
bioptim_folder = TestUtils.bioptim_folder()
cube = TestUtils.load_module(bioptim_folder + "/examples/acados/cube.py")
ocp = cube.prepare_ocp(
biorbd_model_path=bioptim_folder +
"/examples/acados/models/cube.bioMod",
n_shooting=10,
tf=2,
)
model = ocp.nlp[0].model
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Mayer.TRACK_STATE,
index=0,
key="q",
weight=100,
target=np.array([[1]]),
multi_thread=False)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL,
key="tau",
weight=100,
multi_thread=False)
ocp.update_objectives(objective_functions)
# Path constraint
x_bounds = QAndQDotBounds(model)
x_bounds[1:6, [0, -1]] = 0
x_bounds[0, 0] = 0
ocp.update_bounds(x_bounds=x_bounds)
constraints = ConstraintList()
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.END,
first_marker="m0",
second_marker="m2")
ocp.update_constraints(constraints)
sol = ocp.solve(solver=Solver.ACADOS)
# Check some of the results
q, qdot, tau = sol.states["q"], sol.states["qdot"], sol.controls["tau"]
# final position
np.testing.assert_almost_equal(q[:, -1], np.array((2, 0, 0)), decimal=6)
# initial and final controls
np.testing.assert_almost_equal(tau[:, 0],
np.array((2.72727272, 9.81, 0)),
decimal=6)
np.testing.assert_almost_equal(tau[:, -2],
np.array((-2.72727272, 9.81, 0)),
decimal=6)
def test_acados_constraints_all():
if platform == "win32":
print("Test for ACADOS on Windows is skipped")
return
bioptim_folder = TestUtils.bioptim_folder()
track = TestUtils.load_module(bioptim_folder +
"/examples/track/track_marker_on_segment.py")
ocp = track.prepare_ocp(
biorbd_model_path=bioptim_folder +
"/examples/track/cube_and_line.bioMod",
n_shooting=30,
final_time=2,
initialize_near_solution=True,
constr=False,
use_sx=True,
)
constraints = ConstraintList()
constraints.add(ConstraintFcn.TRACK_MARKER_WITH_SEGMENT_AXIS,
node=Node.ALL,
marker="m1",
segment="seg_rt",
axis=(Axis.X))
ocp.update_constraints(constraints)
sol = ocp.solve(solver=Solver.ACADOS)
# Check some of the results
q, qdot, tau = sol.states["q"], sol.states["qdot"], sol.controls["tau"]
# final position
np.testing.assert_almost_equal(q[:, 0],
np.array([0.8385190835, 0, 0,
-0.212027938]),
decimal=6)
np.testing.assert_almost_equal(q[:, -1],
np.array(
(0.8385190835, 0, 1.57, -0.212027938)),
decimal=6)
np.testing.assert_almost_equal(qdot[:, 0],
np.array([0, 0, 0, 0]),
decimal=6)
np.testing.assert_almost_equal(qdot[:, -1],
np.array([0, 0, 0, 0]),
decimal=6)
# initial and final controls
np.testing.assert_almost_equal(tau[:, 0],
np.array((0, 9.81, 2.27903226, 0)),
decimal=6)
np.testing.assert_almost_equal(tau[:, -1],
np.array((0, 9.81, -2.27903226, 0)),
decimal=6)
def test_acados_one_end_constraints():
PROJECT_FOLDER = Path(__file__).parent / ".."
spec = importlib.util.spec_from_file_location(
"constraint",
str(PROJECT_FOLDER) + "/examples/acados/cube.py",
)
constraint = importlib.util.module_from_spec(spec)
spec.loader.exec_module(constraint)
ocp = constraint.prepare_ocp(
biorbd_model_path=str(PROJECT_FOLDER) + "/examples/acados/cube.bioMod",
nbs=10,
tf=2,
)
model = ocp.nlp[0].model
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Mayer.TRACK_STATE,
index=0,
weight=100,
target=np.array([[1]]))
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100)
ocp.update_objectives(objective_functions)
# Path constraint
x_bounds = QAndQDotBounds(model)
x_bounds[1:6, [0, -1]] = 0
x_bounds[0, 0] = 0
ocp.update_bounds(x_bounds=x_bounds)
constraints = ConstraintList()
constraints.add(ConstraintFcn.ALIGN_MARKERS,
node=Node.END,
first_marker_idx=0,
second_marker_idx=2)
ocp.update_constraints(constraints)
sol = ocp.solve(solver=Solver.ACADOS, solver_options={"print_level": 0})
# Check some of the results
states, controls = Data.get_data(ocp, sol["x"])
q, qdot, tau = states["q"], states["q_dot"], controls["tau"]
# final position
np.testing.assert_almost_equal(q[:, -1], np.array((2, 0, 0)), decimal=6)
# initial and final controls
np.testing.assert_almost_equal(tau[:, 0],
np.array((2.72727272, 9.81, 0)),
decimal=6)
np.testing.assert_almost_equal(tau[:, -1],
np.array((-2.72727272, 9.81, 0)),
decimal=6)
def test_acados_one_end_constraints():
bioptim_folder = TestUtils.bioptim_folder()
cube = TestUtils.load_module(bioptim_folder + "/examples/acados/cube.py")
ocp = cube.prepare_ocp(
biorbd_model_path=bioptim_folder + "/examples/acados/cube.bioMod",
n_shooting=10,
tf=2,
)
model = ocp.nlp[0].model
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Mayer.TRACK_STATE,
index=0,
weight=100,
target=np.array([[1]]))
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100)
ocp.update_objectives(objective_functions)
# Path constraint
x_bounds = QAndQDotBounds(model)
x_bounds[1:6, [0, -1]] = 0
x_bounds[0, 0] = 0
ocp.update_bounds(x_bounds=x_bounds)
constraints = ConstraintList()
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.END,
first_marker_idx=0,
second_marker_idx=2)
ocp.update_constraints(constraints)
sol = ocp.solve(solver=Solver.ACADOS, solver_options={"print_level": 0})
# Check some of the results
q, qdot, tau = sol.states["q"], sol.states["qdot"], sol.controls["tau"]
# final position
np.testing.assert_almost_equal(q[:, -1], np.array((2, 0, 0)), decimal=6)
# initial and final controls
np.testing.assert_almost_equal(tau[:, 0],
np.array((2.72727272, 9.81, 0)),
decimal=6)
np.testing.assert_almost_equal(tau[:, -1],
np.array((-2.72727272, 9.81, 0)),
decimal=6)
def test_custom_constraint_mx_fail():
def custom_mx_fail(pn):
if pn.u is None:
return None
u = pn.nlp.controls
return MX.zeros(u.shape), u.cx, MX.zeros(u.shape)
bioptim_folder = TestUtils.bioptim_folder()
model_path = bioptim_folder + "/examples/getting_started/models/cube.bioMod"
constraints = ConstraintList()
constraints.add(custom_mx_fail, node=Node.ALL)
ocp = OptimalControlProgram(
biorbd.Model(model_path), Dynamics(DynamicsFcn.TORQUE_DRIVEN), 30, 2, constraints=constraints
)
with pytest.raises(RuntimeError, match="Ipopt doesn't support SX/MX types in constraints bounds"):
ocp.solve()
def test_custom_constraint_mx_fail():
def custom_mx_fail(pn):
return MX(0), vertcat(*pn.u), MX(0)
bioptim_folder = TestUtils.bioptim_folder()
model_path = bioptim_folder + "/examples/getting_started/cube.bioMod"
constraints = ConstraintList()
constraints.add(custom_mx_fail, node=Node.ALL)
ocp = OptimalControlProgram(
biorbd.Model(model_path),
Dynamics(DynamicsFcn.TORQUE_DRIVEN),
30,
2,
constraints=constraints,
)
with pytest.raises(RuntimeError, match="Ipopt doesn't support SX/MX types in constraints bounds"):
sol = ocp.solve(show_online_optim=True)
def test_acados_custom_dynamics(problem_type_custom):
# Load pendulum
PROJECT_FOLDER = Path(__file__).parent / ".."
spec = importlib.util.spec_from_file_location(
"custom_problem_type_and_dynamics",
str(PROJECT_FOLDER) + "/examples/getting_started/custom_dynamics.py",
)
pendulum = importlib.util.module_from_spec(spec)
spec.loader.exec_module(pendulum)
ocp = pendulum.prepare_ocp(
biorbd_model_path=str(PROJECT_FOLDER) +
"/examples/getting_started/cube.bioMod",
problem_type_custom=problem_type_custom,
ode_solver=OdeSolver.RK,
use_SX=True,
)
constraints = ConstraintList()
constraints.add(ConstraintFcn.ALIGN_MARKERS,
node=Node.END,
first_marker_idx=0,
second_marker_idx=2)
ocp.update_constraints(constraints)
sol = ocp.solve(solver=Solver.ACADOS)
# Check some of the results
states, controls = Data.get_data(ocp, sol["x"])
q, qdot, tau = states["q"], states["q_dot"], controls["tau"]
# initial and final position
np.testing.assert_almost_equal(q[:, 0], np.array((2, 0, 0)), decimal=6)
np.testing.assert_almost_equal(q[:, -1], np.array((2, 0, 1.57)))
# initial and final velocities
np.testing.assert_almost_equal(qdot[:, 0], np.array((0, 0, 0)))
np.testing.assert_almost_equal(qdot[:, -1], np.array((0, 0, 0)))
# initial and final controls
np.testing.assert_almost_equal(tau[:, 0], np.array((0, 9.81, 2.27903226)))
np.testing.assert_almost_equal(tau[:, -1], np.array(
(0, 9.81, -2.27903226)))
def test_custom_constraint_mx_fail():
def custom_mx_fail(ocp, nlp, t, x, u, p):
return MX(0), vertcat(*u), MX(0)
PROJECT_FOLDER = Path(__file__).parent / ".."
model_path = str(PROJECT_FOLDER) + "/examples/getting_started/cube.bioMod"
constraints = ConstraintList()
constraints.add(custom_mx_fail, node=Node.ALL)
ocp = OptimalControlProgram(
biorbd.Model(model_path),
Dynamics(DynamicsFcn.TORQUE_DRIVEN),
30,
2,
constraints=constraints,
)
with pytest.raises(
RuntimeError,
match="Ipopt doesn't support SX/MX types in constraints bounds"):
sol = ocp.solve(show_online_optim=True)
def test_acados_constraints_end_all():
if platform == "win32":
print("Test for ACADOS on Windows is skipped")
return
bioptim_folder = TestUtils.bioptim_folder()
track = TestUtils.load_module(bioptim_folder +
"/examples/track/track_marker_on_segment.py")
ocp = track.prepare_ocp(
biorbd_model_path=bioptim_folder +
"/examples/track/models/cube_and_line.bioMod",
n_shooting=30,
final_time=2,
initialize_near_solution=True,
constr=False,
use_sx=True,
)
constraints = ConstraintList()
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.END,
first_marker="m0",
second_marker="m5")
constraints.add(ConstraintFcn.TRACK_MARKER_WITH_SEGMENT_AXIS,
node=Node.ALL_SHOOTING,
marker="m1",
segment="seg_rt",
axis=Axis.X)
ocp.update_constraints(constraints)
sol = ocp.solve(solver=Solver.ACADOS)
# Check some of the results
q, qdot, tau = sol.states["q"], sol.states["qdot"], sol.controls["tau"]
# final position
np.testing.assert_almost_equal(q[:, 0],
np.array([2.01701330, 0, 0,
3.20057865e-01]),
decimal=6)
np.testing.assert_almost_equal(q[:, -1],
np.array((2, 0, 1.57, 7.85398168e-01)),
decimal=6)
np.testing.assert_almost_equal(qdot[:, 0],
np.array([0, 0, 0, 0]),
decimal=6)
np.testing.assert_almost_equal(qdot[:, -1],
np.array([0, 0, 0, 0]),
decimal=6)
# initial and final controls
np.testing.assert_almost_equal(tau[:, 0],
np.array((0.04648408, 9.88616194,
2.24285498, 0.864213)),
decimal=6)
np.testing.assert_almost_equal(tau[:, -2],
np.array((0.19389194, 9.99905781,
-2.37713652, -0.19858311)),
decimal=6)
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,
node=Node.START,
first_marker_idx=0,
second_marker_idx=1)
constraints.add(custom_func_align_markers,
node=Node.END,
first_marker_idx=0,
second_marker_idx=2)
# Path constraint
x_bounds = BoundsOption(QAndQDotBounds(biorbd_model))
x_bounds[1:6, [0, -1]] = 0
x_bounds[2, -1] = 1.57
# Initial guess
x_init = InitialGuessOption([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 = InitialGuessOption([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="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
# 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, node=Node.START, first_marker_idx=0, second_marker_idx=1)
constraints.add(Constraint.ALIGN_MARKERS, node=Node.END, first_marker_idx=0, second_marker_idx=2)
constraints.add(Constraint.PROPORTIONAL_STATE, node=Node.ALL, first_dof=2, second_dof=3, coef=-1)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
x_bounds[0][4:8, [0, -1]] = 0
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()))
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([[tau_min] * biorbd_model.nbQ(), [tau_max] * biorbd_model.nbQ()])
u_init = InitialGuessList()
u_init.add([tau_init] * biorbd_model.nbQ())
# ------------- #
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 test_acados_constraints_end_all():
bioptim_folder = TestUtils.bioptim_folder()
track = TestUtils.load_module(bioptim_folder +
"/examples/track/track_marker_on_segment.py")
ocp = track.prepare_ocp(
biorbd_model_path=bioptim_folder +
"/examples/track/cube_and_line.bioMod",
n_shooting=30,
final_time=2,
initialize_near_solution=True,
constr=False,
use_sx=True,
)
constraints = ConstraintList()
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.END,
first_marker_idx=0,
second_marker_idx=5)
constraints.add(ConstraintFcn.TRACK_MARKER_WITH_SEGMENT_AXIS,
node=Node.ALL,
marker_idx=1,
segment_idx=2,
axis=(Axis.X))
ocp.update_constraints(constraints)
sol = ocp.solve(solver=Solver.ACADOS, solver_options={"print_level": 0})
# Check some of the results
q, qdot, tau = sol.states["q"], sol.states["qdot"], sol.controls["tau"]
# final position
np.testing.assert_almost_equal(q[:, 0],
np.array([2, 0, 0, -0.139146705]),
decimal=6)
np.testing.assert_almost_equal(q[:, -1],
np.array((2, 0, 1.57, -0.139146705)),
decimal=6)
np.testing.assert_almost_equal(qdot[:, 0],
np.array([0, 0, 0, 0]),
decimal=6)
np.testing.assert_almost_equal(qdot[:, -1],
np.array([0, 0, 0, 0]),
decimal=6)
# initial and final controls
np.testing.assert_almost_equal(tau[:, 0],
np.array((0, 9.81, 2.27903226, 0)),
decimal=6)
np.testing.assert_almost_equal(tau[:, -1],
np.array((0, 9.81, -2.27903226, 0)),
decimal=6)
def test_lagrange1_neg_multiphase_time_constraint():
with pytest.raises(
RuntimeError,
match="Time constraint/objective cannot declare more than once"):
(
biorbd_model,
n_shooting,
final_time,
time_min,
time_max,
torque_min,
torque_max,
torque_init,
dynamics,
x_bounds,
x_init,
u_bounds,
u_init,
) = partial_ocp_parameters(n_phases=3)
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL,
key="tau",
weight=100,
phase=0)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TIME, phase=0)
constraints = ConstraintList()
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.START,
first_marker="m0",
second_marker="m1",
phase=0)
constraints.add(ConstraintFcn.TIME_CONSTRAINT,
node=Node.END,
minimum=time_min[0],
maximum=time_max[0],
phase=0)
OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
)
def prepare_nmpc(model_path, cycle_len, cycle_duration, n_cycles_simultaneous, n_cycles_to_advance, max_torque):
model = biorbd.Model(model_path)
dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN)
x_bound = QAndQDotBounds(model)
x_bound.min[0, :] = -2 * np.pi * n_cycles_simultaneous # Allow the wheel to spin as much as needed
x_bound.max[0, :] = 0
u_bound = Bounds([-max_torque] * model.nbQ(), [max_torque] * model.nbQ())
x_init = InitialGuess(
np.zeros(
model.nbQ() * 2,
)
)
u_init = InitialGuess(
np.zeros(
model.nbQ(),
)
)
new_objectives = Objective(ObjectiveFcn.Lagrange.MINIMIZE_STATE, key="q")
# Rotate the wheel and force the marker of the hand to follow the marker on the wheel
wheel_target = np.linspace(-2 * np.pi * n_cycles_simultaneous, 0, cycle_len * n_cycles_simultaneous + 1)[
np.newaxis, :
]
constraints = ConstraintList()
constraints.add(ConstraintFcn.TRACK_STATE, key="q", index=0, node=Node.ALL, target=wheel_target)
constraints.add(
ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.ALL,
first_marker="wheel",
second_marker="COM_hand",
axes=[Axis.X, Axis.Y],
)
return MyCyclicNMPC(
model,
dynamics,
cycle_len=cycle_len,
cycle_duration=cycle_duration,
n_cycles_simultaneous=n_cycles_simultaneous,
n_cycles_to_advance=n_cycles_to_advance,
objective_functions=new_objectives,
constraints=constraints,
x_init=x_init,
u_init=u_init,
x_bounds=x_bound,
u_bounds=u_bound,
)
def test_lagrange2_neg_multiphase_time_constraint():
with pytest.raises(
RuntimeError,
match="Time constraint/objective cannot declare more than once"):
(
biorbd_model,
number_shooting_points,
final_time,
time_min,
time_max,
torque_min,
torque_max,
torque_init,
dynamics,
x_bounds,
x_init,
u_bounds,
u_init,
) = partial_ocp_parameters(nb_phases=3)
objective_functions = ObjectiveList()
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE,
weight=100,
phase=2)
objective_functions.add(Objective.Lagrange.MINIMIZE_TIME, phase=2)
constraints = ConstraintList()
constraints.add(Constraint.ALIGN_MARKERS,
node=Node.START,
first_marker_idx=0,
second_marker_idx=1,
phase=2)
constraints.add(Constraint.TIME_CONSTRAINT,
node=Node.END,
minimum=time_min[0],
maximum=time_max[0],
phase=2)
OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
)
def test_mayer_multiphase_time_constraint():
(
biorbd_model,
n_shooting,
final_time,
time_min,
time_max,
torque_min,
torque_max,
torque_init,
dynamics,
x_bounds,
x_init,
u_bounds,
u_init,
) = partial_ocp_parameters(n_phases=3)
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE,
weight=100,
phase=0)
objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_TIME, phase=0)
constraints = ConstraintList()
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.START,
first_marker_idx=0,
second_marker_idx=1,
phase=2)
constraints.add(ConstraintFcn.TIME_CONSTRAINT,
node=Node.END,
minimum=time_min[0],
maximum=time_max[0],
phase=2)
OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
)
def test_mayer_neg_monophase_time_constraint():
(
biorbd_model,
n_shooting,
final_time,
time_min,
time_max,
torque_min,
torque_max,
torque_init,
dynamics,
x_bounds,
x_init,
u_bounds,
u_init,
) = partial_ocp_parameters(n_phases=1)
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_TIME)
constraints = ConstraintList()
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.START,
first_marker_idx=0,
second_marker_idx=1)
constraints.add(ConstraintFcn.TIME_CONSTRAINT,
node=Node.END,
minimum=time_min[0],
maximum=time_max[0])
with pytest.raises(
RuntimeError,
match="Time constraint/objective cannot declare more than once"):
OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
)
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,
n_shooting: int,
final_time: float,
loop_from_constraint: bool,
ode_solver: OdeSolver = OdeSolver.RK4(),
) -> OptimalControlProgram:
"""
Prepare the program
Parameters
----------
biorbd_model_path: str
The path of the biorbd model
final_time: float
The time at the final node
n_shooting: int
The number of shooting points
loop_from_constraint: bool
If the looping cost should be a constraint [True] or an objective [False]
ode_solver: OdeSolver
The type of ode solver used
Returns
-------
The ocp ready to be solved
"""
biorbd_model = biorbd.Model(biorbd_model_path)
# Add objective functions
objective_functions = Objective(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE,
weight=100)
# Dynamics
dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN)
# Constraints
constraints = ConstraintList()
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.MID,
first_marker_idx=0,
second_marker_idx=2)
constraints.add(ConstraintFcn.TRACK_STATE, node=Node.MID, index=2)
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.END,
first_marker_idx=0,
second_marker_idx=1)
# Path constraint
x_bounds = QAndQDotBounds(biorbd_model)
# First node is free but mid and last are constrained to be exactly at a certain point.
# The cyclic penalty ensures that the first node and the last node are the same.
x_bounds[2:6, -1] = [1.57, 0, 0, 0]
# Initial guess
x_init = InitialGuess([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()))
# Define control path constraint
tau_min, tau_max, tau_init = -100, 100, 0
u_bounds = Bounds([tau_min] * biorbd_model.nbGeneralizedTorque(),
[tau_max] * biorbd_model.nbGeneralizedTorque())
u_init = InitialGuess([tau_init] * biorbd_model.nbGeneralizedTorque())
# ------------- #
# A phase 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
phase_transitions = PhaseTransitionList()
if loop_from_constraint:
phase_transitions.add(PhaseTransitionFcn.CYCLIC, weight=0)
else:
phase_transitions.add(PhaseTransitionFcn.CYCLIC, weight=10000)
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 prepare_ocp(phase_time_constraint, use_parameter):
# --- Inputs --- #
final_time = (2, 5, 4)
time_min = [1, 3, 0.1]
time_max = [2, 4, 0.8]
ns = (20, 30, 20)
biorbd_model_path = TestUtils.bioptim_folder(
) + "/examples/optimal_time_ocp/cube.bioMod"
ode_solver = OdeSolver.RK4()
# --- Options --- #
n_phases = len(ns)
# Model path
biorbd_model = (biorbd.Model(biorbd_model_path),
biorbd.Model(biorbd_model_path),
biorbd.Model(biorbd_model_path))
# Problem parameters
tau_min, tau_max, tau_init = -100, 100, 0
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE,
weight=100,
phase=0)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE,
weight=100,
phase=1)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE,
weight=100,
phase=2)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, phase=0)
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, phase=1)
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, phase=2)
# 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.TIME_CONSTRAINT,
node=Node.END,
minimum=time_min[0],
maximum=time_max[0],
phase=phase_time_constraint,
)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) # Phase 0
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) # Phase 1
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) # Phase 2
for bounds in x_bounds:
for i in [1, 3, 4, 5]:
bounds[i, [0, -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()))
# 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_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())
parameters = ParameterList()
if use_parameter:
def my_target_function(ocp, value, target_value):
return value - target_value
def my_parameter_function(biorbd_model, value, extra_value):
new_gravity = MX.zeros(3, 1)
new_gravity[2] = value + extra_value
biorbd_model.setGravity(new_gravity)
min_g = -10
max_g = -6
target_g = -8
bound_gravity = Bounds(min_g,
max_g,
interpolation=InterpolationType.CONSTANT)
initial_gravity = InitialGuess((min_g + max_g) / 2)
parameter_objective_functions = Objective(
my_target_function,
weight=10,
quadratic=True,
custom_type=ObjectiveFcn.Parameter,
target_value=target_g)
parameters.add(
"gravity_z",
my_parameter_function,
initial_gravity,
bound_gravity,
size=1,
penalty_list=parameter_objective_functions,
extra_value=1,
)
# ------------- #
return OptimalControlProgram(
biorbd_model[:n_phases],
dynamics,
ns,
final_time[:n_phases],
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
parameters=parameters,
)
def prepare_ocp(biorbd_model_path,
number_shooting_points,
final_time,
use_actuators=False,
ode_solver=OdeSolver.RK):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
# Problem parameters
if use_actuators:
tau_min, tau_max, tau_init = -1, 1, 0
else:
tau_min, tau_max, tau_init = -100, 100, 0
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=100)
# Dynamics
dynamics = DynamicsTypeList()
if use_actuators:
dynamics.add(DynamicsType.TORQUE_ACTIVATIONS_DRIVEN)
else:
dynamics.add(DynamicsType.TORQUE_DRIVEN)
# Constraints
constraints = ConstraintList()
constraints.add(Constraint.ALIGN_MARKERS,
node=Node.START,
first_marker_idx=0,
second_marker_idx=1)
constraints.add(Constraint.ALIGN_MARKERS,
node=Node.END,
first_marker_idx=0,
second_marker_idx=2)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
x_bounds[0][3:6, [0, -1]] = 0
x_bounds[0][2, [0, -1]] = [0, 1.57]
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()))
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([[tau_min] * biorbd_model.nbGeneralizedTorque(),
[tau_max] * biorbd_model.nbGeneralizedTorque()])
u_init = InitialGuessList()
u_init.add([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: 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
dof_mapping = BiMappingList()
dof_mapping.add("tau", [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, node=Node.ALL, axes=Axis.Z, weight=-1)
elif objective_name == "MINIMIZE_COM_VELOCITY":
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_COM_VELOCITY, node=Node.ALL, axes=Axis.Z, weight=-1)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=1 / 100)
# Dynamics
dynamics = DynamicsList()
if use_actuators:
dynamics.add(DynamicsFcn.TORQUE_ACTIVATIONS_DRIVEN, with_contact=True)
else:
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, with_contact=True)
# 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] * len(dof_mapping["tau"].to_first), [tau_max] * len(dof_mapping["tau"].to_first))
u_init = InitialGuessList()
u_init.add([tau_init] * len(dof_mapping["tau"].to_first))
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
phase_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints=constraints,
variable_mappings=dof_mapping,
ode_solver=ode_solver,
)
def prepare_ocp(
biorbd_model_path,
final_time,
number_shooting_points,
initialize_near_solution,
ode_solver=OdeSolver.RK,
constr=True,
use_SX=False,
):
# --- 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 = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
# Constraints
if constr is True:
constraints = ConstraintList()
constraints.add(ConstraintFcn.ALIGN_MARKERS, node=Node.START, first_marker_idx=0, second_marker_idx=4)
constraints.add(ConstraintFcn.ALIGN_MARKERS, node=Node.END, first_marker_idx=0, second_marker_idx=5)
constraints.add(
ConstraintFcn.ALIGN_MARKER_WITH_SEGMENT_AXIS, node=Node.ALL, marker_idx=1, segment_idx=2, axis=(Axe.X)
)
else:
constraints = ConstraintList()
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
for i in range(1, 8):
if i != 3:
x_bounds[0][i, [0, -1]] = 0
x_bounds[0][2, -1] = 1.57
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()))
if initialize_near_solution:
for i in range(2):
x_init[0].init[i] = 1.5
for i in range(4, 6):
x_init[0].init[i] = 0.7
for i in range(6, 8):
x_init[0].init[i] = 0.6
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([tau_min] * biorbd_model.nbGeneralizedTorque(), [tau_max] * biorbd_model.nbGeneralizedTorque())
u_init = InitialGuessList()
u_init.add([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,
use_SX=use_SX,
)
def prepare_ocp_phase_transitions(
biorbd_model_path: str,
with_constraints: bool,
with_mayer: bool,
with_lagrange: bool,
) -> OptimalControlProgram:
"""
Parameters
----------
biorbd_model_path: str
The path to the bioMod
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()
if with_lagrange:
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE,
weight=100,
phase=0)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE,
weight=100,
phase=1)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE,
weight=100,
phase=2)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE,
weight=100,
phase=3)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_COM_VELOCITY,
weight=0,
phase=3,
axis=None)
if with_mayer:
objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_TIME)
objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_COM_POSITION,
phase=0,
node=1)
objective_functions.add(
minimize_difference,
custom_type=ObjectiveFcn.Mayer,
node=Node.TRANSITION,
weight=100,
phase=1,
get_all_nodes_at_once=True,
quadratic=True,
)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
# Constraints
constraints = ConstraintList()
if with_constraints:
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.START,
first_marker="m0",
second_marker="m1",
phase=0)
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=2,
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(custom_func_track_markers,
node=Node.ALL,
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())
# Define phase transitions
phase_transitions = PhaseTransitionList()
phase_transitions.add(PhaseTransitionFcn.IMPACT, phase_pre_idx=1)
phase_transitions.add(PhaseTransitionFcn.CONTINUOUS,
phase_pre_idx=2,
idx_1=1,
idx_2=3)
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,
phase_transitions=phase_transitions,
)
