//! Function to numerical compute the partial derivative of an acceleration w.r.t. a body state.
Eigen::Matrix3d calculateAccelerationWrtStatePartials(
boost::function< void( basic_mathematics::Vector6d ) > setBodyState,
boost::shared_ptr< basic_astrodynamics::AccelerationModel< Eigen::Vector3d > > accelerationModel,
basic_mathematics::Vector6d originalState,
Eigen::Vector3d statePerturbation,
int startIndex,
boost::function< void( ) > updateFunction )
{
Eigen::Matrix3d upAccelerations = Eigen::Matrix3d::Zero( );
Eigen::Matrix3d downAccelerations = Eigen::Matrix3d::Zero( );
basic_mathematics::Vector6d perturbedState = originalState;
// Calculate perturbed accelerations for up-perturbed state entries.
for( int i = 0; i < 3; i++ )
{
perturbedState( i + startIndex ) += statePerturbation( i );
setBodyState( perturbedState );
updateFunction( );
upAccelerations.block( 0, i, 3, 1 ) = basic_astrodynamics::updateAndGetAcceleration< Eigen::Vector3d >(
accelerationModel );
accelerationModel->resetTime( TUDAT_NAN );
perturbedState = originalState;
}
// Calculate perturbed accelerations for down-perturbed state entries.
for( int i = 0; i < 3; i++ )
{
perturbedState( i + startIndex ) -= statePerturbation( i );
setBodyState( perturbedState );
updateFunction( );
downAccelerations.block( 0, i, 3, 1 ) = basic_astrodynamics::updateAndGetAcceleration< Eigen::Vector3d >(
accelerationModel );
accelerationModel->resetTime( TUDAT_NAN );
perturbedState = originalState;
}
// Reset state/environment to original state.
setBodyState( perturbedState );
updateFunction( );
basic_astrodynamics::updateAndGetAcceleration< Eigen::Vector3d >(
accelerationModel );
// Numerically compute partial derivatives.
Eigen::Matrix3d accelerationPartials = upAccelerations - downAccelerations;
for( int i = 0; i < 3; i++ )
{
accelerationPartials.block( 0, i, 3, 1 ) /= ( 2.0 * statePerturbation( i ) );
}
return accelerationPartials;
}
/**
* @function getPlaneTransform
* @brief Assumes plane coefficients are of the form ax+by+cz+d=0, normalized
*/
Eigen::Matrix4d getPlaneTransform ( pcl::ModelCoefficients &coeffs,
double up_direction,
bool flatten_plane ) {
Eigen::Matrix4d tf = Eigen::Matrix4d::Identity();
if( coeffs.values.size() <= 3 ) {
std::cout << "[ERROR] Coefficient size less than 3. I will output nonsense values"<<std::endl;
return tf;
}
double a = coeffs.values[0];
double b = coeffs.values[1];
double c = coeffs.values[2];
double d = coeffs.values[3];
//asume plane coefficients are normalized
Eigen::Vector3d position(-a*d, -b*d, -c*d);
Eigen::Vector3d z(a, b, c);
//if we are flattening the plane, make z just be (0,0,up_direction)
if(flatten_plane) {
z << 0, 0, up_direction;
}
else {
//make sure z points "up" (the plane normal and the table must have a > 90 angle, hence the cosine must be negative)
if ( z.dot( Eigen::Vector3d(0, 0, up_direction) ) > 0) {
z = -1.0 * z;
printf("Changing sign \n");
coeffs.values[0]*= -1; coeffs.values[1]*= -1; coeffs.values[2]*= -1; coeffs.values[3]*= -1;
}
}
//try to align the x axis with the x axis of the original frame
//or the y axis if z and x are too close too each other
Eigen::Vector3d x; x << 1, 0, 0;
if ( fabs(z.dot(x)) > 1.0 - 1.0e-4) x = Eigen::Vector3d(0, 1, 0);
Eigen::Vector3d y = z.cross(x); y.normalized();
x = y.cross(z); x.normalized();
Eigen::Matrix3d rotation;
rotation.block(0,0,3,1) = x;
rotation.block(0,1,3,1) = y;
rotation.block(0,2,3,1) = z;
Eigen::Quaterniond orientation( rotation );
tf.block(0,0,3,3) = orientation.matrix();
tf.block(0,3,3,1) = position;
return tf;
}
//! Function to compute the partial w.r.t. time-independent empirical acceleration components
void EmpiricalAccelerationPartial::wrtEmpiricalAccelerationCoefficientFromIndices(
const int numberOfAccelerationComponents,
const std::map< basic_astrodynamics::EmpiricalAccelerationFunctionalShapes, std::vector< int > >& accelerationIndices,
Eigen::MatrixXd& partial )
{
// Retrieve rotation matrix to inertial frame
Eigen::Matrix3d rotationMatrix = Eigen::Matrix3d( empiricalAcceleration_->getCurrentToInertialFrame( ) );
// Initialize partial derivatives
partial = Eigen::Matrix< double, 3, Eigen::Dynamic >::Zero( 3, numberOfAccelerationComponents );
// Iterate over all terms, and set partial
int currentIndex = 0;
double multiplier = 0.0;
for( std::map< basic_astrodynamics::EmpiricalAccelerationFunctionalShapes, std::vector< int > >::const_iterator
indexIterator = accelerationIndices.begin( );
indexIterator != accelerationIndices.end( ); indexIterator++ )
{
// Get multiplier associated with functional shape of empirical acceleration
switch( indexIterator->first )
{
case basic_astrodynamics::constant_empirical:
multiplier = 1.0;
break;
case basic_astrodynamics::sine_empirical:
multiplier = std::sin( empiricalAcceleration_->getCurrentTrueAnomaly( ) );
break;
case basic_astrodynamics::cosine_empirical:
multiplier = std::cos( empiricalAcceleration_->getCurrentTrueAnomaly( ) );
break;
default:
throw std::runtime_error(
"Error when calculating partial w.r.t. empirical accelerations, could not find functional shape " );
}
// Set partial value for current component and shape
for( unsigned int i = 0; i < indexIterator->second.size( ); i++ )
{
partial.block( 0, currentIndex, 3, 1 ) = multiplier * rotationMatrix.block ( 0, indexIterator->second.at( i ), 3, 1 );
currentIndex++;
}
}
}
