# --- Solve the program --- # tic = time.time() sol, sol_iterations = ocp.solve( show_online_optim=True, return_iterations=True, solver_options={ "tol": 1e-4, "max_iter": 3000, "ipopt.hessian_approximation": "limited-memory" }, ) toc = time.time() - tic print(f"Time to solve : {toc}sec") t = time.localtime(time.time()) date = f"{t.tm_year}_{t.tm_mon}_{t.tm_mday}" OptimalControlProgram.save(ocp, sol, f"results/{date}_xiaModel.bo") OptimalControlProgram.save_get_data(ocp, sol, f"results/{date}_xiaModel.bob", sol_iterations=sol_iterations) OptimalControlProgram.save_get_data( ocp, sol, f"results/{date}_xiaModel_interpolate.bob", interpolate_nb_frames=100) # --- Show results --- # result = ShowResult(ocp, sol) result.graphs()
class ViolinOcp: # TODO Get these values from a better method tau_min, tau_max, tau_init = -100, 100, 0 # TODO add external forces? # TODO Warm starting when updating the objective_bow_target # TODO All the logic from NMPC # TODO include the muscle fatigue dynamics, constraints and objectives # dynamics.add(xia.xia_model_configuration, dynamic_function=xia.xia_model_dynamic) 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 _set_generic_objective_functions(self): # Regularization objectives self.objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE, weight=0.01, list_index=0) if self.use_muscles: self.objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, index=self.violin.virtual_tau, weight=0.01, list_index=1) self.objective_functions.add( ObjectiveFcn.Lagrange.MINIMIZE_MUSCLES_CONTROL, weight=10, list_index=2) else: self.objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=0.01, list_index=1) self.objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_QDDOT, weight=0.01, list_index=3) # Keep the bow align at 90 degrees with the violin self.objective_functions.add( ObjectiveFcn.Lagrange.TRACK_SEGMENT_WITH_CUSTOM_RT, weight=1000, segment_idx=self.bow.segment_idx, rt_idx=self.violin.rt_on_string, list_index=4) def _set_generic_constraints(self): # Keep the bow in contact with the violin if self.solver == Solver.IPOPT: self.constraints.add( ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.ALL, first_marker_idx=self.bow.contact_marker, second_marker_idx=self.violin.bridge_marker, ) else: self.objective_functions.add( ObjectiveFcn.Lagrange.SUPERIMPOSE_MARKERS, node=Node.ALL, first_marker_idx=self.bow.contact_marker, second_marker_idx=self.violin.bridge_marker, list_index=6, weight=1000, ) # Keep the bow in contact with the violin, but allow for prediction error # for j in range(1, 5): # constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, # node=j, # min_bound=0, # max_bound=0, # first_marker_idx=Bow.contact_marker, # second_marker_idx=violin.bridge_marker, list_index=j) # for j in range(5, nb_shooting + 1): # constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, # node=j, # min_bound=-10**(j-14), #-10**(j-14) gives 25 iterations # max_bound=10**(j-14), # (j-4)/10 gives 21 iterations # first_marker_idx=Bow.contact_marker, # second_marker_idx=violin.bridge_marker, list_index=j) # if self.use_muscles: # if self.n_cycles >= 3: # self.constraints.add( # ConstraintFcn.TRACK_TORQUE, # index=self.violin.virtual_tau, # min_bound=-3, # max_bound=3, # node=list(range(self.n_shooting_per_cycle, self.n_shooting - self.n_shooting_per_cycle + 1)), # ) # else: # self.constraints.add( # ConstraintFcn.TRACK_TORQUE, # index=self.violin.virtual_tau, # min_bound=-15, # max_bound=15, # node=Node.ALL, # ) def _set_bounds(self): self.x_bounds = QAndQDotBounds(self.model) self.x_bounds[:self.n_q, 0] = self.violin.q(self.bow_starting) self.x_bounds[self.n_q:, 0] = 0 # self.x_bounds.min[:self.n_q, -1] = np.array(self.violin.q(self.bow_starting)) - 0.01 # self.x_bounds.max[:self.n_q, -1] = np.array(self.violin.q(self.bow_starting)) + 0.01 u_min = [self.tau_min] * self.n_tau + [0] * self.n_mus u_max = [self.tau_max] * self.n_tau + [1] * self.n_mus self.u_bounds = Bounds(u_min, u_max) def _set_initial_guess(self, init_file): if init_file is None: x_init = np.zeros((self.n_q * 2, 1)) x_init[:self.n_q, 0] = self.violin.q(self.bow_starting) u_init = np.zeros((self.n_tau + self.n_mus, 1)) self.x_init = InitialGuess(x_init) self.u_init = InitialGuess(u_init) else: _, sol = ViolinOcp.load(init_file) self.x_init = InitialGuess( sol.states["all"], interpolation=InterpolationType.EACH_FRAME) self.u_init = InitialGuess( sol.controls["all"][:, :-1], interpolation=InterpolationType.EACH_FRAME) def set_bow_target_objective(self, bow_target: np.ndarray, weight: float = 10000, sol: Solution = None): new_objectives = Objective( ObjectiveFcn.Lagrange.TRACK_STATE, node=Node.ALL, weight=weight, target=bow_target, index=self.bow.hair_idx, list_index=5, ) self.ocp.update_objectives(new_objectives) def _set_generic_ocp(self): self.ocp = OptimalControlProgram( biorbd_model=self.model, dynamics=self.dynamics, n_shooting=self.n_shooting, phase_time=self.time, x_init=self.x_init, u_init=self.u_init, x_bounds=self.x_bounds, u_bounds=self.u_bounds, objective_functions=self.objective_functions, constraints=self.constraints, use_sx=self.solver == Solver.ACADOS, n_threads=self.n_threads, ) def solve(self, **opts: Any) -> Solution: return self.ocp.solve(solver=self.solver, **opts) @staticmethod def load(file_path: str): return MovingHorizonEstimator.load(file_path) def save(self, sol: Solution, stand_alone: bool = False): try: os.mkdir("results") except FileExistsError: pass t = time.localtime(time.time()) if stand_alone: self.ocp.save(sol, f"results/{t.tm_year}_{t.tm_mon}_{t.tm_mday}_out.bo", stand_alone=True) else: self.ocp.save(sol, f"results/{t.tm_year}_{t.tm_mon}_{t.tm_mday}.bo", stand_alone=False)