DUECA/DUSIME
Loading...
Searching...
No Matches
PulsedBody.cxx

An example, including subclassing RigidBody and using Runge-Kutta integration to calculate the updates, is given here.

An example, including subclassing RigidBody and using Runge-Kutta integration to calculate the updates, is given here

/* ------------------------------------------------------------------ */
/* item : PulsedBody.cxx
made by : Rene van Paassen
date : 060502
category : header file
description :
changes : 060502 first version
language : C++
copyright : (c) 2016 TUDelft-AE-C&S
copyright : (c) 2022 René van Paassen
license : EUPL-1.2
*/
#include <RigidBody.hxx>
#include <iostream>
/** This is an example, using the RigidBody class as basis for
implementation of equations of motion.
The PulsedBody class derives from RigidBody, and applies forces on
the RigidBody object. Using a Runge Kutta integration, the states
of the RigidBody object are integrated numerically. */
class PulsedBody: public RigidBody
{
public:
/** Time constant simulating the "engine" dynamics. */
double tau_e;
/** Size of time steps. */
double dt;
/** For efficiency, auxiliary variable that represents a force
vector. Note that RigidBody uses eigen3
http://eigen.tuxfamily.org/ for matrix and vector
operations. Vector is a typedef in the RigidBody header file,
and defines a shortcut to the vector class used here. */
Vector force;
/** Auxiliary variable, vector giving the point where the force is
applied. */
Vector fpoint;
/** Auxiliary variable, vector giving gravity. */
Vector gravity;
/** Workspace for the RungeKutta integration */
/** Constructor.
@param dt Time step of the update. */
PulsedBody(double dt);
/** Destructor. */
~PulsedBody();
/** Function needed for integration. This function will be called by
the integrate_rungekutta() function. It calculates the
derivative of the state vector.
@param xd Derivative of the state vector
@param dt_offset Time offset for calculating the
derivative. The integration routine might
need the derivative of the state for time
steps that are a fraction of the integration
time step, e.g. for 0dt, 0.5dt and dt after
the previous integration step. */
void derivative(VectorE& xd, double dt_offset);
/** Function that does an integration step */
void step();
};
PulsedBody::PulsedBody(double dt) :
// create a RigidBody, with mass and inertia. Request one additional
// state for the engine.
RigidBody(100.0, 5.0, 6.0, 9.0, 0.0, 0.0, 0.0, 1),
// time constant of the engine, 1.2 s
tau_e(1.2),
// time step
dt(dt),
// force vector is initially zero, 3 elements.
force(Vector::Zero(3)),
// same for the point on the body
fpoint(Vector::Zero(3)),
// vector for the gravitational pull
gravity(Vector::Zero(3)),
// workspace integration, 13 states for body, 1 for me
workspace(14)
{
// modify point where forces are applied, index 2 is z-coordinate
// there is a slight offset in x coordinate
fpoint[2] = -1.0;
fpoint[0] = 1e-8;
// in z-direction
gravity[2] = 9.810665;
// with this, construction of the PulsedBody is complete.
}
PulsedBody::~PulsedBody()
{
// nothing to delete
}
void PulsedBody::derivative(VectorE& xd, double dt_offset)
{
// we added one state, engine thrust. The engine thrust is state
// variable x[13]. Calculate the derivative, using the time
// constant, assume we have a constant setpoint of 1330 N for the thrust:
xd[13] = (1330.0 - x[13]) / tau_e;
// now, all other motions are calculated by the RigidBody parent.
// use this to clear all forces from the previous step
zeroForces();
// apply the thrust to the body
force.setZero(); // for clarity, all elements of force vector
// are zeroed here.
force[2] = -x[13]; // force in z direction = thruster state
applyBodyForce(force, fpoint); // apply force on the body
addInertialGravity(gravity); // add gravity field
// if we now call derivative from the RigidBody parent, the
// remaining 13 states (0 -- 12) are calculated. Expressly call the
// parent method, otherwise we end up calling ourselves!
// at this point, the derivative of the state vector has been put in
// xd. Return to the calling function, most likely the integration
// routine.
return;
}
void PulsedBody::step()
{
// use Runge-kutta integration on myself
integrate_rungekutta(*this, workspace, dt);
// just to prove that we did something
std::cout << X() << std::endl;
// after a call to output, things like Psi, theta and phi should be
// calculated
output();
std::cout << "psi=" << psi() << " theta=" << theta() << " phi=" << phi()
<< " V=" << V() << " alpha=" << alpha() << " beta=" << beta() << std::endl;
// specific forces. Note that it is really inefficient (especially
// for a real-time program) to be creating and removing vector sp
// for each step.
Vector sp(6);
specific(sp);
std::cout << sp << std::endl;
}
int main()
{
// main function
PulsedBody pb(0.1);
for (int ii = 1000; ii--; ) {
pb.step();
}
return 0;
}
Eigen::VectorXd Vector
a normal vector, allocates its own storage
Definition AxisTransforms.hxx:37
Helper class for implementing rigid body vehicle simulation.
Eigen::Map< Eigen::VectorXd > VectorE
a vector that takes external storage
Definition RigidBody.hxx:49
Rigid body dynamics function, calculates derivative of a rigid body, given sum of moments and forces ...
Definition RigidBody.hxx:108
void derivative(VectorE &xd, double unused=0.0)
Obtain and calculate the derivative, given sum of forces and moments and the gravitational field acti...
This defines a "data-pack", with room for workspace for the Runge-Kutta integration.
Definition integrate_rungekutta.hxx:38
Here a template function for Runge-Kutta integration is defined.
void integrate_rungekutta(MOD &model, RungeKuttaWorkspace &ws, double dt)
This function applies one Runge Kutta integration step to the state given in the kinematics argument.
Definition integrate_rungekutta.hxx:101