mirror of
https://github.com/nasa/trick.git
synced 2025-06-23 09:15:29 +00:00
Merge in the er7_utils integrators
Taking in the latest er7_utils directory and adding it into Trick in the same location it was as an external repository. Made one change to the files_to_ICG.hh file in the repository to remove the CheckpointHelper header files. Those go in the Trick files_to_ICG.hh file. refs #180
This commit is contained in:
Alex Lin
@ -0,0 +1,78 @@
|
||||
/**
|
||||
* @if Er7UtilsUseGroups
|
||||
* @addtogroup Er7Utils
|
||||
* @{
|
||||
* @addtogroup Integration
|
||||
* @{
|
||||
* @endif
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file
|
||||
* Defines Butcher tableau for Runge Kutta Fehlberg 4/5.
|
||||
*/
|
||||
|
||||
/*
|
||||
Purpose: ()
|
||||
*/
|
||||
|
||||
|
||||
// Model includes
|
||||
#include "../include/rkf45_butcher_tableau.hh"
|
||||
|
||||
|
||||
namespace er7_utils {
|
||||
|
||||
// Runge Kutta Fehlberg 4/5 Butcher tableau 'a' elements
|
||||
const double RKFehlberg45ButcherTableau::RKa[6][6] = {
|
||||
|
||||
// a[0] = all zeros, and is not used.
|
||||
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
|
||||
|
||||
// a[1]: 1 element, sum = 1.0/4.0
|
||||
{1.0/4.0,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0},
|
||||
|
||||
// a[2]: 2 elements, sum = 3.0/8.0
|
||||
{3.0/32.0, 9.0/32.0,
|
||||
0.0, 0.0, 0.0, 0.0},
|
||||
|
||||
// a[3]: 3 elements, sum = 12.0/13.0
|
||||
{1932.0/2197.0, -7200.0/2197.0, 7296.0/2197.0,
|
||||
0.0, 0.0, 0.0},
|
||||
|
||||
// a[4]: 4 elements, sum = 1.0
|
||||
{439.0/216.0, -8.0, 3680.0/513.0, -845.0/4104.0,
|
||||
0.0, 0.0},
|
||||
|
||||
// a[5]: 5 elements, sum = 0.5
|
||||
{-8.0/27.0, 2.0, -3544.0/2565.0, 1859.0/4104.0, -11.0/40.0,
|
||||
0.0}
|
||||
};
|
||||
|
||||
|
||||
// Runge Kutta Fehlberg 4/5 Butcher tableau fifth order 'b' elements
|
||||
const double RKFehlberg45ButcherTableau::RKb5[6] = {
|
||||
16.0/135.0, 0.0, 6656.0/12825.0, 28561.0/56430.0, -9.0/50.0, 2.0/55.0
|
||||
};
|
||||
|
||||
|
||||
// Runge Kutta Fehlberg 4/5 Butcher tableau fourth order 'b' elements
|
||||
const double RKFehlberg45ButcherTableau::RKb4[6] = {
|
||||
25.0/216.0, 0.0, 1408.0/2565.0, 2197.0/4104.0, -1.0/5.0, 0.0
|
||||
};
|
||||
|
||||
|
||||
// Runge Kutta Fehlberg 7/8 Butcher tableau 'c' elements
|
||||
const double RKFehlberg45ButcherTableau::RKc[6] = {
|
||||
0.0, 1.0/4.0, 3.0/8.0, 12.0/13.0, 1.0, 1.0/2.0
|
||||
};
|
||||
|
||||
|
||||
}
|
||||
/**
|
||||
* @if Er7UtilsUseGroups
|
||||
* @}
|
||||
* @}
|
||||
* @endif
|
||||
*/
|
||||
@ -0,0 +1,200 @@
|
||||
/**
|
||||
* @if Er7UtilsUseGroups
|
||||
* @addtogroup Er7Utils
|
||||
* @{
|
||||
* @addtogroup Integration
|
||||
* @{
|
||||
* @endif
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file
|
||||
* Defines member functions for the class RKFehlberg45FirstOrderODEIntegrator.
|
||||
*/
|
||||
|
||||
/*
|
||||
Purpose: ()
|
||||
*/
|
||||
|
||||
|
||||
// System includes
|
||||
|
||||
// Interface includes
|
||||
#include "er7_utils/interface/include/alloc.hh"
|
||||
|
||||
// Integration includes
|
||||
#include "er7_utils/integration/core/include/integ_utils.hh"
|
||||
#include "er7_utils/integration/core/include/rk_utils.hh"
|
||||
|
||||
// Model includes
|
||||
#include "../include/rkf45_first_order_ode_integrator.hh"
|
||||
#include "../include/rkf45_butcher_tableau.hh"
|
||||
|
||||
|
||||
namespace er7_utils {
|
||||
|
||||
// RK4FirstOrderODEIntegrator default constructor.
|
||||
RKFehlberg45FirstOrderODEIntegrator::RKFehlberg45FirstOrderODEIntegrator (
|
||||
void)
|
||||
:
|
||||
Er7UtilsDeletable (),
|
||||
FirstOrderODEIntegrator (),
|
||||
init_state (NULL)
|
||||
{
|
||||
alloc::initialize_2D_array<6> (deriv_hist);
|
||||
}
|
||||
|
||||
|
||||
// RK4FirstOrderODEIntegrator copy constructor.
|
||||
RKFehlberg45FirstOrderODEIntegrator::RKFehlberg45FirstOrderODEIntegrator (
|
||||
const RKFehlberg45FirstOrderODEIntegrator & src)
|
||||
:
|
||||
Er7UtilsDeletable (),
|
||||
FirstOrderODEIntegrator (src),
|
||||
init_state (NULL)
|
||||
{
|
||||
if (src.init_state != NULL) {
|
||||
init_state = alloc::replicate_array (state_size, src.init_state);
|
||||
alloc::replicate_2D_array<6> (state_size, src.deriv_hist, deriv_hist);
|
||||
}
|
||||
else {
|
||||
alloc::initialize_2D_array<6> (deriv_hist);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// RK4FirstOrderODEIntegrator non-default constructor.
|
||||
RKFehlberg45FirstOrderODEIntegrator::RKFehlberg45FirstOrderODEIntegrator (
|
||||
unsigned int size,
|
||||
IntegrationControls & controls)
|
||||
:
|
||||
Er7UtilsDeletable (),
|
||||
FirstOrderODEIntegrator (size, controls),
|
||||
init_state (NULL)
|
||||
{
|
||||
|
||||
// Allocate memory used by Runge Kutta Fehlberg 4/5 algorithm.
|
||||
init_state = alloc::allocate_array (state_size);
|
||||
alloc::allocate_2D_array<6> (state_size, deriv_hist);
|
||||
}
|
||||
|
||||
|
||||
// RKFehlberg45FirstOrderODEIntegrator destructor.
|
||||
RKFehlberg45FirstOrderODEIntegrator::~RKFehlberg45FirstOrderODEIntegrator (
|
||||
void)
|
||||
{
|
||||
alloc::deallocate_array (init_state);
|
||||
alloc::deallocate_2D_array<6> (deriv_hist);
|
||||
}
|
||||
|
||||
|
||||
// Clone a RKFehlberg45FirstOrderODEIntegrator.
|
||||
RKFehlberg45FirstOrderODEIntegrator *
|
||||
RKFehlberg45FirstOrderODEIntegrator::create_copy ()
|
||||
const
|
||||
{
|
||||
return alloc::replicate_object (*this);
|
||||
}
|
||||
|
||||
|
||||
// Non-throwing swap.
|
||||
void
|
||||
RKFehlberg45FirstOrderODEIntegrator::swap (
|
||||
RKFehlberg45FirstOrderODEIntegrator & other)
|
||||
{
|
||||
FirstOrderODEIntegrator::swap (other);
|
||||
|
||||
std::swap (init_state, other.init_state);
|
||||
for (int ii = 0; ii < 6; ++ii) {
|
||||
std::swap (deriv_hist[ii], other.deriv_hist[ii]);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
// Propagate state via Runge Kutta Fehlberg 4/5 integration scheme.
|
||||
IntegratorResult
|
||||
RKFehlberg45FirstOrderODEIntegrator::integrate (
|
||||
double dt,
|
||||
unsigned int target_stage,
|
||||
const double * ER7_UTILS_RESTRICT velocity,
|
||||
double * ER7_UTILS_RESTRICT position)
|
||||
{
|
||||
double step_factor;
|
||||
|
||||
// Integrate to the next stage
|
||||
switch (target_stage) {
|
||||
|
||||
// Initial stage (stage 1):
|
||||
// Save initial state and update per RKF45 RKa[1] (one element).
|
||||
case 1:
|
||||
integ_utils::inplace_euler_step_save_both (
|
||||
velocity,
|
||||
RKFehlberg45ButcherTableau::RKa[1][0]*dt, state_size,
|
||||
init_state, deriv_hist[0], position);
|
||||
step_factor = RKFehlberg45ButcherTableau::RKc[1];
|
||||
break;
|
||||
|
||||
// Intermediate stages (2 to 5):
|
||||
// Update state per RKF45 RKa[target_stage] (target_stage elements).
|
||||
case 2:
|
||||
integ_utils::weighted_step_save_deriv<2> (
|
||||
init_state, velocity,
|
||||
RKFehlberg45ButcherTableau::RKa[2],
|
||||
dt, state_size,
|
||||
deriv_hist, position);
|
||||
step_factor = RKFehlberg45ButcherTableau::RKc[2];
|
||||
break;
|
||||
|
||||
case 3:
|
||||
integ_utils::weighted_step_save_deriv<3> (
|
||||
init_state, velocity,
|
||||
RKFehlberg45ButcherTableau::RKa[3],
|
||||
dt, state_size,
|
||||
deriv_hist, position);
|
||||
step_factor = RKFehlberg45ButcherTableau::RKc[3];
|
||||
break;
|
||||
|
||||
case 4:
|
||||
integ_utils::weighted_step_save_deriv<4> (
|
||||
init_state, velocity,
|
||||
RKFehlberg45ButcherTableau::RKa[4],
|
||||
dt, state_size,
|
||||
deriv_hist, position);
|
||||
step_factor = RKFehlberg45ButcherTableau::RKc[4];
|
||||
break;
|
||||
|
||||
case 5:
|
||||
integ_utils::weighted_step_save_deriv<5> (
|
||||
init_state, velocity,
|
||||
RKFehlberg45ButcherTableau::RKa[5],
|
||||
dt, state_size,
|
||||
deriv_hist, position);
|
||||
step_factor = RKFehlberg45ButcherTableau::RKc[5];
|
||||
break;
|
||||
|
||||
// Final stage (6):
|
||||
// Update state per RKF45 RKb5 (6 elements).
|
||||
case 6:
|
||||
integ_utils::weighted_step<6> (
|
||||
init_state, velocity, deriv_hist,
|
||||
RKFehlberg45ButcherTableau::RKb5, dt, state_size,
|
||||
position);
|
||||
step_factor = RKFehlberg45ButcherTableau::RKc[5];
|
||||
break;
|
||||
|
||||
default:
|
||||
step_factor = 1.0;
|
||||
break;
|
||||
}
|
||||
|
||||
return step_factor;
|
||||
}
|
||||
|
||||
}
|
||||
/**
|
||||
* @if Er7UtilsUseGroups
|
||||
* @}
|
||||
* @}
|
||||
* @endif
|
||||
*/
|
||||
@ -0,0 +1,129 @@
|
||||
/**
|
||||
* @if Er7UtilsUseGroups
|
||||
* @addtogroup Er7Utils
|
||||
* @{
|
||||
* @addtogroup Integration
|
||||
* @{
|
||||
* @endif
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file
|
||||
* Defines member functions for the class RKFehlberg45IntegratorConstructor.
|
||||
*/
|
||||
|
||||
/*
|
||||
Purpose: ()
|
||||
*/
|
||||
|
||||
// System includes
|
||||
|
||||
// Interface includes
|
||||
#include "er7_utils/interface/include/alloc.hh"
|
||||
|
||||
// Integration includes
|
||||
#include "er7_utils/integration/core/include/integrator_constructor_utils.hh"
|
||||
#include "er7_utils/integration/core/include/single_cycle_integration_controls.hh"
|
||||
|
||||
// Model includes
|
||||
#include "../include/rkf45_integrator_constructor.hh"
|
||||
#include "../include/rkf45_first_order_ode_integrator.hh"
|
||||
#include "../include/rkf45_second_order_ode_integrator.hh"
|
||||
|
||||
|
||||
namespace er7_utils {
|
||||
|
||||
// Named constructor; create an RKFehlberg45IntegratorConstructor.
|
||||
IntegratorConstructor*
|
||||
RKFehlberg45IntegratorConstructor::create_constructor (
|
||||
void)
|
||||
{
|
||||
return alloc::allocate_object<RKFehlberg45IntegratorConstructor> ();
|
||||
}
|
||||
|
||||
|
||||
// Create a duplicate of the constructor.
|
||||
IntegratorConstructor *
|
||||
RKFehlberg45IntegratorConstructor::create_copy (
|
||||
void)
|
||||
const
|
||||
{
|
||||
return alloc::replicate_object (*this);
|
||||
}
|
||||
|
||||
|
||||
// Create an RKFehlberg45 integration controls.
|
||||
IntegrationControls *
|
||||
RKFehlberg45IntegratorConstructor::create_integration_controls (
|
||||
void)
|
||||
const
|
||||
{
|
||||
SingleCycleIntegrationControls * controller =
|
||||
integ_utils::allocate_controls<SingleCycleIntegrationControls> (6);
|
||||
return controller;
|
||||
}
|
||||
|
||||
|
||||
// Create an RKFehlberg45 state integrator for a first order ODE.
|
||||
FirstOrderODEIntegrator *
|
||||
RKFehlberg45IntegratorConstructor::create_first_order_ode_integrator (
|
||||
unsigned int size,
|
||||
IntegrationControls & controls)
|
||||
const
|
||||
{
|
||||
return integ_utils::allocate_integrator<RKFehlberg45FirstOrderODEIntegrator> (
|
||||
size, controls);
|
||||
}
|
||||
|
||||
|
||||
// Create an RKFehlberg45 state integrator for a second order ODE.
|
||||
SecondOrderODEIntegrator *
|
||||
RKFehlberg45IntegratorConstructor::create_second_order_ode_integrator (
|
||||
unsigned int size,
|
||||
IntegrationControls & controls)
|
||||
const
|
||||
{
|
||||
return integ_utils::allocate_integrator<RKFehlberg45SimpleSecondOrderODEIntegrator> (
|
||||
size, controls);
|
||||
}
|
||||
|
||||
|
||||
// Create an RKFehlberg45 state integrator for a second order ODE.
|
||||
SecondOrderODEIntegrator *
|
||||
RKFehlberg45IntegratorConstructor::
|
||||
create_generalized_deriv_second_order_ode_integrator (
|
||||
unsigned int position_size,
|
||||
unsigned int velocity_size,
|
||||
const GeneralizedPositionDerivativeFunctions & deriv_funs,
|
||||
IntegrationControls & controls)
|
||||
const
|
||||
{
|
||||
return integ_utils::allocate_integrator<
|
||||
RKFehlberg45GeneralizedDerivSecondOrderODEIntegrator> (
|
||||
position_size, velocity_size, deriv_funs, controls);
|
||||
}
|
||||
|
||||
|
||||
// Create an RKFehlberg45 state integrator for a second order ODE.
|
||||
SecondOrderODEIntegrator *
|
||||
RKFehlberg45IntegratorConstructor::
|
||||
create_generalized_step_second_order_ode_integrator (
|
||||
unsigned int position_size,
|
||||
unsigned int velocity_size,
|
||||
const GeneralizedPositionStepFunctions & step_funs,
|
||||
IntegrationControls & controls)
|
||||
const
|
||||
{
|
||||
return integ_utils::allocate_integrator<
|
||||
RKFehlberg45GeneralizedStepSecondOrderODEIntegrator> (
|
||||
position_size, velocity_size, step_funs, controls);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
/**
|
||||
* @if Er7UtilsUseGroups
|
||||
* @}
|
||||
* @}
|
||||
* @endif
|
||||
*/
|
||||
@ -0,0 +1,635 @@
|
||||
/**
|
||||
* @if Er7UtilsUseGroups
|
||||
* @addtogroup Er7Utils
|
||||
* @{
|
||||
* @addtogroup Integration
|
||||
* @{
|
||||
* @endif
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file
|
||||
* Defines member functions for the class RKFehlberg45SecondOrderODEIntegrator.
|
||||
*/
|
||||
|
||||
/*
|
||||
Purpose: ()
|
||||
*/
|
||||
|
||||
|
||||
// System includes
|
||||
|
||||
// Interface includes
|
||||
#include "er7_utils/interface/include/alloc.hh"
|
||||
|
||||
// Integration includes
|
||||
#include "er7_utils/integration/core/include/integ_utils.hh"
|
||||
#include "er7_utils/integration/core/include/rk_utils.hh"
|
||||
|
||||
// Local includes
|
||||
#include "../include/rkf45_butcher_tableau.hh"
|
||||
#include "../include/rkf45_second_order_ode_integrator.hh"
|
||||
|
||||
|
||||
namespace er7_utils {
|
||||
|
||||
|
||||
/**
|
||||
* Make a simple Runge Kutta Fehlberg 4/5 intermediate step.
|
||||
* @tparam target_stage The stage of the integration process
|
||||
* that the integrator should try to attain.
|
||||
* @param[in] dyn_dt Dynamic time step, in dynamic time seconds.
|
||||
* @param[in] state_size The size of the state vectors.
|
||||
* @param[in] init_pos Initial position.
|
||||
* @param[in] init_vel Initial velocity.
|
||||
* @param[in] accel Current acceleration.
|
||||
* @param[in,out] veldot_hist Velocity derivative (acceleration) history.
|
||||
* @param[in,out] posdot_hist Position derivative (velocity) history.
|
||||
* @param[in,out] velocity Generalized velocity vector.
|
||||
* @param[in,out] position Generalized position vector.
|
||||
*
|
||||
* @return Fractional amount by which time should be advanced.
|
||||
*/
|
||||
template <int target_stage>
|
||||
inline double ER7_UTILS_ALWAYS_INLINE
|
||||
rkf45_simple_intermediate_step (
|
||||
double dyn_dt,
|
||||
int state_size[2],
|
||||
double const * ER7_UTILS_RESTRICT init_pos,
|
||||
double const * ER7_UTILS_RESTRICT init_vel,
|
||||
double const * ER7_UTILS_RESTRICT accel,
|
||||
double * ER7_UTILS_RESTRICT * ER7_UTILS_RESTRICT veldot_hist,
|
||||
double * ER7_UTILS_RESTRICT * ER7_UTILS_RESTRICT posdot_hist,
|
||||
double * ER7_UTILS_RESTRICT velocity,
|
||||
double * ER7_UTILS_RESTRICT position)
|
||||
{
|
||||
// Advance position and velocity per the 'a' element of the Butcher tableau,
|
||||
// saving velocity and acceleration in the derivatives history buffers.
|
||||
integ_utils::two_state_weighted_step_save_derivs<target_stage> (
|
||||
init_pos, init_vel, accel,
|
||||
RKFehlberg45ButcherTableau::RKa[target_stage], dyn_dt, state_size[0],
|
||||
posdot_hist, veldot_hist, position, velocity);
|
||||
|
||||
// Advance time per the 'c' element of the Butcher tableau.
|
||||
return RKFehlberg45ButcherTableau::RKc[target_stage];
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Make a generalized deriv Runge Kutta Fehlberg 4/5 intermediate step.
|
||||
* @tparam target_stage The stage of the integration process
|
||||
* that the integrator should try to attain.
|
||||
* @param[in] deriv_fun Function that computes position derivative.
|
||||
* @param[in] dyn_dt Dynamic time step, in dynamic time seconds.
|
||||
* @param[in] state_size The size of the state vectors.
|
||||
* @param[in] init_pos Initial position.
|
||||
* @param[in] init_vel Initial velocity.
|
||||
* @param[in] accel Current acceleration.
|
||||
* @param[in,out] veldot_hist Velocity derivative (acceleration) history.
|
||||
* @param[in,out] posdot_hist Position derivative (velocity) history.
|
||||
* @param[in,out] velocity Generalized velocity vector.
|
||||
* @param[in,out] position Generalized position vector.
|
||||
*
|
||||
* @return Fractional amount by which time should be advanced.
|
||||
*/
|
||||
template <int target_stage>
|
||||
inline double ER7_UTILS_ALWAYS_INLINE
|
||||
rkf45_generalized_deriv_intermediate_step (
|
||||
GeneralizedPositionDerivativeFunctions::FirstDerivative deriv_fun,
|
||||
double dyn_dt,
|
||||
int state_size[2],
|
||||
double const * ER7_UTILS_RESTRICT init_pos,
|
||||
double const * ER7_UTILS_RESTRICT init_vel,
|
||||
double const * ER7_UTILS_RESTRICT accel,
|
||||
double * ER7_UTILS_RESTRICT * ER7_UTILS_RESTRICT veldot_hist,
|
||||
double * ER7_UTILS_RESTRICT * ER7_UTILS_RESTRICT posdot_hist,
|
||||
double * ER7_UTILS_RESTRICT velocity,
|
||||
double * ER7_UTILS_RESTRICT position)
|
||||
{
|
||||
// Compute time derivative of canonical position,
|
||||
// saving the result in the position derivative history.
|
||||
deriv_fun (position, velocity, posdot_hist[target_stage-1]);
|
||||
|
||||
// Advance generalized position and velocity.
|
||||
integ_utils::two_state_weighted_step_save_veldot<target_stage> (
|
||||
init_pos, init_vel, accel,
|
||||
RKFehlberg45ButcherTableau::RKa[target_stage], dyn_dt, state_size,
|
||||
posdot_hist, veldot_hist, position, velocity);
|
||||
|
||||
// Advance time per the 'c' element of the Butcher tableau.
|
||||
return RKFehlberg45ButcherTableau::RKc[target_stage];
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Make a generalized step Runge Kutta Fehlberg 4/5 intermediate step.
|
||||
* @tparam target_stage The stage of the integration process
|
||||
* that the integrator should try to attain.
|
||||
* @param[in] xform_fun Function that transforms generalized velocity.
|
||||
* @param[in] step_fun Function that advances generalized position.
|
||||
* @param[in] dyn_dt Dynamic time step, in dynamic time seconds.
|
||||
* @param[in] vel_size The size of the generalized velocity.
|
||||
* @param[in] init_pos Initial position.
|
||||
* @param[in] init_vel Initial velocity.
|
||||
* @param[in] accel Current acceleration.
|
||||
* @param[in,out] veldot_hist Velocity derivative (acceleration) history.
|
||||
* @param[in,out] posdot_hist Position derivative (velocity) history.
|
||||
* @param[in,out] dtheta Position step, in the velocity space.
|
||||
* @param[in,out] velocity Generalized velocity vector.
|
||||
* @param[in,out] position Generalized position vector.
|
||||
*
|
||||
* @return Fractional amount by which time should be advanced.
|
||||
*/
|
||||
template <int target_stage>
|
||||
inline double ER7_UTILS_ALWAYS_INLINE
|
||||
rkf45_generalized_step_intermediate_step (
|
||||
GeneralizedPositionStepFunctions::DexpinvTransformVelocity xform_fun,
|
||||
GeneralizedPositionStepFunctions::ExpMapPositionStep step_fun,
|
||||
double dyn_dt,
|
||||
int vel_size,
|
||||
double const * ER7_UTILS_RESTRICT init_pos,
|
||||
double const * ER7_UTILS_RESTRICT init_vel,
|
||||
double const * ER7_UTILS_RESTRICT accel,
|
||||
double * ER7_UTILS_RESTRICT * ER7_UTILS_RESTRICT veldot_hist,
|
||||
double * ER7_UTILS_RESTRICT * ER7_UTILS_RESTRICT posdot_hist,
|
||||
double * ER7_UTILS_RESTRICT dtheta,
|
||||
double * ER7_UTILS_RESTRICT velocity,
|
||||
double * ER7_UTILS_RESTRICT position)
|
||||
{
|
||||
// Transform velocity to the start of the interval,
|
||||
// saving the result in the position derivative history.
|
||||
xform_fun (velocity, dtheta, posdot_hist[target_stage-1]);
|
||||
|
||||
// Compute the position step for this stage.
|
||||
integ_utils::weighted_sum<target_stage> (
|
||||
posdot_hist, RKFehlberg45ButcherTableau::RKa[target_stage],
|
||||
dyn_dt, vel_size, dtheta);
|
||||
|
||||
// Take the non-linear position step.
|
||||
step_fun (init_pos, dtheta, position);
|
||||
|
||||
// Make a weighted step for velocity based on the RKF45 Butcher tableau,
|
||||
// saving the input acceleration in veldot_hist.
|
||||
integ_utils::weighted_step_save_deriv<target_stage> (
|
||||
init_vel, accel,
|
||||
RKFehlberg45ButcherTableau::RKa[target_stage], dyn_dt, vel_size,
|
||||
veldot_hist, velocity);
|
||||
|
||||
// Advance time per the 'c' element of the Butcher tableau.
|
||||
return RKFehlberg45ButcherTableau::RKc[target_stage];
|
||||
}
|
||||
|
||||
|
||||
|
||||
// Default constructor for an RKFehlberg45SecondOrderODEIntegrator.
|
||||
RKFehlberg45SecondOrderODEIntegrator::RKFehlberg45SecondOrderODEIntegrator (
|
||||
void)
|
||||
:
|
||||
Er7UtilsDeletable (),
|
||||
SecondOrderODEIntegrator (),
|
||||
init_pos (NULL),
|
||||
init_vel (NULL),
|
||||
dtheta (NULL)
|
||||
{
|
||||
// Initialize RKF45 memory to NULL pointers.
|
||||
alloc::initialize_2D_array<6> (posdot_hist);
|
||||
alloc::initialize_2D_array<6> (veldot_hist);
|
||||
}
|
||||
|
||||
|
||||
// Copy constructor for an RKFehlberg45SecondOrderODEIntegrator.
|
||||
RKFehlberg45SecondOrderODEIntegrator::RKFehlberg45SecondOrderODEIntegrator (
|
||||
const RKFehlberg45SecondOrderODEIntegrator & src)
|
||||
:
|
||||
Er7UtilsDeletable (),
|
||||
SecondOrderODEIntegrator (src),
|
||||
init_pos (NULL),
|
||||
init_vel (NULL),
|
||||
dtheta (NULL)
|
||||
{
|
||||
if (problem_type == Integration::GeneralizedStepSecondOrderODE) {
|
||||
init_pos = alloc::replicate_array (state_size[0], src.init_pos);
|
||||
init_vel = alloc::replicate_array (state_size[1], src.init_vel);
|
||||
dtheta = alloc::replicate_array (state_size[1], src.dtheta);
|
||||
alloc::replicate_2D_array<6> (state_size[1], src.posdot_hist,
|
||||
posdot_hist);
|
||||
alloc::replicate_2D_array<6> (state_size[1], src.veldot_hist,
|
||||
veldot_hist);
|
||||
|
||||
}
|
||||
|
||||
else if (src.init_pos != NULL) {
|
||||
init_pos = alloc::replicate_array (state_size[0], src.init_pos);
|
||||
init_vel = alloc::replicate_array (state_size[1], src.init_vel);
|
||||
alloc::replicate_2D_array<6> (state_size[0], src.posdot_hist,
|
||||
posdot_hist);
|
||||
alloc::replicate_2D_array<6> (state_size[1], src.veldot_hist,
|
||||
veldot_hist);
|
||||
}
|
||||
|
||||
else {
|
||||
alloc::initialize_2D_array<6> (posdot_hist);
|
||||
alloc::initialize_2D_array<6> (veldot_hist);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// Non-default constructor for an RKFehlberg45SecondOrderODEIntegrator
|
||||
// in which the time derivative of position is the generalized velocity.
|
||||
RKFehlberg45SecondOrderODEIntegrator::RKFehlberg45SecondOrderODEIntegrator (
|
||||
unsigned int size,
|
||||
IntegrationControls & controls)
|
||||
:
|
||||
Er7UtilsDeletable (),
|
||||
SecondOrderODEIntegrator (size, controls),
|
||||
init_pos (NULL),
|
||||
init_vel (NULL),
|
||||
dtheta (NULL)
|
||||
{
|
||||
// Allocate memory used by eighth order Runge Kutta Fehlberg algorithm.
|
||||
init_pos = alloc::allocate_array (size);
|
||||
init_vel = alloc::allocate_array (size);
|
||||
alloc::allocate_2D_array<6> (size, posdot_hist);
|
||||
alloc::allocate_2D_array<6> (size, veldot_hist);
|
||||
}
|
||||
|
||||
|
||||
// Non-default constructor for an RKFehlberg45SecondOrderODEIntegrator
|
||||
// for generalized position, generalized velocity.
|
||||
RKFehlberg45SecondOrderODEIntegrator::RKFehlberg45SecondOrderODEIntegrator (
|
||||
unsigned int position_size,
|
||||
unsigned int velocity_size,
|
||||
const GeneralizedPositionDerivativeFunctions & deriv_funs,
|
||||
IntegrationControls & controls)
|
||||
:
|
||||
Er7UtilsDeletable (),
|
||||
SecondOrderODEIntegrator (position_size, velocity_size,
|
||||
deriv_funs, controls),
|
||||
init_pos (NULL),
|
||||
init_vel (NULL),
|
||||
dtheta (NULL)
|
||||
{
|
||||
// Allocate memory used by eighth order Runge Kutta Fehlberg algorithm.
|
||||
init_pos = alloc::allocate_array (position_size);
|
||||
init_vel = alloc::allocate_array (velocity_size);
|
||||
alloc::allocate_2D_array<6> (position_size, posdot_hist);
|
||||
alloc::allocate_2D_array<6> (velocity_size, veldot_hist);
|
||||
}
|
||||
|
||||
|
||||
// Non-default constructor for an RKFehlberg45SecondOrderODEIntegrator
|
||||
// for generalized position, generalized velocity.
|
||||
RKFehlberg45SecondOrderODEIntegrator::RKFehlberg45SecondOrderODEIntegrator (
|
||||
unsigned int position_size,
|
||||
unsigned int velocity_size,
|
||||
const GeneralizedPositionStepFunctions & step_funs,
|
||||
IntegrationControls & controls)
|
||||
:
|
||||
Er7UtilsDeletable (),
|
||||
SecondOrderODEIntegrator (position_size, velocity_size,
|
||||
step_funs, controls),
|
||||
init_pos (NULL),
|
||||
init_vel (NULL),
|
||||
dtheta (NULL)
|
||||
{
|
||||
// Allocate memory used by eighth order Runge Kutta Fehlberg algorithm.
|
||||
init_pos = alloc::allocate_array (position_size);
|
||||
init_vel = alloc::allocate_array (velocity_size);
|
||||
dtheta = alloc::allocate_array (velocity_size);
|
||||
alloc::allocate_2D_array<6> (velocity_size, posdot_hist);
|
||||
alloc::allocate_2D_array<6> (velocity_size, veldot_hist);
|
||||
}
|
||||
|
||||
|
||||
// RKFehlberg45SecondOrderODEIntegrator destructor.
|
||||
RKFehlberg45SecondOrderODEIntegrator::~RKFehlberg45SecondOrderODEIntegrator (
|
||||
void)
|
||||
{
|
||||
alloc::deallocate_array (init_pos);
|
||||
alloc::deallocate_array (init_vel);
|
||||
alloc::deallocate_array (dtheta);
|
||||
alloc::deallocate_2D_array<6> (posdot_hist);
|
||||
alloc::deallocate_2D_array<6> (veldot_hist);
|
||||
}
|
||||
|
||||
|
||||
// Non-throwing swap.
|
||||
void
|
||||
RKFehlberg45SecondOrderODEIntegrator::swap (
|
||||
RKFehlberg45SecondOrderODEIntegrator & other)
|
||||
{
|
||||
SecondOrderODEIntegrator::swap (other);
|
||||
|
||||
std::swap (init_pos, other.init_pos);
|
||||
std::swap (init_vel, other.init_vel);
|
||||
std::swap (dtheta, other.dtheta);
|
||||
for (int ii = 0; ii < 6; ++ii) {
|
||||
std::swap (posdot_hist[ii], other.posdot_hist[ii]);
|
||||
std::swap (veldot_hist[ii], other.veldot_hist[ii]);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// Clone a RKFehlberg45SimpleSecondOrderODEIntegrator.
|
||||
RKFehlberg45SimpleSecondOrderODEIntegrator *
|
||||
RKFehlberg45SimpleSecondOrderODEIntegrator::create_copy ()
|
||||
const
|
||||
{
|
||||
return alloc::replicate_object (*this);
|
||||
}
|
||||
|
||||
|
||||
// Clone a RKFehlberg45GeneralizedDerivSecondOrderODEIntegrator.
|
||||
RKFehlberg45GeneralizedDerivSecondOrderODEIntegrator *
|
||||
RKFehlberg45GeneralizedDerivSecondOrderODEIntegrator::create_copy ()
|
||||
const
|
||||
{
|
||||
return alloc::replicate_object (*this);
|
||||
}
|
||||
|
||||
|
||||
// Clone a RKFehlberg45GeneralizedStepSecondOrderODEIntegrator.
|
||||
RKFehlberg45GeneralizedStepSecondOrderODEIntegrator *
|
||||
RKFehlberg45GeneralizedStepSecondOrderODEIntegrator::create_copy ()
|
||||
const
|
||||
{
|
||||
return alloc::replicate_object (*this);
|
||||
}
|
||||
|
||||
|
||||
// Propagate state for the special case of velocity being the derivative of
|
||||
// position.
|
||||
IntegratorResult
|
||||
RKFehlberg45SimpleSecondOrderODEIntegrator::integrate (
|
||||
double dyn_dt,
|
||||
unsigned int target_stage,
|
||||
double const * ER7_UTILS_RESTRICT accel,
|
||||
double * ER7_UTILS_RESTRICT velocity,
|
||||
double * ER7_UTILS_RESTRICT position)
|
||||
{
|
||||
double step_factor;
|
||||
|
||||
switch (target_stage) {
|
||||
|
||||
// Initial stage (stage 1):
|
||||
// Save initial state and update per RKF45 RKa[1] (one element).
|
||||
case 1:
|
||||
integ_utils::inplace_two_state_euler_step_save_all (
|
||||
accel,
|
||||
RKFehlberg45ButcherTableau::RKa[1][0]*dyn_dt,
|
||||
state_size[0],
|
||||
init_pos, init_vel,
|
||||
posdot_hist[0], veldot_hist[0],
|
||||
position, velocity);
|
||||
step_factor = RKFehlberg45ButcherTableau::RKc[1];
|
||||
break;
|
||||
|
||||
// Intermediate stages (2 to 5):
|
||||
// Update state per RKF45 RKa[target_stage] (target_stage elements).
|
||||
case 2:
|
||||
step_factor =
|
||||
rkf45_simple_intermediate_step<2> (
|
||||
dyn_dt, state_size, init_pos, init_vel, accel,
|
||||
veldot_hist, posdot_hist, velocity, position);
|
||||
break;
|
||||
|
||||
case 3:
|
||||
step_factor =
|
||||
rkf45_simple_intermediate_step<3> (
|
||||
dyn_dt, state_size, init_pos, init_vel, accel,
|
||||
veldot_hist, posdot_hist, velocity, position);
|
||||
break;
|
||||
|
||||
case 4:
|
||||
step_factor =
|
||||
rkf45_simple_intermediate_step<4> (
|
||||
dyn_dt, state_size, init_pos, init_vel, accel,
|
||||
veldot_hist, posdot_hist, velocity, position);
|
||||
break;
|
||||
|
||||
case 5:
|
||||
step_factor =
|
||||
rkf45_simple_intermediate_step<5> (
|
||||
dyn_dt, state_size, init_pos, init_vel, accel,
|
||||
veldot_hist, posdot_hist, velocity, position);
|
||||
break;
|
||||
|
||||
// Final stage (6):
|
||||
// Update state per RKF45 RKb5 (6 elements).
|
||||
case 6:
|
||||
integ_utils::two_state_weighted_step<6> (
|
||||
init_pos, init_vel, accel, posdot_hist, veldot_hist,
|
||||
RKFehlberg45ButcherTableau::RKb5, dyn_dt, state_size[0],
|
||||
position, velocity);
|
||||
step_factor = 1.0;
|
||||
break;
|
||||
|
||||
default:
|
||||
step_factor = 1.0;
|
||||
break;
|
||||
}
|
||||
|
||||
return step_factor;
|
||||
}
|
||||
|
||||
|
||||
// Propagate state for the general case of the generalized position derivative
|
||||
// being a function of generalized position and generalized velocity.
|
||||
IntegratorResult
|
||||
RKFehlberg45GeneralizedDerivSecondOrderODEIntegrator::integrate (
|
||||
double dyn_dt,
|
||||
unsigned int target_stage,
|
||||
double const * ER7_UTILS_RESTRICT accel,
|
||||
double * ER7_UTILS_RESTRICT velocity,
|
||||
double * ER7_UTILS_RESTRICT position)
|
||||
{
|
||||
double step_factor;
|
||||
|
||||
switch (target_stage) {
|
||||
|
||||
// Initial stage (stage 1):
|
||||
// Save initial state and update per RKF45 RKa[1] (one element).
|
||||
case 1:
|
||||
|
||||
// Compute time derivative of canonical position,
|
||||
// saving the result in the position derivative history.
|
||||
compute_posdot (position, velocity, posdot_hist[0]);
|
||||
|
||||
// Advance position and velocity.
|
||||
integ_utils::inplace_two_state_euler_step_save_pos_vel_acc (
|
||||
posdot_hist[0], accel,
|
||||
RKFehlberg45ButcherTableau::RKa[1][0]*dyn_dt, state_size,
|
||||
init_pos, init_vel, veldot_hist[0],
|
||||
position, velocity);
|
||||
|
||||
step_factor = RKFehlberg45ButcherTableau::RKc[1];
|
||||
break;
|
||||
|
||||
// Intermediate stages (2 to 5):
|
||||
// Update state per RKF45 RKa[target_stage] (target_stage elements).
|
||||
case 2:
|
||||
step_factor =
|
||||
rkf45_generalized_deriv_intermediate_step<2> (
|
||||
compute_posdot, dyn_dt, state_size, init_pos, init_vel, accel,
|
||||
veldot_hist, posdot_hist, velocity, position);
|
||||
break;
|
||||
|
||||
case 3:
|
||||
step_factor =
|
||||
rkf45_generalized_deriv_intermediate_step<3> (
|
||||
compute_posdot, dyn_dt, state_size, init_pos, init_vel, accel,
|
||||
veldot_hist, posdot_hist, velocity, position);
|
||||
break;
|
||||
|
||||
case 4:
|
||||
step_factor =
|
||||
rkf45_generalized_deriv_intermediate_step<4> (
|
||||
compute_posdot, dyn_dt, state_size, init_pos, init_vel, accel,
|
||||
veldot_hist, posdot_hist, velocity, position);
|
||||
break;
|
||||
|
||||
case 5:
|
||||
step_factor =
|
||||
rkf45_generalized_deriv_intermediate_step<5> (
|
||||
compute_posdot, dyn_dt, state_size, init_pos, init_vel, accel,
|
||||
veldot_hist, posdot_hist, velocity, position);
|
||||
break;
|
||||
|
||||
// Final stage (6):
|
||||
// Update state per RKF45 RKb5 (6 elements).
|
||||
case 6:
|
||||
compute_posdot (position, velocity, posdot_hist[5]);
|
||||
|
||||
integ_utils::two_state_weighted_step<6> (
|
||||
init_pos, init_vel, posdot_hist[5], accel, posdot_hist, veldot_hist,
|
||||
RKFehlberg45ButcherTableau::RKb5, dyn_dt, state_size,
|
||||
position, velocity);
|
||||
step_factor = 1.0;
|
||||
break;
|
||||
|
||||
default:
|
||||
step_factor = 1.0;
|
||||
break;
|
||||
}
|
||||
|
||||
return step_factor;
|
||||
}
|
||||
|
||||
|
||||
// Propagate state for the general case of the generalized position derivative
|
||||
// being a function of generalized position and generalized velocity.
|
||||
IntegratorResult
|
||||
RKFehlberg45GeneralizedStepSecondOrderODEIntegrator::integrate (
|
||||
double dyn_dt,
|
||||
unsigned int target_stage,
|
||||
double const * ER7_UTILS_RESTRICT accel,
|
||||
double * ER7_UTILS_RESTRICT velocity,
|
||||
double * ER7_UTILS_RESTRICT position)
|
||||
{
|
||||
double step_factor;
|
||||
int vel_size = state_size[1];
|
||||
|
||||
// Advance the state.
|
||||
// Integrate to the next stage
|
||||
switch (target_stage) {
|
||||
|
||||
// Initial stage (stage 1):
|
||||
// Save initial state and update per RKF45 initial step.
|
||||
case 1: {
|
||||
double dt = RKFehlberg45ButcherTableau::RKc[1]*dyn_dt;
|
||||
|
||||
// Save the initial position and velocity in init_pos and posdot_hist.
|
||||
integ_utils::two_state_copy_array (
|
||||
position, velocity, state_size, init_pos, posdot_hist[0]);
|
||||
|
||||
// Compute the initial position step.
|
||||
integ_utils::scale_array (velocity, dt, state_size[1], dtheta);
|
||||
|
||||
// Make a non-linear position step.
|
||||
compute_expmap_position_step (init_pos, dtheta, position);
|
||||
|
||||
// Make an in-place Euler step for velocity to the stage 1 end time,
|
||||
// saving the initial velocity in init_vel, acceleration in veldot_hist.
|
||||
integ_utils::inplace_euler_step_save_both (
|
||||
accel, dt, vel_size,
|
||||
init_vel, veldot_hist[0], velocity);
|
||||
|
||||
// Advance time to stage 1 end time.
|
||||
step_factor = RKFehlberg45ButcherTableau::RKc[1];
|
||||
break;
|
||||
}
|
||||
|
||||
// Intermediate stages (2 to 5):
|
||||
// Update state per RKF45 RKa[target_stage] (target_stage elements).
|
||||
case 2:
|
||||
step_factor =
|
||||
rkf45_generalized_step_intermediate_step<2> (
|
||||
compute_dexpinv_velocity_transform, compute_expmap_position_step,
|
||||
dyn_dt, vel_size, init_pos, init_vel, accel,
|
||||
veldot_hist, posdot_hist, dtheta, velocity, position);
|
||||
break;
|
||||
|
||||
case 3:
|
||||
step_factor =
|
||||
rkf45_generalized_step_intermediate_step<3> (
|
||||
compute_dexpinv_velocity_transform, compute_expmap_position_step,
|
||||
dyn_dt, vel_size, init_pos, init_vel, accel,
|
||||
veldot_hist, posdot_hist, dtheta, velocity, position);
|
||||
break;
|
||||
|
||||
case 4:
|
||||
step_factor =
|
||||
rkf45_generalized_step_intermediate_step<4> (
|
||||
compute_dexpinv_velocity_transform, compute_expmap_position_step,
|
||||
dyn_dt, vel_size, init_pos, init_vel, accel,
|
||||
veldot_hist, posdot_hist, dtheta, velocity, position);
|
||||
break;
|
||||
|
||||
case 5:
|
||||
step_factor =
|
||||
rkf45_generalized_step_intermediate_step<5> (
|
||||
compute_dexpinv_velocity_transform, compute_expmap_position_step,
|
||||
dyn_dt, vel_size, init_pos, init_vel, accel,
|
||||
veldot_hist, posdot_hist, dtheta, velocity, position);
|
||||
break;
|
||||
|
||||
// Final stage (6):
|
||||
// Update state per RKF45 RKb5 (6 elements).
|
||||
case 6:
|
||||
|
||||
// Transform velocity to the start of the interval,
|
||||
// saving the result in the position derivative history.
|
||||
compute_dexpinv_velocity_transform (velocity, dtheta, posdot_hist[5]);
|
||||
|
||||
// Compute the position step for this stage.
|
||||
integ_utils::weighted_sum<6> (
|
||||
posdot_hist, RKFehlberg45ButcherTableau::RKb5, dyn_dt, vel_size,
|
||||
dtheta);
|
||||
|
||||
// Take the non-linear position step.
|
||||
compute_expmap_position_step (init_pos, dtheta, position);
|
||||
|
||||
integ_utils::weighted_step<6> (
|
||||
init_vel, accel, veldot_hist,
|
||||
RKFehlberg45ButcherTableau::RKb5, dyn_dt, vel_size,
|
||||
velocity);
|
||||
|
||||
step_factor = 1.0;
|
||||
break;
|
||||
|
||||
default:
|
||||
step_factor = 1.0;
|
||||
break;
|
||||
}
|
||||
|
||||
return step_factor;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
/**
|
||||
* @if Er7UtilsUseGroups
|
||||
* @}
|
||||
* @}
|
||||
* @endif
|
||||
*/
|
||||
Reference in New Issue
Block a user