def test_simulate_from_initial_single_shoot():
# Load pendulum
bioptim_folder = TestUtils.bioptim_folder()
pendulum = TestUtils.load_module(bioptim_folder + "/examples/getting_started/example_save_and_load.py")
ocp = pendulum.prepare_ocp(
biorbd_model_path=bioptim_folder + "/examples/getting_started/pendulum.bioMod",
final_time=2,
n_shooting=10,
n_threads=4,
)
X = InitialGuess([-1, -2, 1, 0.5])
U = InitialGuess(np.array([[-0.1, 0], [1, 2]]).T, interpolation=InterpolationType.LINEAR)
sol = Solution(ocp, [X, U])
controls = sol.controls
sol = sol.integrate(shooting_type=Shooting.SINGLE_CONTINUOUS, keep_intermediate_points=True)
# Check some of the results
states = sol.states
q, qdot, tau = states["q"], states["qdot"], controls["tau"]
# initial and final position
np.testing.assert_almost_equal(q[:, 0], np.array((-1.0, -2.0)))
np.testing.assert_almost_equal(q[:, -1], np.array((-0.70545232, 2.02188073)))
# initial and final velocities
np.testing.assert_almost_equal(qdot[:, 0], np.array((1.0, 0.5)))
np.testing.assert_almost_equal(qdot[:, -1], np.array((1.21773723, -0.77896332)))
# initial and final controls
np.testing.assert_almost_equal(tau[:, 0], np.array((-0.1, 0.0)))
np.testing.assert_almost_equal(tau[:, -2], np.array((0.89, 1.8)))
def test_simulate_from_initial_multiple_shoot():
bioptim_folder = TestUtils.bioptim_folder()
pendulum = TestUtils.load_module(
bioptim_folder + "/examples/getting_started/example_save_and_load.py")
ocp = pendulum.prepare_ocp(
biorbd_model_path=bioptim_folder +
"/examples/getting_started/models/pendulum.bioMod",
final_time=2,
n_shooting=10,
n_threads=4,
)
X = InitialGuess([-1, -2, 1, 0.5])
U = InitialGuess(np.array([[-0.1, 0], [1, 2]]).T,
interpolation=InterpolationType.LINEAR)
sol = Solution(ocp, [X, U])
controls = sol.controls
sol = sol.integrate(shooting_type=Shooting.MULTIPLE,
keep_intermediate_points=True)
states = sol.states
# Check some of the results
q, qdot, tau = states["q"], states["qdot"], controls["tau"]
# initial and final position
np.testing.assert_almost_equal(q[:, 0], np.array((-1.0, -2.0)))
np.testing.assert_almost_equal(q[:, -1], np.array(
(-0.7553692, -1.6579819)))
# initial and final velocities
np.testing.assert_almost_equal(qdot[:, 0], np.array((1.0, 0.5)))
np.testing.assert_almost_equal(qdot[:, -1], np.array(
(1.05240919, 2.864199)))
# initial and final controls
np.testing.assert_almost_equal(tau[:, 0], np.array((-0.1, 0.0)))
np.testing.assert_almost_equal(tau[:, -2], np.array((0.89, 1.8)))
def test_simulate_from_initial_single_shoot():
# Load pendulum
from bioptim.examples.getting_started import example_save_and_load as ocp_module
bioptim_folder = os.path.dirname(ocp_module.__file__)
ocp = ocp_module.prepare_ocp(
biorbd_model_path=bioptim_folder + "/models/pendulum.bioMod",
final_time=2,
n_shooting=10,
n_threads=4,
)
X = InitialGuess([-1, -2, 0.1, 0.2])
U = InitialGuess(np.array([[-0.1, 0], [1, 2]]).T,
interpolation=InterpolationType.LINEAR)
sol = Solution(ocp, [X, U])
controls = sol.controls
sol = sol.integrate(shooting_type=Shooting.SINGLE_CONTINUOUS,
keep_intermediate_points=True)
# Check some of the results
states = sol.states
q, qdot, tau = states["q"], states["qdot"], controls["tau"]
# initial and final position
np.testing.assert_almost_equal(q[:, 0], np.array((-1.0, -2.0)))
np.testing.assert_almost_equal(q[:, -1], np.array(
(-0.33208579, 0.06094747)))
# initial and final velocities
np.testing.assert_almost_equal(qdot[:, 0], np.array((0.1, 0.2)))
np.testing.assert_almost_equal(qdot[:, -1],
np.array((-4.43192682, 6.38146735)))
# initial and final controls
np.testing.assert_almost_equal(tau[:, 0], np.array((-0.1, 0.0)))
np.testing.assert_almost_equal(tau[:, -2], np.array((0.89, 1.8)))
def main():
# --- Load pendulum --- #
ocp = pendulum.prepare_ocp(
biorbd_model_path="models/pendulum.bioMod",
final_time=2,
n_shooting=10,
)
# Simulation the Initial Guess
# Interpolation: Constant
X = InitialGuess([0, 0, 0, 0])
U = InitialGuess([-1, 1])
sol_from_initial_guess = Solution(ocp, [X, U])
s = sol_from_initial_guess.integrate(
shooting_type=Shooting.SINGLE_CONTINUOUS)
print(
f"Final position of q from single shooting of initial guess = {s.states['q'][:, -1]}"
)
# Uncomment the next line to animate the integration
# s.animate()
# Interpolation: Each frame (for instance, values from a previous optimization or from measured data)
U = np.random.rand(2, 11)
U = InitialGuess(U, interpolation=InterpolationType.EACH_FRAME)
sol_from_initial_guess = Solution(ocp, [X, U])
s = sol_from_initial_guess.integrate(
shooting_type=Shooting.SINGLE_CONTINUOUS)
print(
f"Final position of q from single shooting of initial guess = {s.states['q'][:, -1]}"
)
# Uncomment the next line to animate the integration
# s.animate()
# Uncomment the following lines to graph the solution from initial guesses
# sol_from_initial_guess.graphs(shooting_type=Shooting.SINGLE_CONTINUOUS)
# sol_from_initial_guess.graphs(shooting_type=Shooting.MULTIPLE)
# Simulation of the solution. It is not the graph of the solution,
# it is the graph of a Runge Kutta from the solution
sol = ocp.solve()
s_single = sol.integrate(shooting_type=Shooting.SINGLE_CONTINUOUS)
# Uncomment the next line to animate the integration
# s_single.animate()
print(
f"Final position of q from single shooting of the solution = {s_single.states['q'][:, -1]}"
)
s_multiple = sol.integrate(shooting_type=Shooting.MULTIPLE,
keep_intermediate_points=True)
print(
f"Final position of q from multiple shooting of the solution = {s_multiple.states['q'][:, -1]}"
)
def initial_states_from_single_shooting(model, ns, tf, ode_solver):
ocp = prepare_single_shooting(model, ns, tf, ode_solver)
# Find equilibrium
X = InitialGuess([0, 0.10, 0, 1e-10, 1e-10, 1e-10])
U = InitialGuess([0, 0, 0])
sol_from_initial_guess = Solution(ocp, [X, U])
s = sol_from_initial_guess.integrate(shooting_type=Shooting.SINGLE,
continuous=True)
# s.animate()
# Rolling Sphere at equilibrium
x0 = s.states["q"][:, -1]
dx0 = [0] * 3
X0 = np.concatenate([x0, np.array(dx0)])
X = InitialGuess(X0)
U = InitialGuess([0, 0, -10])
sol_from_initial_guess = Solution(ocp, [X, U])
s = sol_from_initial_guess.integrate(shooting_type=Shooting.SINGLE,
continuous=True)
# s.animate()
return X0
def generate_table(out):
root_path = "/".join(__file__.split("/")[:-1])
model_path = root_path + "/models/arm_wt_rot_scap.bioMod"
biorbd_model = biorbd.Model(model_path)
# --- Prepare and solve MHE --- #
t = 8
ns = 800
ns_mhe = 7
rt_ratio = 3
t_mhe = t / (ns / rt_ratio) * ns_mhe
# --- Prepare reference data --- #
with open(
f"{root_path}/data/sim_ac_8000ms_800sn_REACH2_co_level_0_step5_ERK.bob",
"rb") as file:
data = pickle.load(file)
states = data["data"][0]
controls = data["data"][1]
q_ref, dq_ref, u_ref = states["q"], states["qdot"], controls["muscles"]
ocp = prepare_ocp(biorbd_model=biorbd_model,
final_time=t_mhe,
n_shooting=ns_mhe)
q_noise = 5
x_ref = np.concatenate((generate_noise(biorbd_model, q_ref,
q_noise), dq_ref))
x_est = np.zeros(
(biorbd_model.nbQ() * 2, x_ref[:, ::rt_ratio].shape[1] - ns_mhe))
u_est = np.zeros(
(biorbd_model.nbMuscles(), u_ref[:, ::rt_ratio].shape[1] - ns_mhe))
# Update bounds
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
x_bounds[0].min[:biorbd_model.nbQ(),
0] = x_ref[:biorbd_model.nbQ(), 0] - 0.1
x_bounds[0].max[:biorbd_model.nbQ(),
0] = x_ref[:biorbd_model.nbQ(), 0] + 0.1
ocp.update_bounds(x_bounds)
# Update initial guess
x_init = InitialGuess(x_ref[:, :ns_mhe + 1],
interpolation=InterpolationType.EACH_FRAME)
u_init = InitialGuess([0.2] * biorbd_model.nbMuscles(),
interpolation=InterpolationType.CONSTANT)
ocp.update_initial_guess(x_init, u_init)
# Update objectives functions
objectives = define_objective(q_ref, 0, rt_ratio, ns_mhe, biorbd_model)
ocp.update_objectives(objectives)
# Initialize the solver options
sol = ocp.solve(
solver=Solver.ACADOS,
show_online_optim=False,
solver_options={
"nlp_solver_tol_comp": 1e-10,
"nlp_solver_tol_eq": 1e-10,
"nlp_solver_tol_stat": 1e-8,
"integrator_type": "IRK",
"nlp_solver_type": "SQP",
"sim_method_num_steps": 1,
"print_level": 0,
"nlp_solver_max_iter": 30,
},
)
# Set solutions and set initial guess for next optimisation
x0, u0, x_est[:, 0], u_est[:, 0] = warm_start_mhe(sol)
tic = time() # Save initial time
for i in range(1, x_est.shape[1]):
# set initial state
ocp.nlp[0].x_bounds.min[:, 0] = x0[:, 0]
ocp.nlp[0].x_bounds.max[:, 0] = x0[:, 0]
# Update initial guess
x_init = InitialGuess(x0, interpolation=InterpolationType.EACH_FRAME)
u_init = InitialGuess(u0, interpolation=InterpolationType.EACH_FRAME)
ocp.update_initial_guess(x_init, u_init)
# Update objectives functions
objectives = define_objective(q_ref, i, rt_ratio, ns_mhe, biorbd_model)
ocp.update_objectives(objectives)
# Solve problem
sol = ocp.solve(
solver=Solver.ACADOS,
show_online_optim=False,
solver_options={
"nlp_solver_tol_comp": 1e-6,
"nlp_solver_tol_eq": 1e-6,
"nlp_solver_tol_stat": 1e-5
},
)
# Set solutions and set initial guess for next optimisation
x0, u0, x_out, u_out = warm_start_mhe(sol)
x_est[:, i] = x_out
if i < u_est.shape[1]:
u_est[:, i] = u_out
toc = time() - tic
n = x_est.shape[1] - 1
tf = (ns - ns % rt_ratio) / (ns / t)
final_time = tf - (ns_mhe * (tf / (n + ns_mhe)))
short_ocp = prepare_short_ocp(biorbd_model,
final_time=final_time,
n_shooting=n)
x_init_guess = InitialGuess(x_est,
interpolation=InterpolationType.EACH_FRAME)
u_init_guess = InitialGuess(u_est,
interpolation=InterpolationType.EACH_FRAME)
sol = Solution(short_ocp, [x_init_guess, u_init_guess])
out.solver.append(out.Solver("Acados"))
out.nx = x_est.shape[0]
out.nu = u_est.shape[0]
out.ns = n
out.solver[0].n_iteration = "N.A."
out.solver[0].cost = "N.A."
out.solver[0].convergence_time = toc
out.solver[0].compute_error_single_shooting(sol, 1)
if __name__ == "__main__":
# --- Load pendulum --- #
ocp = pendulum.prepare_ocp(
biorbd_model_path="pendulum.bioMod",
final_time=2,
n_shooting=10,
)
# Simulation the Initial Guess
# Interpolation: Constant
X = InitialGuess([0, 0, 0, 0])
U = InitialGuess([-1, 1])
sol_from_initial_guess = Solution(ocp, [X, U])
s = sol_from_initial_guess.integrate(shooting_type=Shooting.SINGLE_CONTINUOUS)
print(f"Final position of q from single shooting of initial guess = {s.states['q'][:, -1]}")
# Uncomment the next line to animate the integration
# s.animate()
# Interpolation: Each frame (for instance, values from a previous optimization or from measured data)
U = np.random.rand(2, 11)
U = InitialGuess(U, interpolation=InterpolationType.EACH_FRAME)
sol_from_initial_guess = Solution(ocp, [X, U])
s = sol_from_initial_guess.integrate(shooting_type=Shooting.SINGLE_CONTINUOUS)
print(f"Final position of q from single shooting of initial guess = {s.states['q'][:, -1]}")
# Uncomment the next line to animate the integration
# s.animate()