Skip to content
Permalink
Browse files

minimal integrator implementation and wrapper

  • Loading branch information...
Tobias Schoels
Tobias Schoels committed Oct 19, 2018
1 parent 3ea7d41 commit e90a2a4180dd1626a77f653d985570aa80f532ed
Showing with 44 additions and 30 deletions.
  1. +9 −10 examples/python/integrator_test.py
  2. +22 −11 interfaces/acados_cpp/integrator.cpp
  3. +12 −9 interfaces/acados_cpp/integrator.hpp
  4. +1 −0 swig/acados.i
@@ -33,7 +33,7 @@
# # alternative with dict
# model = {
# "x": x,
# "xdot": xdot
# "xdot": xdot
# }
# print(model)

@@ -46,20 +46,19 @@

# end_time = time.time() # stop timer

le_sim = integrator( ode_fun )
opts = {'step': 0.1}
le_sim = integrator(ode_fun, opts)

print(nx)
<<<<<<< HEAD
print(nu)
x0 = np.array([ 0, 0, 0, 0])
x0 = np.array([ 4.2, 0.42, 0, 0])
u0 = np.array([1])
xn = le_sim.integrate(x0, u0)
for k in range(100):
xn = le_sim.integrate(x0, u0)
le_sim.set_step(le_sim.step()*0.99)
print(xn)
x0 = np.array(xn)

print(xn)
=======
x0 = [ 0, 0, 0, 0]
le_sim.integrate(x0)
>>>>>>> 5e74636776107582576685dd6f926c5a345d3a88

## Define opts
start_time = time.time() # start timer
@@ -3,7 +3,6 @@

#include <algorithm>

#include "acados_c/sim_interface.h"
#include "acados_cpp/ocp_nlp/function_generation.hpp"


@@ -51,7 +50,11 @@ integrator::integrator(const casadi::Function &model, std::map<std::string, opti
else
sim_plan.sim_solver = ERK;

// todo add check: modeltype and integrator type consistent
if (!options.count("step")) throw std::invalid_argument("Expected 'step' as an option.");

// TODO set step

// TODO add check: modeltype and integrator type consistent

config_ = sim_config_create(sim_plan);

@@ -93,10 +96,14 @@ integrator::integrator(const casadi::Function &model, std::map<std::string, opti
in_ = sim_in_create(config_, dims_);
out_ = sim_out_create(config_, dims_);

// set step width
set_step(to_double(options.at("step")));

// TODO: generate and set model;
// use external_function_generic stuff
//std::map<std::string, option_t *> model_options = {};
//if (options.count("model_type")) model_options[model_type] = to_int(options.at("model_type"));
// std::map<std::string, option_t *> model_options = {};
// if (options.count("model_type")) model_options[model_type] =
// to_int(options.at("model_type"));
integrator::set_model(model, options);

solver_ = sim_create(config_, dims_, opts_);
@@ -107,25 +114,27 @@ void integrator::set_model(const casadi::Function &model, std::map<std::string,
{
if (options.count("model_type")) model_type_ = (model_t) to_int(options.at("model_type"));

if (model_type_ == GNSF){
if (model_type_ == GNSF)
{
throw std::invalid_argument("Not supported model type.");
}
else if (model_type_ == IMPLICIT)
{
throw std::invalid_argument("Not supported model type.");
}
else // explicit default
else // explicit default
{
module_["expl_vde_for"] = generate_forward_vde(model);

int status = sim_set_model_internal(config_, in_->model,
"expl_vde_for", (void *) module_["expl_vde_for"].as_external_function());
sim_set_model_internal(config_, in_->model, "expl_vde_for",
(void *) module_["expl_vde_for"].as_external_function());
}


}


void integrator::set_step(const double step) { in_->T = step; }


std::vector<double> integrator::integrate(std::vector<double> x, std::vector<double> u)
{
/*
@@ -157,7 +166,8 @@ std::vector<double> integrator::integrate(std::vector<double> x, std::vector<dou
*/

// cast in/out?!
int acados_return = sim_solve(solver_, in_, out_);
// int acados_return = sim_solve(solver_, in_, out_);
sim_solve(solver_, in_, out_);

std::vector<double> xn(out_->xn, out_->xn + nx_);

@@ -169,6 +179,7 @@ std::vector<double> integrator::integrate(std::vector<double> x, std::vector<dou
*/

return xn;
// return std::vector<double>(out_->xn, out_->xn + nx_);
}


@@ -1,30 +1,32 @@

#ifndef INTERFACES_ACADOS_CPP_INTEGRATOR_HPP_
#define INTERFACES_ACADOS_CPP_INTEGRATOR_HPP_

#include <map>
#include <string>
#include <vector>

//#include "casadi/casadi.hpp"

#include "acados/utils/types.h"
#include "acados_c/ocp_nlp_interface.h"
//#include "acados_c/ocp_nlp_interface.h"
#include "acados_c/sim_interface.h"

#include "acados_cpp/ocp.hpp"
#include "acados_cpp/ocp_nlp/ocp_nlp_solution.hpp"
//#include "acados_cpp/ocp.hpp"
//#include "acados_cpp/ocp_nlp/ocp_nlp_solution.hpp"
#include "acados_cpp/ocp_nlp/casadi_module.hpp"
#include "acados_cpp/options.hpp"

// TODO(oj): try to only use C interface, i.e. remove line above and use void pointers
// @tobi: or what do u think?
namespace acados
{

typedef enum
{
EXPLICIT=0,
// TODO do we really need this (see definition of sim_solver_t)
EXPLICIT = 0,
IMPLICIT,
GNSF
} model_t;
} model_t;


class integrator
@@ -34,10 +36,12 @@ class integrator

std::vector<double> integrate(std::vector<double> x, std::vector<double> u = {});

void set_model(const casadi::Function &model, std::map<std::string, option_t *> options = {});
void set_model(const casadi::Function& model, std::map<std::string, option_t*> options = {});

void set_step(const double step);

int num_stages() { return opts_->ns; }
double step() { return in_->T; }

~integrator();

@@ -55,7 +59,6 @@ class integrator
model_t model_type_;

std::map<std::string, casadi_module> module_;

};

} // namespace acados
@@ -40,6 +40,7 @@ typedef PyObject LangObject;
%include "std_vector.i"
namespace std {
%template(vector_i) vector<int>;
%template(vector_d) vector<double>;
%template(vector_string) vector<string>;
%template(vector_O) vector<LangObject *>;
};

0 comments on commit e90a2a4

Please sign in to comment.
You can’t perform that action at this time.