def generate_table(out):
model_path = "/".join(__file__.split("/")[:-1]) + "/models/arm26.bioMod"
biorbd_model_ip = biorbd.Model(model_path)
# IPOPT
use_ipopt = True
weights = np.array([100, 1, 1, 100000])
ocp = prepare_ocp(biorbd_model=biorbd_model_ip,
final_time=2,
n_shooting=50,
use_sx=not use_ipopt,
weights=weights)
opts = {"linear_solver": "ma57", "hessian_approximation": "exact"}
solver = Solver.IPOPT
# --- Solve the program --- #
tic = time()
sol = ocp.solve(
solver=solver,
solver_options=opts,
)
toc = time() - tic
sol_merged = sol.merge_phases()
out.nx = sol_merged.states["all"].shape[0]
out.nu = sol_merged.controls["all"].shape[0]
out.ns = sol_merged.ns[0]
out.solver.append(out.Solver("Ipopt"))
out.solver[0].n_iteration = sol.iterations
out.solver[0].cost = sol.cost
out.solver[0].convergence_time = toc
out.solver[0].compute_error_single_shooting(sol, 1)
# ACADOS
use_ipopt = False
biorbd_model_ac = biorbd.Model(model_path)
ocp = prepare_ocp(biorbd_model=biorbd_model_ac,
final_time=2,
n_shooting=50,
use_sx=not use_ipopt,
weights=weights)
opts = {
"sim_method_num_steps": 5,
"tol": 1e-8,
"integrator_type": "ERK",
"hessian_approx": "GAUSS_NEWTON"
}
solver = Solver.ACADOS
# --- Solve the program --- #
sol = ocp.solve(
solver=solver,
solver_options=opts,
)
out.solver.append(out.Solver("Acados"))
out.solver[1].n_iteration = sol.iterations
out.solver[1].cost = sol.cost
out.solver[1].convergence_time = sol.time_to_optimize
out.solver[1].compute_error_single_shooting(sol, 1)
def test_plot_merged_graphs():
# Load graphs_one_phase
bioptim_folder = TestUtils.bioptim_folder()
merged_graphs = TestUtils.load_module(
bioptim_folder +
"/examples/muscle_driven_ocp/muscle_excitations_tracker.py")
# Define the problem
model_path = bioptim_folder + "/examples/muscle_driven_ocp/arm26.bioMod"
biorbd_model = biorbd.Model(model_path)
final_time = 0.5
n_shooting = 9
# Generate random data to fit
np.random.seed(42)
t, markers_ref, x_ref, muscle_excitations_ref = merged_graphs.generate_data(
biorbd_model, final_time, n_shooting)
biorbd_model = biorbd.Model(
model_path
) # To prevent from non free variable, the model must be reloaded
ocp = merged_graphs.prepare_ocp(
biorbd_model,
final_time,
n_shooting,
markers_ref,
muscle_excitations_ref,
x_ref[:biorbd_model.nbQ(), :].T,
use_residual_torque=True,
kin_data_to_track="markers",
)
sol = ocp.solve()
sol.graphs(automatically_organize=False)
def test_plot_merged_graphs():
# Load graphs_one_phase
PROJECT_FOLDER = Path(__file__).parent / ".."
spec = importlib.util.spec_from_file_location(
"align_markers", str(PROJECT_FOLDER) + "/examples/muscle_driven_ocp/muscle_excitations_tracker.py"
)
merged_graphs = importlib.util.module_from_spec(spec)
spec.loader.exec_module(merged_graphs)
# Define the problem
model_path = str(PROJECT_FOLDER) + "/examples/muscle_driven_ocp/arm26.bioMod"
biorbd_model = biorbd.Model(model_path)
final_time = 0.5
nb_shooting = 9
# Generate random data to fit
np.random.seed(42)
t, markers_ref, x_ref, muscle_excitations_ref = merged_graphs.generate_data(biorbd_model, final_time, nb_shooting)
biorbd_model = biorbd.Model(model_path) # To prevent from non free variable, the model must be reloaded
ocp = merged_graphs.prepare_ocp(
biorbd_model,
final_time,
nb_shooting,
markers_ref,
muscle_excitations_ref,
x_ref[: biorbd_model.nbQ(), :].T,
use_residual_torque=True,
kin_data_to_track="markers",
)
sol = ocp.solve()
plt = ShowResult(ocp, sol)
plt.graphs(automatically_organize=False)
def test_plot_merged_graphs():
# Define the problem
model_path = str(
PROJECT_FOLDER) + "/examples/muscle_driven_ocp/arm26.bioMod"
biorbd_model = biorbd.Model(model_path)
final_time = 0.5
nb_shooting = 9
# Generate random data to fit
np.random.seed(42)
t, markers_ref, x_ref, muscle_excitations_ref = merged_graphs.generate_data(
biorbd_model, final_time, nb_shooting)
biorbd_model = biorbd.Model(
model_path
) # To allow for non free variable, the model must be reloaded
ocp = merged_graphs.prepare_ocp(
biorbd_model,
final_time,
nb_shooting,
markers_ref,
muscle_excitations_ref,
x_ref[:biorbd_model.nbQ(), :].T,
kin_data_to_track="markers",
)
sol = ocp.solve()
plt = ShowResult(ocp, sol)
plt.graphs(automatically_organize=False)
def generate_table(out):
root_path = "/".join(__file__.split("/")[:-1])
# Define the problem -- model path
biorbd_model = (
biorbd.Model(root_path + "/models/Gait_1leg_12dof_heel.bioMod"),
biorbd.Model(root_path + "/models/Gait_1leg_12dof_flatfoot.bioMod"),
biorbd.Model(root_path + "/models/Gait_1leg_12dof_forefoot.bioMod"),
biorbd.Model(root_path + "/models/Gait_1leg_12dof_0contact.bioMod"),
)
# --- files path ---
c3d_file = root_path + "/data/normal01_out.c3d"
q_kalman_filter_file = root_path + "/data/normal01_q_KalmanFilter.txt"
qdot_kalman_filter_file = root_path + "/data/normal01_qdot_KalmanFilter.txt"
data = LoadData(biorbd_model[0], c3d_file, q_kalman_filter_file, qdot_kalman_filter_file)
# --- phase time and number of shooting ---
phase_time, number_shooting_points = get_phase_time_shooting_numbers(data, 0.01)
# --- get experimental data ---
q_ref, qdot_ref, markers_ref, grf_ref, moments_ref, cop_ref = get_experimental_data(data, number_shooting_points)
ocp = prepare_ocp(
biorbd_model=biorbd_model,
final_time=phase_time,
nb_shooting=number_shooting_points,
markers_ref=markers_ref,
grf_ref=grf_ref,
q_ref=q_ref,
qdot_ref=qdot_ref,
M_ref=moments_ref,
CoP=cop_ref,
nb_threads=8,
)
# --- Solve the program --- #
tic = time()
sol = ocp.solve(
solver=Solver.IPOPT,
solver_options={
"tol": 1e-3,
"max_iter": 1000,
"hessian_approximation": "exact",
"limited_memory_max_history": 50,
"linear_solver": "ma57",
},
)
toc = time() - tic
sol_merged = sol.merge_phases()
out.nx = sol_merged.states["all"].shape[0]
out.nu = sol_merged.controls["all"].shape[0]
out.ns = sol_merged.ns[0]
out.solver.append(out.Solver("Ipopt"))
out.solver[0].n_iteration = sol.iterations
out.solver[0].cost = sol.cost
out.solver[0].convergence_time = toc
out.solver[0].compute_error_single_shooting(sol, 1, use_final_time=True)
def test_muscle_activations_and_states_tracking():
# Define the problem
model_path = str(PROJECT_FOLDER) + "/examples/muscle_driven_ocp/arm26.bioMod"
biorbd_model = biorbd.Model(model_path)
final_time = 2
nb_shooting = 9
# Generate random data to fit
np.random.seed(42)
t, markers_ref, x_ref, muscle_activations_ref = muscle_activations_tracker.generate_data(
biorbd_model, final_time, nb_shooting
)
biorbd_model = biorbd.Model(model_path) # To allow for non free variable, the model must be reloaded
ocp = muscle_activations_tracker.prepare_ocp(
biorbd_model,
final_time,
nb_shooting,
markers_ref,
muscle_activations_ref,
x_ref[: biorbd_model.nbQ(), :].T,
kin_data_to_track="q",
)
sol = ocp.solve()
# Check objective function value
f = np.array(sol["f"])
np.testing.assert_equal(f.shape, (1, 1))
np.testing.assert_almost_equal(f[0, 0], 1.4506639252752042e-06)
# Check constraints
g = np.array(sol["g"])
np.testing.assert_equal(g.shape, (36, 1))
np.testing.assert_almost_equal(g, np.zeros((36, 1)), decimal=6)
# Check some of the results
states, controls = Data.get_data(ocp, sol["x"])
q, qdot, tau, mus = states["q"], states["q_dot"], controls["tau"], controls["muscles"]
# initial and final position
np.testing.assert_almost_equal(q[:, 0], np.array([-1.13043502e-05, -1.35629661e-05]))
np.testing.assert_almost_equal(q[:, -1], np.array([-0.49387966, -1.44924784]))
# initial and final velocities
np.testing.assert_almost_equal(qdot[:, 0], np.array([-8.66527631e-05, -1.31486656e-04]))
np.testing.assert_almost_equal(qdot[:, -1], np.array([0.8780829, -2.6474387]))
# initial and final controls
np.testing.assert_almost_equal(tau[:, 0], np.array([-1.55359644e-06, 1.26569700e-05]))
np.testing.assert_almost_equal(tau[:, -1], np.array([-7.41845169e-06, -7.67568954e-07]))
np.testing.assert_almost_equal(
mus[:, 0], np.array([0.37439688, 0.95073361, 0.73203047, 0.59855246, 0.15592687, 0.15595739]),
)
np.testing.assert_almost_equal(
mus[:, -1], np.array([0.54685367, 0.18482085, 0.96945157, 0.77512036, 0.93947405, 0.89483397]),
)
def test_mhe(solver):
if platform == "win32" and solver == Solver.ACADOS:
print("Test for ACADOS on Windows is skipped")
return
root_folder = TestUtils.bioptim_folder() + "/examples/moving_horizon_estimation/"
pendulum = TestUtils.load_module(root_folder + "mhe.py")
biorbd_model = biorbd.Model(root_folder + "cart_pendulum.bioMod")
nq = biorbd_model.nbQ()
torque_max = 5 # Give a bit of slack on the max torque
n_cycles = 5 if solver == Solver.ACADOS else 1
n_frame_by_cycle = 20
window_len = 5
window_duration = 0.2
final_time = window_duration / window_len * n_cycles * n_frame_by_cycle
x_init = np.zeros((nq * 2, window_len + 1))
u_init = np.zeros((nq, window_len))
target_q, _, _, _ = pendulum.generate_data(
biorbd_model, final_time, [0, np.pi / 2, 0, 0], torque_max, n_cycles * n_frame_by_cycle, 0
)
target = pendulum.states_to_markers(biorbd_model, target_q)
def update_functions(mhe, t, _):
def target_func(i: int):
return target[:, :, i : i + window_len + 1]
mhe.update_objectives_target(target=target_func(t), list_index=0)
return t < n_frame_by_cycle * n_cycles - window_len - 1
biorbd_model = biorbd.Model(root_folder + "cart_pendulum.bioMod")
sol = pendulum.prepare_mhe(
biorbd_model=biorbd_model,
window_len=window_len,
window_duration=window_duration,
max_torque=torque_max,
x_init=x_init,
u_init=u_init,
).solve(update_functions, **pendulum.get_solver_options(solver))
# Clean test folder
if solver == Solver.ACADOS:
# Compare the position on the first few frames (only ACADOS, since IPOPT is not precise with current options)
np.testing.assert_almost_equal(
sol.states["q"][:, : -2 * window_len], target_q[:nq, : -3 * window_len - 1], decimal=3
)
os.remove(f"./acados_ocp.json")
shutil.rmtree(f"./c_generated_code/")
def prepare_test_ocp(with_muscles=False,
with_contact=False,
with_actuator=False):
PROJECT_FOLDER = Path(__file__).parent / ".."
if with_muscles and with_contact or with_muscles and with_actuator or with_contact and with_actuator:
raise RuntimeError(
"With muscles and with contact and with_actuator together is not defined"
)
elif with_muscles:
biorbd_model = biorbd.Model(
str(PROJECT_FOLDER) + "/examples/muscle_driven_ocp/arm26.bioMod")
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN)
nx = biorbd_model.nbQ() + biorbd_model.nbQdot()
nu = biorbd_model.nbGeneralizedTorque() + biorbd_model.nbMuscles()
elif with_contact:
biorbd_model = biorbd.Model(
str(PROJECT_FOLDER) +
"/examples/muscle_driven_with_contact/2segments_4dof_2contacts_1muscle.bioMod"
)
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN_WITH_CONTACT)
nx = biorbd_model.nbQ() + biorbd_model.nbQdot()
nu = biorbd_model.nbGeneralizedTorque()
elif with_actuator:
biorbd_model = biorbd.Model(
str(PROJECT_FOLDER) + "/examples/torque_driven_ocp/cube.bioMod")
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
nx = biorbd_model.nbQ() + biorbd_model.nbQdot()
nu = biorbd_model.nbGeneralizedTorque()
else:
biorbd_model = biorbd.Model(
str(PROJECT_FOLDER) + "/examples/align/cube_and_line.bioMod")
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
nx = biorbd_model.nbQ() + biorbd_model.nbQdot()
nu = biorbd_model.nbGeneralizedTorque()
x_init = InitialGuess(np.zeros((nx, 1)))
u_init = InitialGuess(np.zeros((nu, 1)))
x_bounds = Bounds(np.zeros((nx, 1)), np.zeros((nx, 1)))
u_bounds = Bounds(np.zeros((nu, 1)), np.zeros((nu, 1)))
ocp = OptimalControlProgram(biorbd_model, dynamics, 10, 1.0, x_init,
u_init, x_bounds, u_bounds)
ocp.nlp[0].J = [list()]
ocp.nlp[0].g = [list()]
ocp.nlp[0].g_bounds = [list()]
return ocp
def test_acados_one_mayer(cost_type):
PROJECT_FOLDER = Path(__file__).parent / ".."
spec = importlib.util.spec_from_file_location(
"cube",
str(PROJECT_FOLDER) + "/examples/acados/cube.py",
)
cube = importlib.util.module_from_spec(spec)
spec.loader.exec_module(cube)
ocp = cube.prepare_ocp(
biorbd_model_path=str(PROJECT_FOLDER) + "/examples/acados/cube.bioMod",
nbs=10,
tf=2,
)
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_STATE,
index=[0],
target=np.array([[1.0]]).T)
ocp.update_objectives(objective_functions)
sol = ocp.solve(solver=Solver.ACADOS,
solver_options={"cost_type": cost_type})
# Check end state value
model = biorbd.Model(str(PROJECT_FOLDER) + "/examples/acados/cube.bioMod")
q = np.array(sol["qqdot"])[:model.nbQ()]
np.testing.assert_almost_equal(q[0, -1], 1.0)
# Clean test folder
os.remove(f"./acados_ocp.json")
shutil.rmtree(f"./c_generated_code/")
def test_acados_one_lagrange(cost_type):
PROJECT_FOLDER = Path(__file__).parent / ".."
spec = importlib.util.spec_from_file_location(
"cube",
str(PROJECT_FOLDER) + "/examples/acados/cube.py",
)
cube = importlib.util.module_from_spec(spec)
spec.loader.exec_module(cube)
nbs = 10
target = np.expand_dims(np.arange(0, nbs + 1), axis=0)
target[0, -1] = nbs - 2
ocp = cube.prepare_ocp(
biorbd_model_path=str(PROJECT_FOLDER) + "/examples/acados/cube.bioMod",
nbs=nbs,
tf=2,
)
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.TRACK_STATE,
weight=10,
index=[0],
target=target)
ocp.update_objectives(objective_functions)
sol = ocp.solve(solver=Solver.ACADOS,
solver_options={"cost_type": cost_type})
# Check end state value
model = biorbd.Model(str(PROJECT_FOLDER) + "/examples/acados/cube.bioMod")
q = np.array(sol["qqdot"])[:model.nbQ()]
np.testing.assert_almost_equal(q[0, :], target[0, :].squeeze())
# Clean test folder
os.remove(f"./acados_ocp.json")
shutil.rmtree(f"./c_generated_code/")
def prepare_ocp(model_path, phase_time, number_shooting_points,
muscle_activations_ref, contact_forces_ref):
# Model path
biorbd_model = biorbd.Model(model_path)
tau_min, tau_max, tau_init = -500, 500, 0
activation_min, activation_max, activation_init = 0, 1, 0.5
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Lagrange.TRACK_MUSCLES_CONTROL,
target=muscle_activations_ref)
objective_functions.add(Objective.Lagrange.TRACK_CONTACT_FORCES,
target=contact_forces_ref)
# Dynamics
dynamics = DynamicsTypeList()
dynamics.add(
DynamicsType.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN_WITH_CONTACT)
# 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] * biorbd_model.nbGeneralizedTorque() +
[activation_min] * biorbd_model.nbMuscleTotal(),
[tau_max] * biorbd_model.nbGeneralizedTorque() +
[activation_max] * biorbd_model.nbMuscleTotal(),
])
u_init = InitialConditionsList()
u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque() +
[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,
)
def impact(ocp, transition):
"""
TODO
"""
if ocp.nlp[transition.phase_pre_idx]["nx"] != ocp.nlp[
(transition.phase_pre_idx + 1) % ocp.nb_phases]["nx"]:
raise RuntimeError(
"Impact transition without same nx is not possible, please provide a custom state transition"
)
# Aliases
nlp_pre, nlp_post = StateTransitionFunctions.Functions.__get_nlp_pre_and_post(
ocp, transition.phase_pre_idx)
nbQ = nlp_pre["nbQ"]
nbQdot = nlp_pre["nbQdot"]
q = nlp_pre["q_mapping"].expand.map(nlp_pre["X"][-1][:nbQ])
qdot_pre = nlp_pre["q_dot_mapping"].expand.map(
nlp_pre["X"][-1][nbQ:nbQ + nbQdot])
if nlp_post["model"].nbContacts() == 0:
warn("The chosen model does not have any contact")
# A new model is loaded here so we can use pre Qdot with post model, this is a hack and should be dealt
# a better way (e.g. create a supplementary variable in V that link the pre and post phase with a
# constraint. The transition would therefore apply to node_0 and node_1 (with an augmented ns)
model = biorbd.Model(
nlp_post["model"].path().absolutePath().to_string())
func = biorbd.to_casadi_func("impulse_direct",
model.ComputeConstraintImpulsesDirect,
nlp_pre["q"], nlp_pre["qdot"])
qdot_post = func(q, qdot_pre)
qdot_post = nlp_post["q_dot_mapping"].reduce.map(qdot_post)
val = nlp_pre["X"][-1][:nbQ] - nlp_post["X"][0][:nbQ]
val = vertcat(val, qdot_post - nlp_post["X"][0][nbQ:nbQ + nbQdot])
return val
def test_mhe_redim_xbounds_and_init():
root_folder = TestUtils.bioptim_folder() + "/examples/moving_horizon_estimation/"
biorbd_model = biorbd.Model(root_folder + "cart_pendulum.bioMod")
nq = biorbd_model.nbQ()
ntau = biorbd_model.nbGeneralizedTorque()
n_cycles = 3
window_len = 5
window_duration = 0.2
x_init = InitialGuess(np.zeros((nq * 2, 1)), interpolation=InterpolationType.CONSTANT)
u_init = InitialGuess(np.zeros((ntau, 1)), interpolation=InterpolationType.CONSTANT)
x_bounds = Bounds(np.zeros((nq * 2, 1)), np.zeros((nq * 2, 1)), interpolation=InterpolationType.CONSTANT)
u_bounds = Bounds(np.zeros((ntau, 1)), np.zeros((ntau, 1)))
mhe = MovingHorizonEstimator(
biorbd_model,
Dynamics(DynamicsFcn.TORQUE_DRIVEN),
window_len,
window_duration,
x_init=x_init,
u_init=u_init,
x_bounds=x_bounds,
u_bounds=u_bounds,
n_threads=4,
)
def update_functions(mhe, t, _):
return t < n_cycles
mhe.solve(update_functions, Solver.IPOPT)
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_CoM():
m = biorbd.Model("../../models/pyomecaman.bioMod")
q = np.array(
[0.1, 0.1, 0.1, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3])
q_dot = np.array([1, 1, 1, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3])
q_ddot = np.array([10, 10, 10, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30])
expected_CoM = np.array(
[-0.0034679564024098523, 0.15680579877453169, 0.07808112642459612])
expected_CoM_dot = np.array(
[-0.05018973433722229, 1.4166208451420528, 1.4301750486035787])
expected_CoM_ddot = np.array(
[-0.7606169667295027, 11.508107073695976, 16.58853835505851])
if biorbd.currentLinearAlgebraBackend() == 1:
# If CasADi backend is used
from casadi import Function, MX
q_sym = MX.sym("q", m.nbQ(), 1)
q_dot_sym = MX.sym("q_dot", m.nbQdot(), 1)
q_ddot_sym = MX.sym("q_ddot", m.nbQddot(), 1)
CoM_func = Function(
"Compute_CoM",
[q_sym],
[m.CoM(q_sym).to_mx()],
["q"],
["CoM"],
).expand()
CoM_dot_func = Function(
"Compute_CoM_dot",
[q_sym, q_dot_sym],
[m.CoMdot(q_sym, q_dot_sym).to_mx()],
["q", "q_dot"],
["CoM_dot"],
).expand()
CoM_ddot_func = Function(
"Compute_CoM_ddot",
[q_sym, q_dot_sym, q_ddot_sym],
[m.CoMddot(q_sym, q_dot_sym, q_ddot_sym).to_mx()],
["q", "q_dot", "q_ddot"],
["CoM_ddot"],
).expand()
CoM = np.array(CoM_func(q))
CoM_dot = np.array(CoM_dot_func(q, q_dot))
CoM_ddot = np.array(CoM_ddot_func(q, q_dot, q_ddot))
elif not biorbd.currentLinearAlgebraBackend():
# If Eigen backend is used
CoM = m.CoM(q).to_array()
CoM_dot = m.CoMdot(q, q_dot).to_array()
CoM_ddot = m.CoMddot(q, q_dot, q_ddot).to_array()
np.testing.assert_almost_equal(CoM.squeeze(), expected_CoM)
np.testing.assert_almost_equal(CoM_dot.squeeze(), expected_CoM_dot)
np.testing.assert_almost_equal(CoM_ddot.squeeze(), expected_CoM_ddot)
def test_pendulum_max_time_lagrange_constrained(ode_solver):
# Load pendulum_min_time_Lagrange
biorbd_model_path = (TestUtils.bioptim_folder() +
"/examples/optimal_time_ocp/pendulum.bioMod", )
# --- Options --- #
biorbd_model = biorbd.Model(biorbd_model_path[0])
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TIME,
weigth=-1,
max_bound=1)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
# ------------- #
with pytest.raises(
RuntimeError,
match=
"ObjectiveFcn.Lagrange.MINIMIZE_TIME cannot have max_bound. Please either use MAYER or constraint",
):
OptimalControlProgram(biorbd_model,
dynamics,
10,
2,
objective_functions=objective_functions)
def muscles_forces(states, controls, nlp, force=None, len=None, tsl=None, pa=None, insx=None, insy=None, insz=None):
nq = nlp["q_mapping"].reduce.len
q = nlp["q_mapping"].expand.map(states[:nq])
qdot = nlp["q_dot_mapping"].expand.map(states[nq:])
biorbd_model = biorbd.Model("arm_Belaise.bioMod")
activations = states[nlp["nbQ"] + nlp["nbQdot"]:]
muscles_states = biorbd.VecBiorbdMuscleState(nlp["nbMuscle"])
for k in range(nlp["nbMuscle"]):
muscles_states[k].setActivation(activations[k])
muscles_states[k].setExcitation(controls[k])
if force is not None:
biorbd_model.muscle(k).characteristics().setForceIsoMax(force[k])
if len is not None:
biorbd_model.muscle(k).characteristics().setOptimalLength(len[k])
if tsl is not None:
biorbd_model.muscle(k).characteristics().setTendonSlackLength(tsl[k])
if pa is not None:
biorbd_model.muscle(k).characteristics().setPennationAngle(pa[k])
if insx is not None:
biorbd_model.muscle(k).position().setInsertionInLocal(biorbd.Vector3d(insx[k], insy[k], insz[k]))
return biorbd_model.muscleForces(muscles_states, q, qdot).to_mx()
def prepare_ocp(biorbd_model_path,
n_shooting,
tf,
ode_solver=OdeSolver.RK4(),
use_sx=True):
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
# Dynamics
dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN)
# Path constraint
x_bounds = QAndQDotBounds(biorbd_model)
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())
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
tf,
x_init,
u_init,
x_bounds,
u_bounds,
ode_solver=ode_solver,
use_sx=use_sx,
)
def test_set_scalar():
def check_value(target):
if biorbd.currentLinearAlgebraBackend() == 1:
assert m.segment(0).characteristics().mass().to_mx() == target
else:
assert m.segment(0).characteristics().mass() == target
m = biorbd.Model("../../models/pyomecaman.bioMod")
m.segment(0).characteristics().setMass(10)
check_value(10)
m.segment(0).characteristics().setMass(11.0)
check_value(11.0)
with pytest.raises(ValueError,
match="Scalar must be a 1x1 array or a float"):
m.segment(0).characteristics().setMass(np.array([]))
m.segment(0).characteristics().setMass(np.array((12, )))
check_value(12.0)
m.segment(0).characteristics().setMass(np.array([[13]]))
check_value(13.0)
with pytest.raises(ValueError,
match="Scalar must be a 1x1 array or a float"):
m.segment(0).characteristics().setMass(np.array([[[14]]]))
if biorbd.currentLinearAlgebraBackend() == 1:
from casadi import MX
m.segment(0).characteristics().setMass(MX(15))
check_value(15.0)
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, 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[:, [0, -1]] = 0
x_bounds[1, -1] = 3.14
# Initial guess
x_init = InitialGuessOption([0] * (n_q + n_qdot))
# Define control path constraint
u_bounds = BoundsOption([[tau_min] * n_tau, [tau_max] * n_tau])
u_bounds[1, :] = 0
u_init = InitialGuessOption([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 = InitialGuess((min_g + max_g) / 2)
parameter_objective_functions = ObjectiveOption(
my_target_function, weight=10, quadratic=True, custom_type=Objective.Parameter, target=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, 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 test_muscle_activation_and_contacts_tracking(ode_solver):
# Load muscle_activations_contact_tracker
bioptim_folder = TestUtils.bioptim_folder()
contact = TestUtils.load_module(
bioptim_folder + "/examples/muscle_driven_with_contact/muscle_activations_contacts_tracker.py"
)
ode_solver = ode_solver()
# Define the problem
model_path = bioptim_folder + "/examples/muscle_driven_with_contact/2segments_4dof_2contacts_1muscle.bioMod"
biorbd_model = biorbd.Model(model_path)
final_time = 0.1
n_shooting = 5
# Generate random data to fit
np.random.seed(42)
contact_forces_ref = np.random.rand(biorbd_model.nbContacts(), n_shooting)
muscle_activations_ref = np.random.rand(biorbd_model.nbMuscles(), n_shooting + 1)
ocp = contact.prepare_ocp(
model_path,
final_time,
n_shooting,
muscle_activations_ref[:, :-1],
contact_forces_ref,
ode_solver=ode_solver,
)
sol = ocp.solve()
# Check objective function value
f = np.array(sol.cost)
np.testing.assert_equal(f.shape, (1, 1))
np.testing.assert_almost_equal(f[0, 0], 1.2080146471135251)
# Check constraints
g = np.array(sol.constraints)
np.testing.assert_equal(g.shape, (40, 1))
np.testing.assert_almost_equal(g, np.zeros((40, 1)), decimal=6)
# Check some of the results
q, qdot, tau, mus_controls = sol.states["q"], sol.states["qdot"], sol.controls["tau"], sol.controls["muscles"]
# initial and final position
np.testing.assert_almost_equal(q[:, 0], np.array([0.0, 0.0, -0.75, 0.75]))
np.testing.assert_almost_equal(q[:, -1], np.array([0.01785865, -0.01749107, -0.8, 0.8]), decimal=5)
# initial and final velocities
np.testing.assert_almost_equal(qdot[:, 0], np.array([0.0, 0.0, 0.0, 0.0]))
np.testing.assert_almost_equal(qdot[:, -1], np.array([0.5199767, -0.535388, -1.49267023, 1.4926703]), decimal=5)
# initial and final controls
np.testing.assert_almost_equal(tau[:, 0], np.array([5.3773376, 127.6205162, -21.9933179, 1.3644034]), decimal=5)
np.testing.assert_almost_equal(tau[:, -1], np.array([57.203734, 72.3153286, -7.4076227, 1.2641681]), decimal=5)
np.testing.assert_almost_equal(mus_controls[:, 0], np.array([0.18722964]), decimal=5)
np.testing.assert_almost_equal(mus_controls[:, -1], np.array([0.29591125]), decimal=5)
# save and load
TestUtils.save_and_load(sol, ocp, False)
# simulate
TestUtils.simulate(sol)
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 test_forward_dynamics_with_external_forces():
m = biorbd.Model("../../models/pyomecaman_withActuators.bioMod")
q = np.array([i * 1.1 for i in range(m.nbQ())])
qdot = np.array([i * 1.1 for i in range(m.nbQ())])
tau = np.array([i * 1.1 for i in range(m.nbQ())])
# With external forces
if biorbd.currentLinearAlgebraBackend() == 1:
from casadi import Function, MX
sv1 = MX((11.1, 22.2, 33.3, 44.4, 55.5, 66.6))
sv2 = MX((11.1 * 2, 22.2 * 2, 33.3 * 2, 44.4 * 2, 55.5 * 2, 66.6 * 2))
else:
sv1 = np.array((11.1, 22.2, 33.3, 44.4, 55.5, 66.6))
sv2 = np.array(
(11.1 * 2, 22.2 * 2, 33.3 * 2, 44.4 * 2, 55.5 * 2, 66.6 * 2))
f_ext = biorbd.VecBiorbdSpatialVector([
biorbd.SpatialVector(sv1),
biorbd.SpatialVector(sv2),
])
if biorbd.currentLinearAlgebraBackend() == 1:
q_sym = MX.sym("q", m.nbQ(), 1)
qdot_sym = MX.sym("qdot", m.nbQdot(), 1)
tau_sym = MX.sym("tau", m.nbGeneralizedTorque(), 1)
ForwardDynamics = Function(
"ForwardDynamics",
[q_sym, qdot_sym, tau_sym],
[m.ForwardDynamics(q_sym, qdot_sym, tau_sym, f_ext).to_mx()],
["q_sym", "qdot_sym", "tau_sym"],
["qddot"],
).expand()
qddot = ForwardDynamics(q, qdot, tau)
qddot = np.array(qddot)[:, 0]
elif biorbd.currentLinearAlgebraBackend() == 0:
# if Eigen backend is used
qddot = m.ForwardDynamics(q, qdot, tau, f_ext).to_array()
qddot_expected = np.array([
8.8871711208009998,
-13.647827029817943,
-33.606145294752132,
16.922669487341341,
-21.882821189868423,
41.15364990805439,
68.892537246574463,
-324.59756885799197,
-447.99217990207387,
18884.241415786601,
-331.24622725851572,
1364.7620674666462,
3948.4748602722384,
])
np.testing.assert_almost_equal(qddot, qddot_expected)
def prepare_ocp(biorbd_model_path,
final_time,
number_shooting_points,
use_SX=False):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
tau_min, tau_max, tau_init = -1, 1, 0
muscle_min, muscle_max, muscle_init = 0, 1, 0.5
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE)
objective_functions.add(Objective.Lagrange.MINIMIZE_MUSCLES_CONTROL)
objective_functions.add(Objective.Mayer.ALIGN_MARKERS,
first_marker_idx=0,
second_marker_idx=5)
# Dynamics
dynamics = DynamicsTypeList()
dynamics.add(DynamicsType.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
x_bounds[0].min[:, 0] = (0.07, 1.4, 0, 0)
x_bounds[0].max[:, 0] = (0.07, 1.4, 0, 0)
# Initial guess
x_init = InitialConditionsList()
x_init.add([1.57] * biorbd_model.nbQ() + [0] * biorbd_model.nbQdot())
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([
[tau_min] * biorbd_model.nbGeneralizedTorque() +
[muscle_min] * biorbd_model.nbMuscleTotal(),
[tau_max] * biorbd_model.nbGeneralizedTorque() +
[muscle_max] * biorbd_model.nbMuscleTotal(),
])
u_init = InitialConditionsList()
u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque() +
[muscle_init] * biorbd_model.nbMuscleTotal())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
use_SX=use_SX,
)
def prepare_ocp(
biorbd_model_path, phase_time, n_shooting, muscle_activations_ref, contact_forces_ref, ode_solver=OdeSolver.RK4()
):
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
tau_min, tau_max, tau_init = -500, 500, 0
activation_min, activation_max, activation_init = 0, 1, 0.5
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.TRACK_MUSCLES_CONTROL, target=muscle_activations_ref)
objective_functions.add(ObjectiveFcn.Lagrange.TRACK_CONTACT_FORCES, target=contact_forces_ref)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE, weight=0.001)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_ALL_CONTROLS, weight=0.001)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN_WITH_CONTACT)
# 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] * biorbd_model.nbGeneralizedTorque() + [activation_min] * biorbd_model.nbMuscleTotal(),
[tau_max] * biorbd_model.nbGeneralizedTorque() + [activation_max] * biorbd_model.nbMuscleTotal(),
)
u_init = InitialGuessList()
u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque() + [activation_init] * biorbd_model.nbMuscleTotal())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
phase_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions=objective_functions,
ode_solver=ode_solver,
)
def prepare_ocp(biorbd_model_path, final_time, number_shooting_points, nb_threads, use_SX=False):
# --- Options --- #
biorbd_model = biorbd.Model(biorbd_model_path)
torque_min, torque_max, torque_init = -100, 100, 0
n_q = biorbd_model.nbQ()
n_qdot = biorbd_model.nbQdot()
n_tau = biorbd_model.nbGeneralizedTorque()
data_to_track = np.zeros((number_shooting_points + 1, n_q + n_qdot))
data_to_track[:, 1] = 3.14
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=100.0)
objective_functions.add(Objective.Lagrange.MINIMIZE_STATE, weight=1.0)
objective_functions.add(
Objective.Mayer.MINIMIZE_STATE, weight=50000.0, target=data_to_track.T, instant=Instant.END,
)
# Dynamics
dynamics = DynamicsTypeList()
dynamics.add(DynamicsType.TORQUE_DRIVEN)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
x_bounds[0].min[:, 0] = 0
x_bounds[0].max[:, 0] = 0
# Initial guess
x_init = InitialConditionsList()
x_init.add([0] * (n_q + n_qdot))
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add(
[[torque_min] * n_tau, [torque_max] * n_tau,]
)
u_bounds[0].min[n_tau - 1, :] = 0
u_bounds[0].max[n_tau - 1, :] = 0
u_init = InitialConditionsList()
u_init.add([torque_init] * n_tau)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
nb_threads=nb_threads,
use_SX=use_SX,
)
def impact(ocp, transition: PhaseTransition) -> MX:
"""
A discontinuous function that simulates an inelastic impact of a new contact point
Parameters
----------
ocp: OptimalControlProgram
A reference to the ocp
transition: PhaseTransition
A reference to the phase transition
Returns
-------
The difference between the last and first node after applying the impulse equations
"""
if (
ocp.nlp[transition.phase_pre_idx].states.shape
!= ocp.nlp[(transition.phase_pre_idx + 1) % ocp.n_phases].states.shape
):
raise RuntimeError(
"Impact transition without same nx is not possible, please provide a custom phase transition"
)
# Aliases
nlp_pre, nlp_post = PhaseTransitionFunctions.Functions.__get_nlp_pre_and_post(ocp, transition.phase_pre_idx)
n_q = len(nlp_pre.states["q"])
n_qdot = len(nlp_pre.states["qdot"])
q = DynamicsFunctions.get(nlp_pre.states["q"], nlp_pre.X[-1])
qdot_pre = DynamicsFunctions.get(nlp_pre.states["qdot"], nlp_pre.X[-1])
if nlp_post.model.nbContacts() == 0:
warn("The chosen model does not have any contact")
# A new model is loaded here so we can use pre Qdot with post model, this is a hack and should be dealt
# a better way (e.g. create a supplementary variable in v that link the pre and post phase with a
# constraint. The transition would therefore apply to node_0 and node_1 (with an augmented ns)
model = biorbd.Model(nlp_post.model.path().absolutePath().to_string())
func = biorbd.to_casadi_func(
"impulse_direct",
model.ComputeConstraintImpulsesDirect,
nlp_pre.states["q"].mx,
nlp_pre.states["qdot"].mx,
)
qdot_post = func(q, qdot_pre)
qdot_post = nlp_post.variable_mappings["qdot"].to_first.map(qdot_post)
val = []
for key in nlp_pre.states:
if key != "qdot":
# Continuity constraint
var_pre = DynamicsFunctions.get(nlp_pre.states[key], nlp_pre.X[-1])
var_post = DynamicsFunctions.get(nlp_post.states[key], nlp_post.X[0])
val = vertcat(val, var_pre - var_post)
else:
var_post = DynamicsFunctions.get(nlp_post.states[key], nlp_post.X[0])
val = vertcat(val, qdot_post - var_post)
return val
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,
)
