I have to find next state of my robot which can be found though solving differential equations. In MATLAB i used ode45 function and on c++ i found on internet that i have to use some method like stepper runga kutta dopri5. I tried to understand its implementation and somehow got an idea. Now my states are X,Y and theta and to find next states my differential equations are
Xdot=v*cos(theta)
Ydot=v*sin(theta)
thetadot=w
Now there is a public function stepper.do_step_impl(System,state,...etc) where what i understood is that system represents a function whose parameters could be &state,dstate and t. Now here is my problem. My differential equations have variables v and w which i need in my System function but according to my understanding System function could have only fixed parameters i.e State, dstate and t. How do i put v and w in it? I hope someone understands my question. Kindly help. Below is my code
using namespace boost::numeric::odeint;
typedef std::vector< double > state_type;
void pdot_w( const state_type &state, state_type &dstate, const double t )
{
double p_robot_xdot = v*cos(state[2]);
double p_robot_ydot = v*sin(state[2]);
double thetadot = w;
dstate[0] = p_robot_xdot;
dstate[1] = p_robot_ydot;
dstate[2] = thetadot;
}
runge_kutta_dopri5<state_type> stepper;
stepper.do_step(pdot_w, state , 0, 0.01 );
The answer depends on how you want to pass the parameters v and w to the integrator.
One approach in C++11 is to use a lambda. Your function and a call to the stepper would look like this (depending on context, the capture [] may need to be more explicit):
void pdot_w(const state_type &state,
state_type & dstate,
const double t,
const double v,
const double w) {
dstate[0] = v * cos(state[2]);
dstate[1] = v * sin(state[2]);
dstate[2] = w;
}
runge_kutta_dopri5<state_type> stepper;
/* v = 4 and w = 5 example */
stepper.do_step([](const state_type & state, state_type & d_state, const double t) {
return pdot_w(state, d_state, t, 4, 5);
}, state,
0, 0.01);
Related
I am trying to use the boost RK4 integration together with the rigid body dynamics library.
I am getting a strange error I dont recognize, that I think has to do with the operator() override but I am not sure based on this post.
My code is similar to this example. Which makes me think that either I am missing something obvious. I think that it may be a compiler problem.
This is my class I use to convert from RBDL to boost.
class rbdlToBoost {
public:
rbdlToBoost(Model* model) : model(model)
{
q = VectorNd::Zero(model->dof_count);
qd = VectorNd::Zero(model->dof_count);
qdd = VectorNd::Zero(model->dof_count);
tau = VectorNd::Zero(model->dof_count);
}
//3c. Boost uses this 'operator()' function to evaluate the state
// derivative of the pendulum.
void operator() (const state_type &x, state_type &dxdt, const double t)
{
//do stuff
}
private:
Model* model;
VectorNd q, qd, qdd, tau;
};
This is my main I am using to test the integration. This is a minimal example I put together.
#include "rbdl/Model.h"
#include "rbdl/Dynamics.h"
#include "rbdl_model_tests/DynamicTesting.h"
int main(int argc, char const *argv[])
{
int nPts = 100;
double t0 = 0;
double t1 = 3;
double t = 0; //time
double ts = 0; //scaled time
double dtsdt = M_PI/(t1-t0); //dertivative scaled time
double tp = 0;
double dt = (t1-t0)/((double)nPts);
//Integration settings
double absTolVal = 1e-10;
double relTolVal = 1e-6;
double a_x = 1.0 , a_dxdt = 1.0;
Model* model = NULL;
model = new Model();
rbdlToBoost rbdlBoostModel(model);
state_type xState(2);
int steps = 0;
xState[0] = -M_PI/4.0;
xState[1] = 0;
controlled_stepper_type controlled_stepper(
default_error_checker< double , range_algebra , default_operations >
( absTolVal , relTolVal , a_x , a_dxdt )
);
integrate_adaptive(
controlled_stepper ,
model , xState , tp , t , (t-tp)/10 );//This seems to be the problem
tp = t;
return 0;
}
I am getting this error:
~/catkin_ws/src/ambf_control_system/rbdl_model_tests/src/tempDyn.cpp:37:47: required from here
/usr/include/boost/numeric/odeint/stepper/controlled_runge_kutta.hpp:481:12: error: expression cannot be used as a function
sys( x , m_dxdt.m_v ,t );
It turns out it was a simple fix
I just had to change.
I was passing in the wrong variable into the function. I had to pass in my custom class variable (rbdlBoostModel) not my RBDL model (model).
integrate_adaptive(
controlled_stepper ,
model , xState , tp , t , (t-tp)/10 );//This seems to be the problem
to
integrate_adaptive(
controlled_stepper ,
rbdlBoostModel , xState , tp , t , (t-tp)/10 );
I am trying to implement a numerical simulation of a state space model using Eigen and Odeint. My trouble is that I need to reference control data U (predefined before integration) in order to properly solve the Ax+Bu part of the state space model. I was trying to accomplish this by using a counter to keep track of the current time step, but for whatever reason, it is reset to zero every time the System Function is called by Odeint.
How would I get around this? Is my approach to modeling the state space system flawed?
My System
struct Eigen_SS_NLTIV_Model
{
Eigen_SS_NLTIV_Model(matrixXd &ssA, matrixXd &ssB, matrixXd &ssC,
matrixXd &ssD, matrixXd &ssU, matrixXd &ssY)
:A(ssA), B(ssB), C(ssC), D(ssD), U(ssU), Y(ssY)
{
Y.resizeLike(U);
Y.setZero();
observerStep = 0;
testPtr = &observerStep;
}
/* Observer Function:*/
void operator()(matrixXd &x, double t)
{
Y.col(observerStep) = C*x + D*U.col(observerStep);
observerStep += 1;
}
/* System Function:
* ONLY the mathematical description of the system dynamics may be placed
* here. Any data placed in here is destroyed after each iteration of the
* stepper.
*/
void operator()(matrixXd &x, matrixXd &dxdt, double t)
{
dxdt = A*x + B*U.col(*testPtr);
//Cannot reference the variable "observerStep" directly as it gets reset
//every time this is called. *testPtr doesn't work either.
}
int observerStep;
int *testPtr;
matrixXd &A, &B, &C, &D, &U, &Y; //Input Vectors
};
My ODE Solver Setup
const double t_end = 3.0;
const double dt = 0.5;
int steps = (int)std::ceil(t_end / dt) + 1;
matrixXd A(2, 2), B(2, 2), C(2, 2), D(2, 2), x(2, 1);
matrixXd U = matrixXd::Constant(2, steps, 1.0);
matrixXd Y;
A << -0.5572, -0.7814, 0.7814, 0.0000;
B << 1.0, -1.0, 0.0, 2.0;
C << 1.9691, 6.4493, 1.9691, 6.4493;
D << 0.0, 0.0, 0.0, 0.0;
x << 0, 0;
Eigen_SS_NLTIV_Model matrixTest(A, B, C, D, U, Y);
odeint::integrate_const(odeint::runge_kutta4<matrixXd, double, matrixXd, double,
odeint::vector_space_algebra>(),
matrixTest, x, 0.0, t_end, dt, matrixTest);
//Ignore these two functions. They are there mostly for debugging.
writeCSV<matrixXd>(Y, "Y_OUT.csv");
prettyPrint<matrixXd>(Y, "Out Full");
With classical Runge-Kutta you know that your ODE model function is called 4 times per step with times t, t+h/2, t+h/2, t+h. With other solvers that implement adaptive step size you can not know in advance at what t the ODE model function is called.
You should implement U via some kind of interpolation function, in the most simple case as step function that computes some index from t and returns the U value for that index. Something like
i = (int)(t/U_step)
dxdt = A*x + B*U.col(i);
I am trying to numerically integrate a nonlinear system using boost::odeint. The system has time-varying parameters that are generated externally, and I want to incorporate this into my program. Is this possible with odeint? In Matlab, if you were to do something similar, you would need to interpolate the values as they become available.
Thank you in advance for your help!
Edit:
You can solve nonlinear time-varying system easily with odeint. The following example of a nonlinear time-varying system is taken from Applied Nonlinear Control by Slotine
Notice that we can insert 6sin(t) inside ode safely since we are not doing anything at each time step. If your system has a controller that depends on a time step like PID controller that requires delta time to compute derivatives, then in this case, don't put it inside the ode since ode() is called several times by the ode solver. This is my code for solving the system.
#include <iostream>
#include <boost/math/constants/constants.hpp>
#include <boost/numeric/odeint.hpp>
#include <fstream>
std::ofstream data("data.txt");
using namespace boost::numeric::odeint;
typedef std::vector< double > state_type;
class System
{
public:
System(const double& deltaT);
void updateODE();
void updateSystem();
private:
double t, dt;
runge_kutta_dopri5 < state_type > stepper;
state_type y;
void ode(const state_type &y, state_type &dy, double t);
};
System::System(const double& deltaT) : dt(deltaT), t(0.0), y(2)
{
/*
x = y[0]
dx = y[1] = dy[0]
ddx = dy[1] = ode equation
*/
// initial values
y[0] = 2.0; // x1
y[1] = 0.0; // x2
}
void System::updateODE()
{
// store data for plotting
data << t << " " << y[0] << std::endl;
//=========================================================================
using namespace std::placeholders;
stepper.do_step(std::bind(&System::ode, this, _1, _2, _3), y, t, dt);
t += dt;
}
void System::updateSystem()
{
// you can utitilize this function in case you have a controller and
// you need to update the controller at a fixed step size.
}
void System::ode(const state_type &y, state_type &dy, double t)
{
//#####################( ODE Equation )################################
dy[0] = y[1];
dy[1] = 6.0*sin(t) - 0.1*y[1] - pow(y[0],5);
}
int main(int argc, char **argv)
{
const double dt(0.001);
System sys(dt);
for (double t(0.0); t <= 50.0; t += dt){
// update time-varying parameters of the system
//sys.updateSystem();
// solve the ODE one step forward.
sys.updateODE();
}
return 0;
}
The result is (i.e. same result presented in the aforementioned book).
Yes, this is possible. But you might need to interpolate the values too. You interact with the solver via the system function. It can be functor and it can contain the a link to the data, for example
struct ode
{
void operator()( const state_type& x , state_type& dxdt , double t ) const
{
// get the the parameter from time_varying_parameters
}
std::vector<double> time_varying_parameters;
};
I'm trying to speedup my R code performing some computationally expensive task with C++ and Rcpp. My problem involves approximately a system of 100 equations, so any hint to speed up the computation is welcome.
What I need is to import a matrix MX created in R into a C++ script. The C++ script have to use rows of MX as x0 (x initial values) in a systems of ODE.
To simplify the explanation of my problem, the code below is based on the Lorenz systems.
As it is clear from the quality of my code, I'm new to C++ (and Rcpp).
For clarity, I don't post all my test code that are terrible, bu I really need your help to try solve this problem.
Any help will be really, really appreciated!
Thanks in advance.
#include <boost/array.hpp>
#include <boost/numeric/odeint.hpp>
#include <Rcpp.h>
// [[Rcpp::depends(BH)]]
// [[Rcpp::plugins(cpp11)]]
using namespace std;
using namespace boost::numeric::odeint;
double theta [] = {10.000,28,2.5};
typedef boost::array< double , 3 > state_type;
void lorenz( const state_type &x , state_type &dxdt , double t ) {
dxdt[0] = theta[0] * ( x[1] - x[0] );
dxdt[1] = theta[1] * x[0] - x[1] - x[0] * x[2];
dxdt[2] = -theta[2] * x[2] + x[0] * x[1];
}
struct foo { std::vector<double> a, b, c; };
struct foo f;
//observer should be a function that append a single output row for each input row of mx corresponding to the last integration step.
void append_lorenz(const state_type &x , const double t ) {
f.a.push_back(x[0]);
f.b.push_back(x[1]);
f.c.push_back(x[2]);
}
using namespace Rcpp;
// [[Rcpp::export]]
DataFrame callMain(NumericMatrix mx){
int n = mx.nrow();
NumericMatrix total(mx);
for(int i = 0; i < n; ++i) {
// state_type x should be mx rows
state_type x = total.row(i); // initial conditions
const double dt =0.1;
integrate(lorenz , x , 0.0 , 1.0 , dt , append_lorenz );
}
return DataFrame::create(Named("a") = f.a, Named("b") = f.b, Named("c") = f.c);
}
/*** R
mx=matrix(1:9,3,3)
res <- callMain(mx)
print((res))
*/
the error I get is:
error: conversion from ‘Rcpp::Matrix<14>::Row {aka Rcpp::MatrixRow<14>}’ to non-scalar type ‘state_type {aka boost::array}’ requested
state_type x = total.row(i); // initial conditions
I think the error message is clear enough.
state_type x = total.row(i);
There is no conversion between Rcpp object and boost::array, you need to develop your own.
I would like to use runge_kutta4 method in the odeint C++ library. I've solved the problem in Matlab. My following code in Matlab to solve x'' = -x - g*x', with initial values x1 = 1, x2 = 0, is as follows
main.m
clear all
clc
t = 0:0.1:10;
x0 = [1; 0];
[t, x] = ode45('ODESolver', t, x0);
plot(t, x(:,1));
title('Position');
xlabel('time (sec)');
ylabel('x(t)');
ODESolver.m
function dx = ODESolver(t, x)
dx = zeros(2,1);
g = 0.15;
dx(1) = x(2);
dx(2) = -x(1) - g*x(2);
end
I've installed the odeint Library. My code for using runge_kutta4 is as follows
#include <iostream>
#include <boost/numeric/odeint.hpp>
using namespace std;
using namespace boost::numeric::odeint;
/* The type of container used to hold the state vector */
typedef std::vector< double > state_type;
const double gam = 0.15;
/* The rhs of x' = f(x) */
void lorenz( const state_type &x , state_type &dx , double t )
{
dx[0] = x[1];
dx[1] = -x[0] - gam*x[1];
}
int main(int argc, char **argv)
{
const double dt = 0.1;
runge_kutta_dopri5<state_type> stepper;
state_type x(2);
x[0] = 1.0;
x[1] = 0.0;
double t = 0.0;
cout << x[0] << endl;
for ( size_t i(0); i <= 100; ++i){
stepper.do_step(lorenz, x , t, dt );
t += dt;
cout << x[0] << endl;
}
return 0;
}
The result is in the following picture
My question is why the result varies? Is there something wrong with my C++ code?
These are the first values of both methods
Matlab C++
-----------------
1.0000 0.9950
0.9950 0.9803
0.9803 0.9560
0.9560 0.9226
0.9226 0.8806
0.8806 0.8304
0.8304 0.7728
0.7728 0.7084
0.7083 0.6379
Update:
The problem is that I forgot to include the initial value in my C++ code. Thanks for #horchler for noticing it. After including the proper values and using runge_kutta_dopri5 as #horchler suggested, the result is
Matlab C++
-----------------
1.0000 1.0000
0.9950 0.9950
0.9803 0.9803
0.9560 0.9560
0.9226 0.9226
0.8806 0.8806
0.8304 0.8304
0.7728 0.7728
0.7083 0.7084
I've updated the code to reflect these modifications.
The runge_kutta4 stepper in odeint is nothing like Matlab's ode45, which is an adaptive scheme based on the Dormand-Prince method. To replicate Matlab's results, you should probably try the runge_kutta_dopri5 stepper. Also, make sure that your C++ code uses the same absolute and relative tolerances as ode45 (defaults are 1e-6 and 1e-3, respectively). Lastly, it looks like you may not be saving/printing your initial condition in your C++ results.
If you're confused at why ode45 is not taking fixed steps even though you specified t = 0:0.1:10;, see my detailed answer here.
If you must use the fixed steprunge_kutta4 stepper, then you'll need to reduce the integration step-size in your C++ code to match Matlab's output.
The Matlab ode45 function already includes error control and I think also interpolation (dense output). to compare with boost.odeint you should use the same functionality there. Boost.odeint provides integrate functions that perform step-size control and dense output if the used stepper algorithm provides this functionality. The following code piece shows how you this is used with the default error control parameters from Matlab given by horchler:
#include <boost/numeric/odeint.hpp>
using namespace std;
using namespace boost::numeric::odeint;
/* The type of container used to hold the state vector */
typedef std::vector< double > state_type;
const double gam = 0.15;
/* The rhs of x' = f(x) */
void damped_osc( const state_type &x , state_type &dx , const double t )
{
dx[0] = x[1];
dx[1] = -x[0] - gam*x[1];
}
void print( const state_type &x, const double t )
{
cout << x[0] << endl;
}
int main(int argc, char **argv)
{
cout.precision(16); // full precision output
const double dt = 0.1;
typedef runge_kutta_dopri5<state_type> stepper_type;
state_type x(2);
x[0] = 1.0;
x[1] = 0.0;
integrate_const(make_dense_output<stepper_type>( 1E-6, 1E-3 ),
damped_osc, x, 0.0, 10.0, dt , print);
return 0;
}
Please note that the results might still not be exactly the same (as in all 16 digits) because the error control in Boost.odeint might not be impemented exactly as in Matlab's ode45.