def simulate(sol, ocp, decimal_value=None):
sol_from_solver = np.array(sol["x"]).squeeze()
sol_simulation_from_solve = Simulate.from_solve(ocp, sol)["x"]
sol_simulation_from_data = Simulate.from_data(ocp,
Data.get_data(ocp,
sol))["x"]
if decimal_value is None:
np.testing.assert_almost_equal(sol_from_solver,
sol_simulation_from_solve)
np.testing.assert_almost_equal(sol_from_solver,
sol_simulation_from_data)
np.testing.assert_almost_equal(sol_simulation_from_solve,
sol_simulation_from_data)
else:
np.testing.assert_almost_equal(sol_from_solver,
sol_simulation_from_solve,
decimal=decimal_value)
np.testing.assert_almost_equal(sol_from_solver,
sol_simulation_from_data,
decimal=decimal_value)
np.testing.assert_almost_equal(sol_simulation_from_solve,
sol_simulation_from_data,
decimal=decimal_value)
def test_simulate_from_initial_single_shoot():
# Load pendulum
PROJECT_FOLDER = Path(__file__).parent / ".."
spec = importlib.util.spec_from_file_location(
"pendulum",
str(PROJECT_FOLDER) + "/examples/getting_started/pendulum.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/pendulum.bioMod",
final_time=2,
number_shooting_points=10,
nb_threads=4,
)
X = InitialConditions([-1, -2, 1, 0.5])
U = InitialConditions(np.array([[-0.1, 0], [1, 2]]).T,
interpolation=InterpolationType.LINEAR)
sol_simulate_single_shooting = Simulate.from_controls_and_initial_states(
ocp, X, U, single_shoot=True)
# Check some of the results
states, controls = Data.get_data(ocp, sol_simulate_single_shooting["x"])
q, qdot, tau = states["q"], states["q_dot"], 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.59371229, 2.09731719)))
# 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.38153013, -0.60425128)))
# initial and final controls
np.testing.assert_almost_equal(tau[:, 0], np.array((-0.1, 0.0)))
np.testing.assert_almost_equal(tau[:, -1], np.array((1.0, 2.0)))
i = 6
x_ac = np.ndarray((biorbd_model.nbQ()*2, n_shooting_points+1))
sol_ac = ocp_ac.solve(solver=Solver.ACADOS, solver_options={
"qp_solver": "PARTIAL_CONDENSING_HPIPM", "integrator_type": "IRK",
"nlp_solver_max_iter": 150, "sim_method_num_steps": 5,
"nlp_solver_tol_ineq": float("1e%d" %-i), "nlp_solver_tol_stat": float("1e%d" %-i),
"nlp_solver_tol_comp": float("1e%d" %-i), "nlp_solver_tol_eq": float("1e%d" %-i)
})# FULL_CONDENSING_QPOASES, "PARTIAL_CONDENSING_HPIPM"
n_shooting_points += 1
states_sol, controls_sol = Data.get_data(ocp_ac, sol_ac["x"])
x_ac[:, :] = np.concatenate((states_sol['q'], states_sol['q_dot']))
sol_simulate_single_shooting_ac = Simulate.from_solve(ocp_ac, sol_ac, single_shoot=True)
RMSE_ac = np.ndarray((1, biorbd_model.nbQ()*2))
for k in range(biorbd_model.nbQ()*2):
sum = 0
for s in range(4, n_shooting_points):
sum = sum + ((x_ac[k, s] - sol_simulate_single_shooting_ac['x'][s*28 + k])**2)
RMSE_ac[0, k] = (np.sqrt((sum/n_shooting_points)))
sio.savemat("RMSE_ac_nX_" + str(i) + ".mat",
{"RMSE_ac": RMSE_ac})
RMSE_ac = np.ndarray((1, n_shooting_points))
for s in range(n_shooting_points):
sum = 0
for k in range(biorbd_model.nbQ()*2):
sum = sum + ((x_ac[k, s] - sol_simulate_single_shooting_ac['x'][s*28 + k])**2)
import time
import sys
import pickle
from biorbd_optim import OptimalControlProgram, ShowResult, Data, Simulate
from up_and_down_bow import xia_model_dynamic, xia_model_configuration, xia_model_fibers, xia_initial_fatigue_at_zero
file_path = "results/xia 5 phases/2020_7_25_upDown.bo"
if len(sys.argv) > 1:
file_path = str(sys.argv[1])
if not isinstance(file_path, str):
t = time.localtime(time.time())
file_path = f"results/{t.tm_year}_{t.tm_mon}_{t.tm_mday}_upDown.bo"
ocp, sol = OptimalControlProgram.load(file_path)
d = Data.get_data(ocp, Simulate.from_solve(ocp, sol, single_shoot=True))
dict = {"data": d}
with open(file_path[:-3] + "_single.bob", "wb") as file:
pickle.dump(dict, file)
solver=Solver.IPOPT,
show_online_optim=False,
solver_options={
"tol": float("1e%d" % -i),
"max_iter": 150,
# "dual_inf_tol": 1,
"constr_viol_tol": float("1e%d" % -i),
"compl_inf_tol": float("1e%d" % -i),
# "hessian_approximation": "limited-memory",
"hessian_approximation": "exact",
"linear_solver": "ma57", # "ma57", "ma86", "mumps"
})
states_sol, controls_sol = Data.get_data(ocp_ip, sol_ip["x"])
x_ip[idx, :, :] = np.concatenate((states_sol['q'], states_sol['q_dot']))
sol_simulate_single_shooting_ip[idx] = Simulate.from_solve(
ocp_ip, sol_ip, single_shoot=True)
n_shooting_points += 1
RMSE_ip = np.ndarray((tol_final - tol_init, biorbd_model.nbQ() * 2))
sum = 0
for i in range(tol_final - tol_init):
for s in range(biorbd_model.nbQ() * 2):
sum = 0
for k in range(n_shooting_points):
sum = sum + (
(x_ip[i, s, k] -
sol_simulate_single_shooting_ip[i]['x'][k * 28 + s])**2)
RMSE_ip[i, s] = np.sqrt(sum / n_shooting_points)
sio.savemat("RMSE_ip_nQ.mat", {"RMSE_ip": RMSE_ip})
if __name__ == "__main__":
ocp = prepare_ocp(biorbd_model_path="pendulum.bioMod",
final_time=3,
number_shooting_points=100,
nb_threads=4)
# --- Solve the program --- #
tic = time()
sol, sol_iterations = ocp.solve(show_online_optim=True,
return_iterations=True)
toc = time() - tic
print(f"Time to solve : {toc}sec")
# --- Simulation --- #
# It is not an optimal control, it only apply a Runge Kutta at each nodes
Simulate.from_solve(ocp, sol, single_shoot=True)
Simulate.from_data(ocp, Data.get_data(ocp, sol), single_shoot=False)
# --- Access to all iterations --- #
nb_iter = len(sol_iterations)
third_iteration = sol_iterations[2]
# --- Save result of get_data --- #
ocp.save_get_data(
sol, "pendulum.bob",
sol_iterations) # you don't have to specify the extension ".bob"
# --- Load result of get_data --- #
with open("pendulum.bob", "rb") as file:
data = pickle.load(file)
str(PROJECT_FOLDER) + "/examples/getting_started/pendulum.py")
pendulum = importlib.util.module_from_spec(spec)
spec.loader.exec_module(pendulum)
ocp = pendulum.prepare_ocp(
biorbd_model_path="pendulum.bioMod",
final_time=2,
number_shooting_points=10,
nb_threads=2,
)
X = InitialConditions([0, 0, 0, 0])
U = InitialConditions([-1, 1])
# --- Single shooting --- #
sol_simulate_single_shooting = Simulate.from_controls_and_initial_states(
ocp, X, U, single_shoot=True)
result_single = ShowResult(ocp, sol_simulate_single_shooting)
result_single.graphs()
# --- Multiple shooting --- #
sol_simulate_multiple_shooting = Simulate.from_controls_and_initial_states(
ocp, X, U, single_shoot=False)
result_multiple = ShowResult(ocp, sol_simulate_multiple_shooting)
result_multiple.graphs()
# --- Single shooting --- #
result_single.animate()
# --- Multiple shooting --- #
result_multiple.animate()