//! 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++; } } }