def main(): model_path = "models/arm2.bioMod" torque_max = 50 cycle_duration = 1 cycle_len = 20 n_cycles_to_advance = 1 n_cycles_simultaneous = 3 n_cycles = 4 nmpc = prepare_nmpc( model_path, cycle_len=cycle_len, cycle_duration=cycle_duration, n_cycles_to_advance=n_cycles_to_advance, n_cycles_simultaneous=n_cycles_simultaneous, max_torque=torque_max, ) def update_functions(_nmpc: MultiCyclicNonlinearModelPredictiveControl, cycle_idx: int, _sol: Solution): return cycle_idx < n_cycles # True if there are still some cycle to perform # Solve the program sol = nmpc.solve( update_functions, solver=Solver.IPOPT(show_online_optim=True), n_cycles_simultaneous=n_cycles_simultaneous, ) sol.print() sol.graphs() sol.animate(n_frames=100)
def main(): """ Create multiple new plot and add new stuff to them using a custom defined function """ # Prepare the Optimal Control Program ocp = prepare_ocp(biorbd_model_path="models/pendulum.bioMod", final_time=2, n_shooting=50) # Add my lovely new plot ocp.add_plot("My New Extra Plot", lambda t, x, u, p: custom_plot_callback(x, [0, 1, 3]), plot_type=PlotType.PLOT) ocp.add_plot( "My New Extra Plot", lambda t, x, u, p: custom_plot_callback(x, [1, 3]), plot_type=PlotType.STEP, axes_idx=[1, 2], ) ocp.add_plot( "My Second New Extra Plot", lambda t, x, u, p: custom_plot_callback(x, [1, 3]), plot_type=PlotType.INTEGRATED, axes_idx=[1, 2], ) # --- Solve the program --- # sol = ocp.solve(Solver.IPOPT(show_online_optim=False)) # --- Show results --- # sol.graphs()
def solve_ocp(implicit_dynamics: bool) -> OptimalControlProgram: """ The initialization of ocp with implicit_dynamics as the only argument Parameters ---------- implicit_dynamics: bool implicit Returns ------- The OptimalControlProgram ready to be solved """ model_path = "models/pendulum.bioMod" n_shooting = 200 # The higher it is, the closer implicit and explicit solutions are. ode_solver = OdeSolver.RK2(n_integration_steps=1) time = 1 # --- Prepare the ocp with implicit dynamics --- # ocp = prepare_ocp( biorbd_model_path=model_path, final_time=time, n_shooting=n_shooting, ode_solver=ode_solver, implicit_dynamics=implicit_dynamics, ) # --- Custom Plots --- # ocp.add_plot_penalty(CostType.ALL) # --- Solve the ocp --- # sol_opt = Solver.IPOPT(show_online_optim=False) sol = ocp.solve(sol_opt) return sol
def assert_warm_start(ocp, sol, state_decimal=2, control_decimal=2, param_decimal=2): ocp.set_warm_start(sol) solver = Solver.IPOPT() solver.set_maximum_iterations(0) solver.set_initialization_options(1e-10) sol_warm_start = ocp.solve(solver) if ocp.n_phases > 1: for i in range(ocp.n_phases): np.testing.assert_almost_equal(sol_warm_start.states[i]["all"], sol.states[i]["all"], decimal=state_decimal) np.testing.assert_almost_equal( sol_warm_start.controls[i]["all"], sol.controls[i]["all"], decimal=control_decimal) else: np.testing.assert_almost_equal(sol_warm_start.states["all"], sol.states["all"], decimal=state_decimal) np.testing.assert_almost_equal(sol_warm_start.controls["all"], sol.controls["all"], decimal=control_decimal) np.testing.assert_almost_equal(sol_warm_start.parameters["all"], sol.parameters["all"], decimal=param_decimal)
def main(): """ Solve and print the optimized value for the gravity and animate the solution """ optim_gravity = True optim_mass = True ocp = prepare_ocp( biorbd_model_path="models/pendulum.bioMod", final_time=3, n_shooting=100, optim_gravity=optim_gravity, optim_mass=optim_mass, min_g=np.array([-1, -1, -10]), max_g=np.array([1, 1, -5]), min_m=10, max_m=30, target_g=np.array([0, 0, -9.81]), target_m=20, ) # --- Solve the program --- # sol = ocp.solve(Solver.IPOPT(show_online_optim=False)) # --- Get the results --- # if optim_gravity: gravity = sol.parameters["gravity_xyz"] print(f"Optimized gravity: {gravity[:, 0]}") if optim_mass: mass = sol.parameters["mass"] print(f"Optimized mass: {mass[0, 0]}") # --- Show results --- # sol.animate(n_frames=200)
def main(): """ Generate random data, then create a tracking problem, and finally solve it and plot some relevant information """ # Define the problem biorbd_model = biorbd.Model("models/arm26.bioMod") final_time = 0.5 n_shooting_points = 30 use_residual_torque = True # Generate random data to fit t, markers_ref, x_ref, muscle_excitations_ref = generate_data( biorbd_model, final_time, n_shooting_points) # Track these data biorbd_model = biorbd.Model( "models/arm26.bioMod" ) # To allow for non free variable, the model must be reloaded ocp = prepare_ocp( biorbd_model, final_time, n_shooting_points, markers_ref, muscle_excitations_ref, x_ref[:biorbd_model.nbQ(), :], use_residual_torque=use_residual_torque, kin_data_to_track="q", ) # --- Solve the program --- # sol = ocp.solve(Solver.IPOPT(show_online_optim=True)) # --- Show the results --- # q = sol.states["q"] n_q = ocp.nlp[0].model.nbQ() n_mark = ocp.nlp[0].model.nbMarkers() n_frames = q.shape[1] markers = np.ndarray((3, n_mark, q.shape[1])) symbolic_states = MX.sym("x", n_q, 1) markers_func = biorbd.to_casadi_func("ForwardKin", biorbd_model.markers, symbolic_states) for i in range(n_frames): markers[:, :, i] = markers_func(q[:, i]) plt.figure("Markers") n_steps_ode = ocp.nlp[0].ode_solver.steps + 1 if ocp.nlp[ 0].ode_solver.is_direct_collocation else 1 for i in range(markers.shape[1]): plt.plot(np.linspace(0, 2, n_shooting_points + 1), markers_ref[:, i, :].T, "k") plt.plot(np.linspace(0, 2, n_shooting_points * n_steps_ode + 1), markers[:, i, :].T, "r--") plt.xlabel("Time") plt.ylabel("Markers Position") # --- Plot --- # plt.show()
def main(): ocp = prepare_ocp() # --- Solve the program --- # sol = ocp.solve(Solver.IPOPT(show_online_optim=True)) # --- Show results --- # sol.animate()
def main(): """ Prepare and solve and animate a reaching task ocp """ ocp = prepare_ocp(biorbd_model_path="models/arm26.bioMod", final_time=0.5, n_shooting=50, weight=1000) # --- Solve the program --- # sol = ocp.solve(Solver.IPOPT(show_online_optim=True)) # --- Show results --- # sol.animate(show_meshes=True)
def main(): """ Solves an ocp where the symmetry must be respected, and animates it """ ocp = prepare_ocp() # --- Solve the program --- # sol = ocp.solve(Solver.IPOPT(show_online_optim=True)) # --- Show results --- # sol.animate()
def main(): """ Runs and animate the program """ ocp = prepare_ocp("models/cube.bioMod", n_shooting=30, final_time=2, loop_from_constraint=True) # --- Solve the program --- # sol = ocp.solve(Solver.IPOPT(show_online_optim=True)) # --- Show results --- # sol.animate()
def main(): """ Prepares and solves an ocp that has quaternion in it. Animates the results """ ocp = prepare_ocp("models/TruncAnd2Arm_Quaternion.bioMod", n_shooting=5, final_time=0.25) sol = ocp.solve(Solver.IPOPT(show_online_optim=True)) # Print the last solution sol.animate(n_frames=-1)
def main(): """ Solve an ocp with external forces and animates the solution """ ocp = prepare_ocp() # --- Solve the program --- # sol = ocp.solve(Solver.IPOPT(show_online_optim=True)) # --- Show results --- # sol.animate()
def main(): """ Defines a multiphase ocp and animate the results """ ocp = prepare_ocp() # --- Solve the program --- # sol = ocp.solve(Solver.IPOPT(show_online_optim=False)) sol.print_cost() sol.graphs() # --- Show results --- # sol.animate()
def main(): """ Defines a multiphase ocp and animate the results """ ocp = prepare_ocp(long_optim=False) ocp.add_plot_penalty(CostType.ALL) # --- Solve the program --- # sol = ocp.solve(Solver.IPOPT(show_online_optim=True)) # --- Show results --- # sol.animate()
def main(): """ Runs the optimization and animates it """ model_path = "models/cube.bioMod" ocp = prepare_ocp(biorbd_model_path=model_path) # --- Solve the program --- # sol = ocp.solve(Solver.IPOPT(show_online_optim=True)) # --- Show results --- # sol.animate()
def main(): """ Solves an ocp where the symmetry is enforced by constraints, and animates it """ ocp = prepare_ocp() # Objective and constraints plots ocp.add_plot_penalty() # --- Solve the program --- # sol = ocp.solve(Solver.IPOPT(show_online_optim=True)) # --- Show results --- # sol.animate()
def test_multi_cyclic_nmpc(): def update_functions(_nmpc, cycle_idx, _sol): return cycle_idx < n_cycles_total # True if there are still some cycle to perform from bioptim.examples.moving_horizon_estimation import multi_cyclic_nmpc as ocp_module bioptim_folder = os.path.dirname(ocp_module.__file__) n_cycles_simultaneous = 2 n_cycles_to_advance = 1 n_cycles_total = 3 cycle_len = 20 nmpc = ocp_module.prepare_nmpc( model_path=bioptim_folder + "/models/arm2.bioMod", cycle_len=cycle_len, cycle_duration=1, n_cycles_simultaneous=n_cycles_simultaneous, n_cycles_to_advance=n_cycles_to_advance, max_torque=50, ) sol = nmpc.solve(update_functions, solver=Solver.IPOPT(), n_cycles_simultaneous=n_cycles_simultaneous) # Check some of the results states, controls = sol.states, sol.controls q, qdot, tau = states["q"], states["qdot"], controls["tau"] # initial and final position np.testing.assert_equal(q.shape, (3, n_cycles_total * cycle_len)) np.testing.assert_almost_equal( q[:, 0], np.array((-12.56637061, 1.04359174, 1.03625065))) np.testing.assert_almost_equal( q[:, -1], np.array((-6.59734457, 0.89827771, 1.0842402))) # initial and final velocities np.testing.assert_almost_equal( qdot[:, 0], np.array((6.28293718, 2.5617072, -0.00942694))) np.testing.assert_almost_equal( qdot[:, -1], np.array((6.28343343, 3.28099958, -1.27304428))) # initial and final controls np.testing.assert_almost_equal( tau[:, 0], np.array((0.00992505, 4.88488618, 2.4400698))) np.testing.assert_almost_equal(tau[:, -2], np.array( (0.00992505, 9.18387711, 5.22418771)), decimal=6)
def test_add_new_plot(): # Load graphs_one_phase from bioptim.examples.torque_driven_ocp import track_markers_with_torque_actuators as ocp_module bioptim_folder = os.path.dirname(ocp_module.__file__) ocp = ocp_module.prepare_ocp( biorbd_model_path=bioptim_folder + "/models/cube.bioMod", n_shooting=20, final_time=0.5, ) solver = Solver.IPOPT() solver.set_maximum_iterations(1) sol = ocp.solve(solver) # Saving/loading files reset the plot settings to normal save_name = "test_plot.bo" ocp.save(sol, save_name) # Test 1 - Working plot ocp.add_plot("My New Plot", lambda t, x, u, p: x[0:2, :]) sol.graphs(automatically_organize=False) # Test 2 - Combine using combine_to is not allowed ocp, sol = OptimalControlProgram.load(save_name) with pytest.raises(RuntimeError): ocp.add_plot("My New Plot", lambda t, x, u, p: x[0:2, :], combine_to="NotAllowed") # Test 3 - Create a completely new plot ocp, sol = OptimalControlProgram.load(save_name) ocp.add_plot("My New Plot", lambda t, x, u, p: x[0:2, :]) ocp.add_plot("My Second New Plot", lambda t, x, p, u: x[0:2, :]) sol.graphs(automatically_organize=False) # Test 4 - Combine to the first using fig_name ocp, sol = OptimalControlProgram.load(save_name) ocp.add_plot("My New Plot", lambda t, x, u, p: x[0:2, :]) ocp.add_plot("My New Plot", lambda t, x, u, p: x[0:2, :]) sol.graphs(automatically_organize=False) # Add the plot of objectives and constraints to this mess ocp.add_plot_penalty(CostType.ALL) sol.graphs(automatically_organize=False) # Delete the saved file os.remove(save_name)
def main(): """ Prepares, solves and animates the program """ ocp = prepare_ocp( biorbd_model_path="models/cube_and_line.bioMod", n_shooting=30, final_time=1, ) # --- Solve the program --- # sol = ocp.solve(Solver.IPOPT(show_online_optim=True)) # --- Show results --- # sol.animate()
def main(): """ Prepare and solve and animate a reaching task ocp """ ocp = prepare_ocp(biorbd_model_path="models/arm26_with_contact.bioMod", final_time=1, n_shooting=30, weight=1000) # --- Solve the program --- # sol = ocp.solve(Solver.IPOPT(show_online_optim=True)) # --- Show results --- # sol.print_cost() sol.animate()
def main(): """ Solve and animate the solution """ model_path = "models/cube.bioMod" ocp = prepare_ocp(biorbd_model_path=model_path) # Custom plots ocp.add_plot_penalty() # --- Solve the program --- # sol = ocp.solve(Solver.IPOPT(show_online_optim=True)) # --- Show results --- # sol.animate()
def main(): """ Run a multiphase problem with free time phases and animate the results """ final_time = [2, 5, 4] time_min = [1, 3, 0.1] time_max = [2, 4, 0.8] ns = [20, 30, 20] ocp = prepare_ocp(final_time=final_time, time_min=time_min, time_max=time_max, n_shooting=ns) # --- Solve the program --- # sol = ocp.solve(Solver.IPOPT(show_online_optim=True)) # --- Show results --- # param = sol.parameters print(f"The optimized phase time are: {param['time'][0, 0]}s, {param['time'][1, 0]}s and {param['time'][2, 0]}s.") sol.animate()
def main(): model_path = "../torque_driven_ocp/models/2segments_4dof_2contacts.bioMod" t = 0.3 ns = 10 mu = 0.2 ocp = prepare_ocp( biorbd_model_path=model_path, phase_time=t, n_shooting=ns, min_bound=50, max_bound=np.inf, mu=mu, ) # --- Solve the program --- # sol = ocp.solve(Solver.IPOPT(show_online_optim=True)) # --- Show results --- # sol.animate()
def test_mhe_redim_xbounds_not_implemented(): root_folder = TestUtils.bioptim_folder( ) + "/examples/moving_horizon_estimation/" biorbd_model = biorbd.Model(root_folder + "models/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, window_len + 1)), np.zeros((nq * 2, window_len + 1)), interpolation=InterpolationType.EACH_FRAME, ) 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 with pytest.raises( NotImplementedError, match="The MHE is not implemented yet for x_bounds not being " "CONSTANT or CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT", ): mhe.solve(update_functions, Solver.IPOPT())
def test_cyclic_nmpc(): def update_functions(_nmpc, cycle_idx, _sol): return cycle_idx < n_cycles # True if there are still some cycle to perform from bioptim.examples.moving_horizon_estimation import cyclic_nmpc as ocp_module bioptim_folder = os.path.dirname(ocp_module.__file__) n_cycles = 3 cycle_len = 20 nmpc = ocp_module.prepare_nmpc( model_path=bioptim_folder + "/models/arm2.bioMod", cycle_len=cycle_len, cycle_duration=1, max_torque=50, ) sol = nmpc.solve(update_functions, solver=Solver.IPOPT()) # Check some of the results states, controls = sol.states, sol.controls q, qdot, tau = states["q"], states["qdot"], controls["tau"] # initial and final position np.testing.assert_equal(q.shape, (3, n_cycles * cycle_len)) np.testing.assert_almost_equal( q[:, 0], np.array((-3.14159265, 0.12979378, 2.77623291))) np.testing.assert_almost_equal( q[:, -1], np.array((2.82743339, 0.63193395, 2.68235056))) # initial and final velocities np.testing.assert_almost_equal( qdot[:, 0], np.array((6.28268908, -11.63289399, 0.37215021))) np.testing.assert_almost_equal( qdot[:, -1], np.array((6.28368154, -7.73180135, 3.56900657))) # initial and final controls np.testing.assert_almost_equal( tau[:, 0], np.array((0.01984925, 17.53758229, -1.92204945))) np.testing.assert_almost_equal(tau[:, -2], np.array( (0.01984925, -3.09892348, 0.23160067)), decimal=6)
def main(): """ Prepare and solve and animate a reaching task ocp """ ocp = prepare_ocp( biorbd_model_path="models/arm26_constant.bioMod", final_time=0.8, n_shooting=50, fatigue_type="effort", torque_level=1, ) # --- Solve the program --- # solver = Solver.IPOPT(show_online_optim=True) solver.set_hessian_approximation("exact") sol = ocp.solve(solver) sol.print_cost() # --- Show results --- # sol.animate(show_meshes=True)
def main(): """ Prepare, solve and animate a time minimizer ocp using a Lagrange criteria """ ocp = prepare_ocp(biorbd_model_path="models/pendulum.bioMod", final_time=2, n_shooting=50) # Let's show the objectives ocp.add_plot_penalty(CostType.OBJECTIVES) # --- Solve the program --- # sol = ocp.solve(Solver.IPOPT(show_online_optim=True)) # --- Show results --- # print( f"The optimized phase time is: {sol.parameters['time'][0, 0]}, good job Lagrange!" ) sol.animate()
def main(): """ Prepares and solves a maximal velocity at center of mass program and animates it """ model_path = "models/2segments_4dof_2contacts.bioMod" t = 0.5 ns = 20 ocp = prepare_ocp( biorbd_model_path=model_path, phase_time=t, n_shooting=ns, use_actuators=False, objective_name="MINIMIZE_COM_VELOCITY", com_constraints=True, ) # --- Solve the program --- # sol = ocp.solve(Solver.IPOPT(show_online_optim=True)) # --- Show results --- # sol.animate(n_frames=40)
def main(): """ Prepare, solve and animate a free time ocp """ time_min = 0.6 time_max = 1 ocp = prepare_ocp( biorbd_model_path="models/pendulum.bioMod", final_time=2, n_shooting=50, time_min=time_min, time_max=time_max, ) # --- Solve the program --- # sol = ocp.solve(Solver.IPOPT(show_online_optim=True)) # --- Show results --- # print(f"The optimized phase time is: {sol.parameters['time'][0, 0]}") sol.animate()
def main(): """ Defines a multiphase ocp and animate the results """ model = "../torque_driven_ocp/models/soft_contact_sphere.bioMod" ode_solver = OdeSolver.RK8() # Prepare OCP to reach the second marker ocp = prepare_ocp(model, 37, 0.37, ode_solver, slack=1e-4) ocp.add_plot_penalty(CostType.ALL) ocp.print(to_graph=True) # --- Solve the program --- # solv = Solver.IPOPT(show_online_optim=False, show_options=dict(show_bounds=True)) solv.set_linear_solver("mumps") solv.set_maximum_iterations(500) sol = ocp.solve(solv) sol.animate() sol.print() sol.graphs()