text: """
  This example is a trivial box that tries to superimpose one of its corner to a marker at the beginning of the movement
  and superimpose the same corner to a different marker at the end.
  It is designed to show how one can define its own custom objective function if the provided ones are not
  sufficient.
  
  More specifically this example reproduces the behavior of the Mayer.SUPERIMPOSE_MARKERS objective function.
  """
  
  import biorbd_casadi as biorbd
  from casadi import MX
  from bioptim import (
      Node,
      OptimalControlProgram,
      Dynamics,
      DynamicsFcn,
      ObjectiveFcn,
      ObjectiveList,
      Bounds,
      QAndQDotBounds,
      InitialGuess,
      OdeSolver,
      PenaltyNodeList,
      BiorbdInterface,
      Solver,
  )
  
  
  def custom_func_track_markers(all_pn: PenaltyNodeList, first_marker: str, second_marker: str, method: int) -> MX:
      """
      The used-defined objective function (This particular one mimics the ObjectiveFcn.SUPERIMPOSE_MARKERS)
      Except for the last two
  
      Parameters
      ----------
      all_pn: PenaltyNodeList
          The penalty node elements
      first_marker: str
          The index of the first marker in the bioMod
      second_marker: str
          The index of the second marker in the bioMod
      method: int
          Two identical ways are shown to help the new user to navigate the biorbd API
  
      Returns
      -------
      The cost that should be minimize in the MX format. If the cost is quadratic, do not put
      the square here, but use the quadratic=True parameter instead
      """
  
      # Get the index of the markers from their name
      marker_0_idx = biorbd.marker_index(all_pn.nlp.model, first_marker)
      marker_1_idx = biorbd.marker_index(all_pn.nlp.model, second_marker)
  
      if method == 0:
          # Convert the function to the required format and then subtract
          markers = BiorbdInterface.mx_to_cx("markers", all_pn.nlp.model.markers, all_pn.nlp.states["q"])
          markers_diff = markers[:, marker_1_idx] - markers[:, marker_0_idx]
  
      else:
          # Do the calculation in biorbd API and then convert to the required format
          markers = all_pn.nlp.model.markers(all_pn.nlp.states["q"].mx)
          markers_diff = markers[marker_1_idx].to_mx() - markers[marker_0_idx].to_mx()
          markers_diff = BiorbdInterface.mx_to_cx("markers", markers_diff, all_pn.nlp.states["q"])
  
      return markers_diff
  
  
  def prepare_ocp(biorbd_model_path, ode_solver=OdeSolver.RK4()) -> OptimalControlProgram:
      """
      Prepare the program
  
      Parameters
      ----------
      biorbd_model_path: str
          The path of the biorbd model
      ode_solver: OdeSolver
          The type of ode solver used
  
      Returns
      -------
      The ocp ready to be solved
      """
  
      # --- Options --- #
      # Model path
      biorbd_model = biorbd.Model(biorbd_model_path)
  
      # Problem parameters
      n_shooting = 30
      final_time = 2
      tau_min, tau_max, tau_init = -100, 100, 0
  
      # Add objective functions
      objective_functions = ObjectiveList()
      objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau")
      objective_functions.add(
          custom_func_track_markers,
          custom_type=ObjectiveFcn.Mayer,
          node=Node.START,
          quadratic=True,
          first_marker="m0",
          second_marker="m1",
          weight=1000,
          method=0,
      )
      objective_functions.add(
          custom_func_track_markers,
          custom_type=ObjectiveFcn.Mayer,
          node=Node.END,
          quadratic=True,
          first_marker="m0",
          second_marker="m2",
          weight=1000,
          method=1,
      )
  
      # Dynamics
      dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN)
  
      # Path constraint
      x_bounds = QAndQDotBounds(biorbd_model)
      x_bounds[1:6, [0, -1]] = 0
      x_bounds[2, -1] = 1.57
  
      # Initial guess
      x_init = InitialGuess([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()))
  
      # Define control path constraint
      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,
          final_time,
          x_init,
          u_init,
          x_bounds,
          u_bounds,
          objective_functions,
          ode_solver=ode_solver,
      )
  
  
  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()
  
  
  if __name__ == "__main__":
      main()
  