ocp_model_handler.hpp

Go to the documentation of this file.

// Copyright Institute for Automotive Engineering (ika), RWTH Aachen University
// SPDX-License-Identifier: Apache-2.0
 
#pragma once
 
#include <array>
 
// NOLINTBEGIN(clang-diagnostic-gnu-zero-variadic-macro-arguments)
 
// acados
#include <acados/utils/math.h>
#include <acados/utils/print.h>
#include <acados_c/external_function_interface.h>
#include <acados_c/ocp_nlp_interface.h>
#include <blasfeo_d_aux_ext_dep.h>  // for printing dense matrices
 
// models
#include <trajectory_optimization_ocp/acados_solver_karl.h>
#include <trajectory_optimization_ocp/acados_solver_shuttle.h>
 
namespace trajectory_optimization {
 
typedef std::variant<karl_solver_capsule*, shuttle_solver_capsule*> ocp_model_capsule_t;
 
inline ocp_model_capsule_t acados_create_capsule(const std::string& model_name) {
  if (model_name == "karl") {
    return karl_acados_create_capsule();
  } else if (model_name == "shuttle") {
    return shuttle_acados_create_capsule();
  } else {
    throw std::invalid_argument("Invalid model name: " + model_name);
  }
}
 
#define ACADOS_DISPATCH(function_name, ...)                                                    \
  if (std::holds_alternative<karl_solver_capsule*>(capsule)) {                                 \
    return karl_##function_name(std::get<karl_solver_capsule*>(capsule), ##__VA_ARGS__);       \
  } else if (std::holds_alternative<shuttle_solver_capsule*>(capsule)) {                       \
    return shuttle_##function_name(std::get<shuttle_solver_capsule*>(capsule), ##__VA_ARGS__); \
  } else {                                                                                     \
    throw std::invalid_argument("Invalid capsule type.");                                      \
  }
 
inline int acados_create(ocp_model_capsule_t capsule) { ACADOS_DISPATCH(acados_create); }
 
inline int acados_create_with_discretization(ocp_model_capsule_t capsule, int n_time_steps, double* new_time_steps) {
  ACADOS_DISPATCH(acados_create_with_discretization, n_time_steps, new_time_steps);
}
 
inline int acados_free_capsule(ocp_model_capsule_t capsule) { ACADOS_DISPATCH(acados_free_capsule); }
 
inline int acados_free(ocp_model_capsule_t capsule) { ACADOS_DISPATCH(acados_free); }
 
inline int acados_solve(ocp_model_capsule_t capsule) { ACADOS_DISPATCH(acados_solve); }
 
inline void acados_evaluate_dynamics(ocp_model_capsule_t capsule, int stage, double* x, double* u, double* x_dot) {
  std::array<ext_fun_arg_t, 2> input_types = {COLMAJ, COLMAJ};
  std::array<void*, 2> inputs = {x, u};
  std::array<ext_fun_arg_t, 1> output_types = {COLMAJ};
  std::array<void*, 1> outputs = {x_dot};
 
  external_function_external_param_casadi* function = nullptr;
 
  if (std::holds_alternative<karl_solver_capsule*>(capsule)) {
    function = &std::get<karl_solver_capsule*>(capsule)->expl_ode_fun[stage];  // NOLINT
  } else if (std::holds_alternative<shuttle_solver_capsule*>(capsule)) {
    function = &std::get<shuttle_solver_capsule*>(capsule)->expl_ode_fun[stage];  // NOLINT
  } else {
    throw std::invalid_argument("Invalid capsule type.");
  }
  function->evaluate(function, input_types.data(), inputs.data(), output_types.data(), outputs.data());
}
 
inline int acados_reset(ocp_model_capsule_t capsule,
                        int reset_qp_solver_mem,
                        int reset_numerical_values,
                        int reset_solver_options,
                        int reset_x_to_x0_bar) {
  ACADOS_DISPATCH(acados_reset, reset_qp_solver_mem, reset_numerical_values, reset_solver_options, reset_x_to_x0_bar);
}
 
inline int acados_update_params_sparse(ocp_model_capsule_t capsule, int stage, int* idx, double* p, int n_update) {
  ACADOS_DISPATCH(acados_update_params_sparse, stage, idx, p, n_update);
}
 
inline int acados_set_p_global_and_precompute_dependencies(ocp_model_capsule_t capsule, double* data, int data_len) {
  ACADOS_DISPATCH(acados_set_p_global_and_precompute_dependencies, data, data_len);
}
 
inline void acados_print_stats(ocp_model_capsule_t capsule) { ACADOS_DISPATCH(acados_print_stats); }
 
inline ocp_nlp_in* acados_get_nlp_in(ocp_model_capsule_t capsule) { ACADOS_DISPATCH(acados_get_nlp_in); }
 
inline ocp_nlp_out* acados_get_nlp_out(ocp_model_capsule_t capsule) { ACADOS_DISPATCH(acados_get_nlp_out); }
 
inline ocp_nlp_solver* acados_get_nlp_solver(ocp_model_capsule_t capsule) { ACADOS_DISPATCH(acados_get_nlp_solver); }
 
inline ocp_nlp_config* acados_get_nlp_config(ocp_model_capsule_t capsule) { ACADOS_DISPATCH(acados_get_nlp_config); }
 
inline void* acados_get_nlp_opts(ocp_model_capsule_t capsule) { ACADOS_DISPATCH(acados_get_nlp_opts); }
 
inline ocp_nlp_dims* acados_get_nlp_dims(ocp_model_capsule_t capsule) { ACADOS_DISPATCH(acados_get_nlp_dims); }
 
}  // namespace trajectory_optimization
 
// NOLINTEND(clang-diagnostic-gnu-zero-variadic-macro-arguments)