An example, including subclassing RigidBody and using Runge-Kutta integration to calculate the updates, is given here
#include <iostream>
{
public:
double tau_e;
double dt;
PulsedBody(double dt);
~PulsedBody();
void step();
};
PulsedBody::PulsedBody(double dt) :
RigidBody(100.0, 5.0, 6.0, 9.0, 0.0, 0.0, 0.0, 1),
tau_e(1.2),
dt(dt),
force(Vector::Zero(3)),
fpoint(Vector::Zero(3)),
gravity(Vector::Zero(3)),
workspace(14)
{
fpoint[2] = -1.0;
fpoint[0] = 1e-8;
gravity[2] = 9.810665;
}
PulsedBody::~PulsedBody()
{
}
void PulsedBody::derivative(
VectorE& xd,
double dt_offset)
{
xd[13] = (1330.0 - x[13]) / tau_e;
force.setZero();
force[2] = -x[13];
return;
}
void PulsedBody::step()
{
std::cout <<
X() << std::endl;
std::cout <<
"psi=" <<
psi() <<
" theta=" <<
theta() <<
" phi=" <<
phi()
<<
" V=" <<
V() <<
" alpha=" <<
alpha() <<
" beta=" <<
beta() << std::endl;
std::cout << sp << std::endl;
}
int main()
{
PulsedBody pb(0.1);
for (int ii = 1000; ii--; ) {
pb.step();
}
return 0;
}