예제 #1
0
void DiagonalBlockSparseMatrix::Solve(const Eigen::VectorXd& rhs, Eigen::VectorXd& sol)
{
	int numRows = GetNumRows();
	int numCols = GetNumCols();
	LOG_IF(FATAL, numRows != numCols) << "DiagonalBlockSparseMatrix is not a square matrix and noninvertible.";
	int vecLength = rhs.size();
	LOG_IF(FATAL, numCols != vecLength) << "DiagonalBlockSparseMatrix and right hand side vector are of different dimensions.";

	sol.resize(numCols);
	int numUntilLast = 0;
	int numDiagElements = static_cast<int>(mDiagElements.size());
	for (int i = 0; i < numDiagElements; ++i)
	{
		int numColsElement = mDiagElements[i].GetNumCols();	
		Eigen::VectorXd partSol(numColsElement);
		Eigen::VectorXd partRhs = rhs.segment(numUntilLast, numColsElement);
#if LLT_SOLVE
		mDiagElements[i].LltSolve(partRhs, partSol);
#else
		mDiagElements[i].ConjugateGradientSolve(partRhs, partSol);
#endif		
		sol.segment(numUntilLast, numColsElement) = partSol;
		numUntilLast += numColsElement;
	}
}
예제 #2
0
// Compute the average contour, by calling compute_contour_vals repeatedly
//
// [[Rcpp::export]]
Eigen::MatrixXd rcppstandardize_rates(const Eigen::VectorXd &tiles, const Eigen::VectorXd &rates,
                                      const Eigen::VectorXd &xseed, const Eigen::VectorXd &yseed,
                                      const Eigen::MatrixXd &marks, const Eigen::VectorXd &nmrks,
                                      const std::string &distm) {
    bool use_weighted_mean = true;
    int nxmrks = nmrks(0);
    int nymrks = nmrks(1);
    Eigen::MatrixXd Zvals = Eigen::MatrixXd::Zero(nymrks,nxmrks);
    Eigen::MatrixXd zvals = Eigen::MatrixXd::Zero(nymrks,nxmrks);
    int niters = tiles.size();
    for ( int i = 0, pos = 0 ; i < niters ; i++ ) {
        int now_tiles = (int)tiles(i);
        Eigen::VectorXd now_rates = rates.segment(pos,now_tiles);
        Eigen::VectorXd now_xseed = xseed.segment(pos,now_tiles);
        Eigen::VectorXd now_yseed = yseed.segment(pos,now_tiles);
        Eigen::MatrixXd now_seeds(now_tiles, 2);
        now_seeds << now_xseed,now_yseed;
        if (use_weighted_mean) {
            compute_contour_vals(zvals,marks,now_rates,now_seeds,distm);
            zvals = zvals.array() - zvals.mean();
        } else {
            now_rates = now_rates.array() - now_rates.mean();
            compute_contour_vals(zvals,marks,now_rates,now_seeds,distm);
        }
        Zvals += zvals;
        pos += now_tiles;
    }
    // Do not divide by niters here but in 'average.eems.contours' instead
    // Zvals = Zvals.array() / niters;
    return Zvals.transpose();
}
예제 #3
0
void BallJointConstraint::updateDynamics(Eigen::MatrixXd & _J1, Eigen::VectorXd & _C, Eigen::VectorXd & _CDot, int _rowIndex) {
    getJacobian();
    _J1.block(_rowIndex, 0, 3, mBodyNode1->getSkeleton()->getNumGenCoords()) = mJ1;
    Eigen::Vector3d worldP1 = mBodyNode1->getWorldTransform() * mOffset1;
    Eigen::VectorXd qDot1 = mBodyNode1->getSkeleton()->get_dq();
    _C.segment(_rowIndex, 3) = worldP1 - mOffset2;
    _CDot.segment(_rowIndex, 3) = mJ1 * qDot1;
}
//! Convert Cartesian to spherical state.
Eigen::VectorXd convertCartesianToSphericalState( const Eigen::VectorXd& cartesianState )
{
    // Create spherical state vector, initialized with zero entries.
    Eigen::VectorXd convertedSphericalState = Eigen::VectorXd::Zero( 6 );

    // Compute radius.
    convertedSphericalState( 0 ) = cartesianState.segment( 0, 3 ).norm( );

    // Check if radius is nonzero.
    /*
     * If r > 0, the elevation and azimuth angles are computed using trigonometric relationships.
     * If r = 0, the coordinates are at the origin, the elevation and azimuth angles equal to zero.
     * Since the state vector was initialized with zeroes, this is already the case.
     */
    if ( convertedSphericalState( 0 ) > std::numeric_limits< double >::epsilon( ) )
    {
        // Compute elevation and azimuth angles using trigonometric relationships.
        // Azimuth angle.
        convertedSphericalState( 1 ) = std::atan2( cartesianState( 1 ), cartesianState( 0 ) );
        // Elevation angle.
        convertedSphericalState( 2 ) = std::asin( cartesianState( 2 )
                                                   / convertedSphericalState( 0 ) );
    }

    // Precompute sine/cosine of angles, which has multiple usages, to save computation time.
    const double cosineOfElevationAngle = std::cos( convertedSphericalState( 2 ) );
    const double sineOfElevationAngle = std::sin( convertedSphericalState( 2 ) );
    const double cosineOfAzimuthAngle = std::cos( convertedSphericalState( 1 ) );
    const double sineOfAzimuthAngle = std::sin( convertedSphericalState( 1 ) );

    // Set up transformation matrix for cylindrical to spherical conversion.
    Eigen::MatrixXd transformationMatrixCylindricalToSpherical = Eigen::MatrixXd::Zero( 3, 3 );
    transformationMatrixCylindricalToSpherical( 0, 0 ) = cosineOfElevationAngle;
    transformationMatrixCylindricalToSpherical( 0, 2 ) = sineOfElevationAngle;
    transformationMatrixCylindricalToSpherical( 1, 1 ) = 1.0;
    transformationMatrixCylindricalToSpherical( 2, 0 ) = -sineOfElevationAngle;
    transformationMatrixCylindricalToSpherical( 2, 2 ) = cosineOfElevationAngle;

    // Set up transformation matrix for Cartesian to cylindrical conversion.
    Eigen::MatrixXd transformationMatrixCartesianToCylindrical = Eigen::MatrixXd::Zero( 3, 3 );
    transformationMatrixCartesianToCylindrical( 0, 0 ) = cosineOfAzimuthAngle;
    transformationMatrixCartesianToCylindrical( 0, 1 ) = sineOfAzimuthAngle;
    transformationMatrixCartesianToCylindrical( 1, 0 ) = -sineOfAzimuthAngle;
    transformationMatrixCartesianToCylindrical( 1, 1 ) = cosineOfAzimuthAngle;
    transformationMatrixCartesianToCylindrical( 2, 2 ) = 1.0;

    // Compute transformation matrix for Cartesian to spherical conversion.
    const Eigen::MatrixXd transformationMatrixCartesianToSpherical
            = transformationMatrixCylindricalToSpherical
            * transformationMatrixCartesianToCylindrical;

    // Perform transformation of velocity vector.
    convertedSphericalState.segment( 3, 3 )
            = transformationMatrixCartesianToSpherical * cartesianState.segment( 3, 3 );

    // Return spherical state vector.
    return convertedSphericalState;
}
예제 #5
0
 void PointConstraint::updateDynamics(std::vector<Eigen::MatrixXd> & _J, Eigen::VectorXd & _C, Eigen::VectorXd & _CDot, int _rowIndex) {
     getJacobian();
     SkeletonDynamics *skel = (SkeletonDynamics*)mBody->getSkel();
     _J[mSkelIndex].block(_rowIndex, 0, 3, skel->getNumDofs()) = mJ;
     Vector3d worldP = xformHom(mBody->getWorldTransform(), mOffset);
     VectorXd qDot = skel->getQDotVector();
     _C.segment(_rowIndex, 3) = worldP - mTarget;
     _CDot.segment(_rowIndex, 3) = mJ * qDot;
 }
예제 #6
0
void PointConstraint::updateDynamics(std::vector<Eigen::MatrixXd> & _J, Eigen::VectorXd & _C, Eigen::VectorXd & _CDot, int _rowIndex) {
    getJacobian();
    dynamics::Skeleton *skel = mBody->getSkeleton();
    _J[mSkelIndex].block(_rowIndex, 0, 3, skel->getDOF()) = mJ;
    Eigen::Vector3d worldP = mBody->getWorldTransform() * mOffset;
    Eigen::VectorXd qDot = skel->get_dq();
    _C.segment(_rowIndex, 3) = worldP - mTarget;
    _CDot.segment(_rowIndex, 3) = mJ * qDot;
}
예제 #7
0
Eigen::VectorXd IEFSolver::computeCharge_impl(const Eigen::VectorXd & potential, int irrep) const
{
  // The potential and charge vector are of dimension equal to the
  // full dimension of the cavity. We have to select just the part
  // relative to the irrep needed.
  int fullDim = fullPCMMatrix_.rows();
  Eigen::VectorXd charge = Eigen::VectorXd::Zero(fullDim);
  int nrBlocks = blockPCMMatrix_.size();
  int irrDim = fullDim/nrBlocks;
  charge.segment(irrep*irrDim, irrDim) =
    - blockPCMMatrix_[irrep] * potential.segment(irrep*irrDim, irrDim);
  return charge;
}
예제 #8
0
    void ClosedLoopConstraint::updateDynamics(std::vector<Eigen::MatrixXd> & _J, Eigen::VectorXd & _C, Eigen::VectorXd & _CDot, int _rowIndex) {
        getJacobian();
        _J[mSkelIndex2].block(_rowIndex, 0, 3, mBody2->getSkel()->getNumDofs()).setZero();
        _J[mSkelIndex1].block(_rowIndex, 0, 3, mBody1->getSkel()->getNumDofs()) = mJ1;
        _J[mSkelIndex2].block(_rowIndex, 0, 3, mBody2->getSkel()->getNumDofs()) += mJ2;

        Vector3d worldP1 = xformHom(mBody1->getWorldTransform(), mOffset1);
        Vector3d worldP2 = xformHom(mBody2->getWorldTransform(), mOffset2);
        VectorXd qDot1 = ((SkeletonDynamics*)mBody1->getSkel())->getPoseVelocity();
        VectorXd qDot2 = ((SkeletonDynamics*)mBody2->getSkel())->getPoseVelocity();
        _C.segment(_rowIndex, 3) = worldP1 - worldP2;
        _CDot.segment(_rowIndex, 3) = mJ1 * qDot1 + mJ2 * qDot2;
    }
예제 #9
0
void BallJointConstraint::updateDynamics(Eigen::MatrixXd & _J1, Eigen::MatrixXd & _J2, Eigen::VectorXd & _C, Eigen::VectorXd & _CDot, int _rowIndex) {
    getJacobian();
    _J2.block(_rowIndex, 0, 3, mBodyNode2->getSkeleton()->getNumGenCoords()).setZero();
    _J1.block(_rowIndex, 0, 3, mBodyNode1->getSkeleton()->getNumGenCoords()) = mJ1;
    _J2.block(_rowIndex, 0, 3, mBodyNode2->getSkeleton()->getNumGenCoords()) += mJ2;

    Eigen::Vector3d worldP1 = mBodyNode1->getWorldTransform() * mOffset1;
    Eigen::VectorXd qDot1 = ((dynamics::Skeleton*)mBodyNode1->getSkeleton())->getGenVels();
    Eigen::VectorXd worldP2 = mBodyNode2->getWorldTransform() * mOffset2;
    Eigen::VectorXd qDot2 = ((dynamics::Skeleton*)mBodyNode2->getSkeleton())->getGenVels();
    
    _C.segment(_rowIndex, 3) = worldP1 - worldP2;
    _CDot.segment(_rowIndex, 3) = mJ1 * qDot1 + mJ2 * qDot2;
}
//! Convert spherical to Cartesian state.
Eigen::VectorXd convertSphericalToCartesianState( const Eigen::VectorXd& sphericalState )
{
    // Create Cartesian state vector, initialized with zero entries.
    Eigen::VectorXd convertedCartesianState = Eigen::VectorXd::Zero( 6 );

    // Create local variables.
    const double radius = sphericalState( 0 );
    const double azimuthAngle = sphericalState( 1 );
    const double elevationAngle = sphericalState( 2 );

    // Precompute sine/cosine of angles, which has multiple usages, to save computation time.
    const double cosineOfElevationAngle = std::cos( elevationAngle );
    const double sineOfElevationAngle = std::sin( elevationAngle );
    const double cosineOfAzimuthAngle = std::cos( azimuthAngle );
    const double sineOfAzimuthAngle = std::sin( azimuthAngle );

    // Set up transformation matrix for spherical to cylindrical conversion.
    Eigen::MatrixXd transformationMatrixSphericalToCylindrical = Eigen::MatrixXd::Zero( 3, 3 );
    transformationMatrixSphericalToCylindrical( 0, 0 ) = cosineOfElevationAngle;
    transformationMatrixSphericalToCylindrical( 0, 2 ) = -sineOfElevationAngle;
    transformationMatrixSphericalToCylindrical( 1, 1 ) = 1.0;
    transformationMatrixSphericalToCylindrical( 2, 0 ) = sineOfElevationAngle;
    transformationMatrixSphericalToCylindrical( 2, 2 ) = cosineOfElevationAngle;

    // Set up transformation matrix for cylindrical to Cartesian conversion.
    Eigen::MatrixXd transformationMatrixCylindricalToCartesian = Eigen::MatrixXd::Zero( 3, 3 );
    transformationMatrixCylindricalToCartesian( 0, 0 ) = cosineOfAzimuthAngle;
    transformationMatrixCylindricalToCartesian( 0, 1 ) = -sineOfAzimuthAngle;
    transformationMatrixCylindricalToCartesian( 1, 0 ) = sineOfAzimuthAngle;
    transformationMatrixCylindricalToCartesian( 1, 1 ) = cosineOfAzimuthAngle;
    transformationMatrixCylindricalToCartesian( 2, 2 ) = 1.0;

    // Compute transformation matrix for spherical to Cartesian conversion.
    const Eigen::MatrixXd transformationMatrixSphericalToCartesian
            = transformationMatrixCylindricalToCartesian
            * transformationMatrixSphericalToCylindrical;

    // Perform transformation of position coordinates.
    convertedCartesianState( 0 ) = radius * cosineOfAzimuthAngle * cosineOfElevationAngle;
    convertedCartesianState( 1 ) = radius * sineOfAzimuthAngle * cosineOfElevationAngle;
    convertedCartesianState( 2 ) = radius * sineOfElevationAngle;

    // Perform transformation of velocity vector.
    convertedCartesianState.segment( 3, 3 ) =
        transformationMatrixSphericalToCartesian * sphericalState.segment( 3, 3 );

    // Return Cartesian state vector.
    return convertedCartesianState;
}
예제 #11
0
void inertialParametersVectorToUndirectedTreeLoop(const Eigen::VectorXd & parameters_vector,
                                                  UndirectedTree & undirected_tree)
{
    for(int i=0; i < (int)undirected_tree.getNrOfLinks(); i++ ) {
            undirected_tree.getLink(i,false)->setInertia(deVectorize(parameters_vector.segment(i*10,10)));
    }
}
예제 #12
0
/**
* Performs eigenvector decomposition of an affinity matrix
*
* @param data 		the affinity matrix
* @param numDims	the number of dimensions to consider when clustering
*/
SpectralClustering::SpectralClustering(Eigen::MatrixXd& data, int numDims):
	mNumDims(numDims),
	mNumClusters(0)
{
	Eigen::MatrixXd Deg = Eigen::MatrixXd::Zero(data.rows(),data.cols());

	// calc normalised laplacian 
	for ( int i=0; i < data.cols(); i++) {
		Deg(i,i)=1/(sqrt((data.row(i).sum())) );
	}
	Eigen::MatrixXd Lapla = Deg * data * Deg;

	Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> s(Lapla, true);
	Eigen::VectorXd val = s.eigenvalues();
	Eigen::MatrixXd vec = s.eigenvectors();

	//sort eigenvalues/vectors
	int n = data.cols();
	for (int i = 0; i < n - 1; ++i) {
		int k;
		val.segment(i, n - i).maxCoeff(&k);
		if (k > 0) {
			std::swap(val[i], val[k + i]);
			vec.col(i).swap(vec.col(k + i));
		}
	}

	//choose the number of eigenvectors to consider
	if (mNumDims < vec.cols()) {
		mEigenVectors = vec.block(0,0,vec.rows(),mNumDims);
	} else {
		mEigenVectors = vec;
	}
}
예제 #13
0
//! Function to evaluate a set of double and vector-returning functions and concatenate the results.
Eigen::VectorXd evaluateListOfFunctions(
        const std::vector< boost::function< double( ) > >& doubleFunctionList,
        const std::vector< std::pair< boost::function< Eigen::VectorXd( ) >, int > > vectorFunctionList,
        const int totalSize)
{
    Eigen::VectorXd variableList = Eigen::VectorXd::Zero( totalSize );
    int currentIndex = 0;

    for( unsigned int i = 0; i < doubleFunctionList.size( ); i++ )
    {
        variableList( i ) = doubleFunctionList.at( i )( );
        currentIndex++;
    }

    for( unsigned int i = 0; i < vectorFunctionList.size( ); i++ )
    {
        variableList.segment( currentIndex, vectorFunctionList.at( i ).second ) =
                vectorFunctionList.at( i ).first( );
        currentIndex += vectorFunctionList.at( i ).second;
    }

    // Check consistency with input
    if( currentIndex != totalSize )
    {
        std::string errorMessage = "Error when evaluating lists of functions, sizes are inconsistent: " +
                boost::lexical_cast< std::string >( currentIndex ) + " and " +
                boost::lexical_cast< std::string >( totalSize );
        throw std::runtime_error( errorMessage );
    }

    return variableList;
}
예제 #14
0
void inertialParametersVectorLoop(const UndirectedTree & undirected_tree,
                                  Eigen::VectorXd & parameters_vector)
{
    for(int i=0; i < (int)undirected_tree.getNrOfLinks(); i++ ) {
            parameters_vector.segment(i*10,10) = Vectorize(undirected_tree.getLink(i)->getInertia());
    }
}
예제 #15
0
파일: dirgen.cpp 프로젝트: MRtrix3/mrtrix3
 bool operator() (
     Eigen::VectorXd& newx,
     const Eigen::VectorXd& x,
     const Eigen::VectorXd& g,
     double step_size) {
   newx.noalias() = x - step_size * g;
   for (ssize_t n = 0; n < newx.size(); n += 3)
     newx.segment (n,3).normalize();
   return newx != x;
 }
    // ONLY USE WHEN VECTORS CONTAINS FREE BASE TRANSLATION AND ROTATION
    //      Since the translation and rotation are switched
    //      WBI order [T R Q]
    //      ocra order [R T Q]
    /* static */ bool ocraWbiConversions::wbiToOcraBodyVector(int qdof, const Eigen::VectorXd &v_wbi, Eigen::VectorXd &v_ocra)
    {
        int dof = qdof + DIM_T + DIM_R;
        if(dof != v_wbi.size() || dof != v_ocra.size())
        {
            std::cout<<"ERROR: Input and output matrices - Is the model free root?" <<std::endl;
            return false;
        }

        Eigen::VectorXd t(DIM_T);
        Eigen::VectorXd r(DIM_R);
        Eigen::VectorXd q(qdof);

        t = v_wbi.segment(0, DIM_T);
        r = v_wbi.segment(DIM_T, DIM_R);
        q = v_wbi.segment(DIM_T+DIM_R, qdof);

        v_ocra << r,
                t,
                q;
    }
예제 #17
0
void inertialParametersVectorLoopFakeLinks(const UndirectedTree & undirected_tree,
                                           Eigen::VectorXd & parameters_vector,
                                           std::vector< std::string > fake_links_names)
{
    int real_index_loop = 0;
    for(int i=0; i < (int)undirected_tree.getNrOfLinks(); i++ ) {
        if( std::find(fake_links_names.begin(), fake_links_names.end(), undirected_tree.getLink(i)->getName()) == fake_links_names.end() ) {
            parameters_vector.segment(real_index_loop*10,10) = Vectorize(undirected_tree.getLink(i)->getInertia());
            real_index_loop++;
        }
    }
}
예제 #18
0
파일: World.cpp 프로젝트: ehuang3/dart
Eigen::VectorXd World::evalDeriv()
{
    // compute contact forces
    mCollisionHandle->applyContactForces();

    // compute derivatives for integration
    Eigen::VectorXd deriv = Eigen::VectorXd::Zero(mIndices.back() * 2);

    for (unsigned int i = 0; i < getNumSkeletons(); i++)
    {
        // skip immobile objects in forward simulation
        if (mSkeletons[i]->getImmobileState())
            continue;
        int start = mIndices[i] * 2;
        int size = getSkeleton(i)->getNumDofs();

        Eigen::VectorXd qddot = mSkeletons[i]->getInvMassMatrix()
                                * (-mSkeletons[i]->getCombinedVector()
                                   + mSkeletons[i]->getExternalForces()
                                   + mSkeletons[i]->getInternalForces()
                                   + mCollisionHandle->getConstraintForce(i)
                                   );

        //        Eigen::VectorXd qddot = mSkeletons[i]->getMassMatrix().ldlt().solve(
        //                                    -mSkeletons[i]->getCombinedVector()
        //                                    + mSkeletons[i]->getExternalForces()
        //                                    + mSkeletons[i]->getInternalForces()
        //                                    + mCollisionHandle->getConstraintForce(i)
        //                                    );
        
        // set velocities
        deriv.segment(start, size) = getSkeleton(i)->getPoseVelocity() + (qddot * mTimeStep);

        // set qddot (accelerations)
        deriv.segment(start + size, size) = qddot;
    }

    return deriv;
}
예제 #19
0
void MultiAgent3dNavigation::getStateEstimate(std::vector<tf::Transform> &stateEstimate) {
  Eigen::VectorXd mean = ekf->getMean();
  unsigned int nA = motionModel->getNumAgents();
  unsigned int nT = motionModel->getNumTargets();
  unsigned int aSD = motionModel->getAgentStateDim();
  unsigned int tSD = motionModel->getTargetStateDim();
  stateEstimate.resize(nA+nT);
  assert(addOn3d.size() == nA);
  for (unsigned int i=0; i<nA; ++i) {
    assert(aSD == 3);
    Eigen::Vector3d v = mean.segment(i*aSD, 3);
    stateEstimate[i].setOrigin(tf::Vector3(v(0), v(1), addOn3d[i].z));
    tf::Quaternion q;
    q.setRPY(addOn3d[i].roll, addOn3d[i].pitch, v(2));
    stateEstimate[i].setRotation(q);
  }
  assert(nT == 1);
  for (unsigned int i=0; i<nT; ++i) {
    assert(tSD >= 2);
    Eigen::Vector2d v = mean.segment(nA*aSD+i*tSD, 2);
    stateEstimate[nA+i].setOrigin(tf::Vector3(v(0), v(1), 0));
  }
}
예제 #20
0
파일: World.cpp 프로젝트: ehuang3/dart
void World::setState(const Eigen::VectorXd& _newState)
{
    for (int i = 0; i < getNumSkeletons(); i++)
    {
        int start = mIndices[i] * 2;
        int size = getSkeleton(i)->getNumDofs();

        VectorXd pose = _newState.segment(start, size);
        VectorXd qDot = _newState.segment(start + size, size);
        getSkeleton(i)->clampRotation(pose, qDot);
        if (getSkeleton(i)->getImmobileState())
        {
            // need to update node transformation for collision
            getSkeleton(i)->setPose(pose, true, false);
        }
        else
        {
            // need to update first derivatives for collision
            getSkeleton(i)->setPose(pose, false, true);
            getSkeleton(i)->computeDynamics(mGravity, qDot, true);
        }

    }
}
    void getGoal(Eigen::VectorXd &x) override
    {
        unsigned int offset = 3 * links_ * chainNum_;

        if (links_ == 7)
        {
            Eigen::VectorXd nstep = offset_ * length_;
            Eigen::VectorXd estep =
                Eigen::AngleAxisd(boost::math::constants::pi<double>() / 2, Eigen::Vector3d::UnitZ()) * offset_ *
                length_;
            Eigen::VectorXd sstep =
                Eigen::AngleAxisd(boost::math::constants::pi<double>(), Eigen::Vector3d::UnitZ()) * offset_ * length_;
            Eigen::VectorXd wstep =
                Eigen::AngleAxisd(3 * boost::math::constants::pi<double>() / 2, Eigen::Vector3d::UnitZ()) * offset_ *
                length_;

            Eigen::VectorXd joint = offset_ + nstep;
            x.segment(3 * 0 + offset, 3) = joint;
            x.segment(3 * 1 + offset, 3) = x.segment(3 * 0 + offset, 3) + estep;
            x.segment(3 * 2 + offset, 3) = x.segment(3 * 1 + offset, 3) + estep;
            x.segment(3 * 3 + offset, 3) = x.segment(3 * 2 + offset, 3) + Eigen::Vector3d::UnitZ() * length_;
            x.segment(3 * 4 + offset, 3) = x.segment(3 * 3 + offset, 3) + sstep;
            x.segment(3 * 5 + offset, 3) = x.segment(3 * 4 + offset, 3) + sstep;
            x.segment(3 * 6 + offset, 3) = x.segment(3 * 5 + offset, 3) + wstep;
        }
        else
        {
            Eigen::VectorXd step = offset_ * length_;
            Eigen::VectorXd joint = offset_ + step;

            unsigned int i = 0;
            for (; i < links_ / 2; ++i, joint += step)
                x.segment(3 * i + offset, 3) = joint;

            joint += Eigen::Vector3d::UnitZ() * length_ - step;
            for (; i < links_; ++i, joint -= step)
                x.segment(3 * i + offset, 3) = joint;
        }
    }
    void getStart(Eigen::VectorXd &x) override
    {
        const double angle = boost::math::constants::pi<double>() / 16;
        const unsigned int offset = 3 * links_ * chainNum_;
        const Eigen::VectorXd axis =
            Eigen::AngleAxisd(boost::math::constants::pi<double>() / 2, Eigen::Vector3d::UnitZ()) * offset_;

        const Eigen::VectorXd step = Eigen::Vector3d::UnitZ() * length_;
        Eigen::VectorXd joint = offset_ + Eigen::AngleAxisd(angle, axis) * step;

        unsigned int i = 0;
        for (; i < links_; ++i)
        {
            x.segment(3 * i + offset, 3) = joint;
            if (i < links_ - 2)
                joint += step;
            else
                joint += Eigen::AngleAxisd(-angle, axis) * step;
        }
    }
    void jacobian(const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::MatrixXd> out) const override
    {
        const unsigned int offset = 3 * links_ * chainNum_;
        out.setZero();

        Eigen::VectorXd plus(3 * (links_ + 1));
        plus.head(3 * links_) = x.segment(offset, 3 * links_);
        plus.tail(3) = Eigen::VectorXd::Zero(3);

        Eigen::VectorXd minus(3 * (links_ + 1));
        minus.head(3) = offset_;
        minus.tail(3 * links_) = x.segment(offset, 3 * links_);

        const Eigen::VectorXd diagonal = plus - minus;

        for (unsigned int i = 0; i < links_; i++)
            out.row(i).segment(3 * i + offset, 3) = diagonal.segment(3 * i, 3).normalized();

        out.block(1, offset, links_ - 1, 3 * links_ - 3) -= out.block(1, offset + 3, links_ - 1, 3 * links_ - 3);
    }
// Computes the difference vector of all design variables between the linearization point at marginalization and the current guess, on the tangent space (i.e. log(x_bar - x))
Eigen::VectorXd MarginalizationPriorErrorTerm::getDifferenceSinceMarginalization()
{
  Eigen::VectorXd diff = Eigen::VectorXd(_dimensionDesignVariables);
  diff.setZero();
  int index = 0;
  std::vector<Eigen::MatrixXd>::iterator it_marg = _designVariableValuesAtMarginalization.begin();
  std::vector<aslam::backend::DesignVariable*>::iterator it_current = _designVariables.begin();
  for(;it_current != _designVariables.end(); ++it_current, ++it_marg)
  {
	  // retrieve current value (xbar) and value at marginalization(xHat)
      Eigen::MatrixXd xHat = *it_marg;
      //get minimal difference in tangent space
      Eigen::VectorXd diffVector;
      (*it_current)->minimalDifference(xHat, diffVector);
      //int base = index;
      int dim = diffVector.rows();
      diff.segment(index, dim) = diffVector;
      index += dim;
  }
  return diff;
}
void ExperimentalTrajectory::initializeTrajectory()
{
    originalWaypoints = waypoints;
    kernelLengthParameter = pointToPointDurationVector.mean();
    // Add a waypoint to the end.
    int extraWaypoints = 1;
    int nStartWp = extraWaypoints;
    int nEndWp = extraWaypoints;

    int nWpNew = nWaypoints + nStartWp + nEndWp;

    Eigen::MatrixXd waypointsTmp = Eigen::MatrixXd::Zero(nDoF, nWpNew);

    waypointsTmp.leftCols(nStartWp) = waypoints.leftCols(1).replicate(1,nStartWp);
    waypointsTmp.middleCols(nStartWp, nWaypoints) = waypoints;
    waypointsTmp.rightCols(nEndWp) = waypoints.rightCols(1).replicate(1,nEndWp);

    waypoints.resize(nDoF, nWaypoints);
    waypoints = waypointsTmp;
    // std::cout << pointToPointDurationVector << std::endl;


    Eigen::VectorXd durationVectorTmp = Eigen::VectorXd::Zero(nWpNew-1);

    double extraWpDt = 0.01 * kernelLengthParameter;
    // double extraWpDt = 0.01 * pointToPointDurationVector.sum();
    // std::cout << "extraWpDt: " << extraWpDt << std::endl;

    durationVectorTmp.head(nStartWp) = Eigen::VectorXd::Constant(nStartWp, extraWpDt);
    durationVectorTmp.segment(nStartWp, nWaypoints-1) = pointToPointDurationVector;
    durationVectorTmp.tail(nEndWp) = Eigen::VectorXd::Constant(nEndWp, extraWpDt);

    pointToPointDurationVector.resize(durationVectorTmp.size());
    pointToPointDurationVector = durationVectorTmp;

    nWaypoints = nWpNew;

    std::cout << pointToPointDurationVector << std::endl;
    calculateVarianceParameters();
}
예제 #26
0
int main(int argc, char **argv) {
  Random::randomize();
  std::string paramfile = "multi.ini";
  int test = 1;

  char c;
  while ((c = getopt(argc, argv, "p:h")) != EOF) {
    switch (c) {
      case 'p':
        paramfile = optarg;
        break;
      case 'h':
      default:
        std::cerr << "Usage: " << argv[0] << " [options]\n";
        std::cerr << "\nOptions:\n";
        std::cerr << "-p <file>:  use the given parameter file\n";
        std::cerr << "-h:         this help\n";
        return 1;
    }
  }

  TParam p;
  p.loadTree(paramfile);

  MultiAgentMotionModel *motionModel;
  std::string typestr = p("multi_rotor_control/type").toString();
  if (typestr == "point2d") {
    motionModel = new Point2dMotionModel();
  } else if (typestr == "rotor2d") {
    motionModel = new Rotor2dMotionModel();
  } else {
    std::cerr << "Error: Unknown type '" << typestr << "' - exiting\n";
    exit(-1);
  }
  motionModel->init(p);

  TargetTrackingController ttc;
  ttc.init(p);
  std::vector<const SensorModel*> sensorModels = ttc.getTopology().getSensorModels();

  if (test == 1) {
    // online test
    TargetTrajectory tt;
    tt.init(p);
    EKF ekf(motionModel);
    ekf.init(p);

    std::ofstream topo_out("topo.out");
    std::ofstream multi_out("multi.out");
    unsigned int nA = motionModel->getNumAgents();
    unsigned int nT = motionModel->getNumTargets();
    unsigned int aSD = motionModel->getAgentStateDim();
    unsigned int cSD = motionModel->getAgentControlDim();
    unsigned int tSD = motionModel->getTargetStateDim();

    // test output
    Eigen::VectorXd state = p("estimation/initialState").toVectorXd();
    for (unsigned int i=0; !tt.atEnd(); ++i) {
      Eigen::VectorXd mean = ekf.getMean();
      Eigen::MatrixXd cov = ekf.getCovariance();
//      Eigen::VectorXd control(nA*cSD);
//      Eigen::VectorXd targetPosition = state.segment(aSD*nA, cSD);
//      for (unsigned int i=0; i<nA; ++i) {
//        control.segment(cSD*i, cSD) = (targetPosition - state.segment(aSD*i, cSD))/p("estimation/motionDt").toDouble();
//      }
      Eigen::VectorXd control = ttc.getControlTopo(&ekf, motionModel, sensorModels);
      multi_out << aSD << " " << cSD << " " << tSD << " 0 0 " << nA << " " << nT << " 0 0 0"
          << " " << state.transpose()
          << " " << control.transpose()
          << " " << mean.transpose();
      multi_out << " " << Eigen::Map<Eigen::MatrixXd>(cov.data(), 1, cov.cols()*cov.cols());
//      for (int i=0; i<N+1; ++i) {
//        multi_out << " " << cov(2*agentStateDim, 2*agentStateDim) << " " << cov(2*agentStateDim, 2*agentStateDim+1) << " " << cov(2*agentStateDim+1, 2*agentStateDim) << " " << cov(2*agentStateDim+1, 2*agentStateDim+1);
//      }
      multi_out << "\n";
      ttc.getTopology().write(topo_out, state);
      // simulation
      state = motionModel->move(state, control, motionModel->sampleNoise(state, control));
      if (Random::uniform() < p("multi_rotor_control/kidnap/target").toDouble()) {
        // kidnap target
        state.segment(aSD*nA, 2) = tt.randomJump();
        ekf.getCovariance().block(aSD*nA, aSD*nA, 4, 4) = Eigen::MatrixXd::Identity(4, 4)*4.0;
      } else {
        state.segment(aSD*nA, 2) = tt.step();
      }
      if (Random::uniform() < p("multi_rotor_control/kidnap/agent").toDouble()) {
        // kidnap agent
        int agent = rand()%nA;
        state.segment(aSD*agent, 2) = Eigen::Vector2d(Random::uniform(-1, 1), Random::uniform(-0.5, 0.5));
        ekf.getCovariance().block(aSD*agent, aSD*agent, 2, 2) = Eigen::MatrixXd::Identity(2, 2)*4.0;
      }
      // state estimation
      ekf.predict(control);
      for (std::vector<const SensorModel*>::const_iterator it = sensorModels.begin();
          it != sensorModels.end(); ++it) {
        if ((*it)->measurementAvailable(state)) {
          Eigen::VectorXd noiseSample = (*it)->sampleNoise(state, (*it)->sense(state));
          Eigen::VectorXd measurementSample = (*it)->sense(state, noiseSample);
          ekf.correct(measurementSample, *(*it));
        }
      }
    }
  }

  return 0;
}
예제 #27
0
int main() {
  // Logging:
  // --------
  const int nbFile=7;

  std::vector<std::string> name_vec(nbFile);
  name_vec[0]="CoM_X";
  name_vec[1]="CoM_Y";
  name_vec[2]="CoM_Yaw";
  name_vec[3]="CoP_X";
  name_vec[4]="CoP_Y";
  name_vec[5]="BAS_X";
  name_vec[6]="BAS_Y";

  std::vector<std::ofstream*> data_vec(nbFile);
  std::vector<std::ifstream*> ref_vec(nbFile);
  for (int i = 0; i < nbFile; ++i) {
    data_vec[i] = new std::ofstream((name_vec[i]+".data").c_str());
    ref_vec[i] = new std::ifstream((name_vec[i]+".ref").c_str());
  }



  // Create and initialize generator:
  // -------------------------------

  WalkgenAbstract * walk = createWalkgen(CURRENT_QPSOLVERTYPE);

  MPCData mpcData;
  mpcData.weighting.basePositionInt[0]=0;
  mpcData.weighting.basePosition[0]=10;
  walk->init(mpcData);



  EnvData envData;
  Eigen::VectorXd obstacleRadius(3);
  Eigen::VectorXd obstaclePositionX(3);
  Eigen::VectorXd obstaclePositionY(3);
  obstacleRadius(0) = 1;
  obstaclePositionX(0) = 2;
  obstaclePositionY(0) = -0.2;
  obstacleRadius(1) = 0.5;
  obstaclePositionX(1) = 4;
  obstaclePositionY(1) = 0.1;
  obstacleRadius(2) = 0.5;
  obstaclePositionX(2) = 5;
  obstaclePositionY(2) = -0.3;
  envData.obstacleRadius = obstacleRadius;
  envData.obstaclePositionX = obstaclePositionX;
  envData.obstaclePositionY = obstaclePositionY;
  envData.nbObstacle = 3;


  // Run:
  // ----

  BodyState state = walk->bodyState(COM);

  state.x(1) += 0;
  state.x(2) += 0;
  walk->bodyState(COM, state);

  double velocity = 0.5;
  walk->velReferenceInGlobalFrame(velocity, 0, 0);
  walk->posReferenceInGlobalFrame(0, 0, 0);
  walk->online(0.0);
  double t = 0;
  walk->online(t);
  MPCSolution result;

  Eigen::VectorXd position(10);
  Eigen::VectorXd zero(10);
  Eigen::VectorXd basePos(20);
  basePos.fill(0);
  zero.fill(0);

  for (t += 0.001; t < 45; t += 0.001){

    for(int i=0; i<10; ++i)
    {
      position(i)=0.16*(i+1)*velocity+result.state_vec[1].baseTrajX_(0);
    }

    walk->posReferenceInGlobalFrame(position, zero, zero);
    result = walk->online(t);
    Eigen::VectorXd basePos;
    walk->QPBasePosition(basePos);
    envData.obstacleLinearizationPointX = basePos.segment(0, 10);
    envData.obstacleLinearizationPointY = basePos.segment(10, 10);
    walk->envData(envData);


    dumpTrajectory(result, data_vec);
  }

  // Reopen the files:
  // -----------------
  std::vector<std::ifstream*> check_vec(nbFile);
  for(int i = 0;i < nbFile; ++i){
    check_vec[i] = new std::ifstream((name_vec[i]+".data").c_str());
  }
  bool success = ((fabs(result.state_vec[2].baseTrajX_(0)-velocity)<1e-4)
      &&(fabs(result.state_vec[2].baseTrajY_(0)-0)<1e-4)
      &&(fabs(result.state_vec[2].CoMTrajYaw_(0)-0)<1e-4));
  for(unsigned i = 0; i < check_vec.size();++i){
    // if the reference file exists, compare with the previous version.
    if (*ref_vec[i]){
      if (!checkFiles(*check_vec[i],*ref_vec[i])){
        //success = false;
      }
    }
    // otherwise, create it
    else{
      copyFile((name_vec[i]+".data"), (name_vec[i]+".ref"));
    }
    check_vec[i]->close();
    ref_vec[i]->close();
  }
  makeScilabFile("data", t);
  makeScilabFile("ref", t);

  for (int i=0; i < nbFile; ++i) {
    delete check_vec[i];
    delete ref_vec[i];
    delete data_vec[i];
  }
  delete walk;
  return (success)?0:1;
}
 Eigen::Ref<const Eigen::VectorXd> getTip(const Eigen::VectorXd &x, unsigned int id) const
 {
     return x.segment(3 * links_ * ((id % chains_) + 1) - 3, 3);
 }
 Eigen::Ref<const Eigen::VectorXd> getLink(const Eigen::VectorXd &x, const unsigned int idx) const
 {
     const unsigned int offset = 3 * links_ * chainNum_;
     return x.segment(offset + 3 * idx, 3);
 }
예제 #30
0
void MagCal::calcMagComp()
{
    /*
     * Inspired by
     * http://davidegironi.blogspot.it/2013/01/magnetometer-calibration-helper-01-for.html#.UriTqkMjulM
     *
     * Ellipsoid fit from:
     * http://www.mathworks.com/matlabcentral/fileexchange/24693-ellipsoid-fit
     *
     * To use Eigen to convert matlab code, have a look at Eigen/AsciiQuickReference.txt
     */

    if (mMagSamples.size() < 9) {
        QMessageBox::warning(this, "Magnetometer compensation",
                             "Too few points.");
        return;
    }

    int samples = mMagSamples.size();
    Eigen::VectorXd ex(samples);
    Eigen::VectorXd ey(samples);
    Eigen::VectorXd ez(samples);

    for (int i = 0;i < samples;i++) {
        ex(i) = mMagSamples.at(i).at(0);
        ey(i) = mMagSamples.at(i).at(1);
        ez(i) = mMagSamples.at(i).at(2);
    }

    Eigen::MatrixXd eD(samples, 9);

    for (int i = 0;i < samples;i++) {
        eD(i, 0) = ex(i) * ex(i);
        eD(i, 1) = ey(i) * ey(i);
        eD(i, 2) = ez(i) * ez(i);
        eD(i, 3) = 2.0 * ex(i) * ey(i);
        eD(i, 4) = 2.0 * ex(i) * ez(i);
        eD(i, 5) = 2.0 * ey(i) * ez(i);
        eD(i, 6) = 2.0 * ex(i);
        eD(i, 7) = 2.0 * ey(i);
        eD(i, 8) = 2.0 * ez(i);
    }

    Eigen::MatrixXd etmp1 = eD.transpose() * eD;
    Eigen::MatrixXd etmp2 = eD.transpose() * Eigen::MatrixXd::Ones(samples, 1);
    Eigen::VectorXd eV = etmp1.lu().solve(etmp2);

    Eigen::MatrixXd eA(4, 4);
    eA(0,0)=eV(0);   eA(0,1)=eV(3);   eA(0,2)=eV(4);   eA(0,3)=eV(6);
    eA(1,0)=eV(3);   eA(1,1)=eV(1);   eA(1,2)=eV(5);   eA(1,3)=eV(7);
    eA(2,0)=eV(4);   eA(2,1)=eV(5);   eA(2,2)=eV(2);   eA(2,3)=eV(8);
    eA(3,0)=eV(6);   eA(3,1)=eV(7);   eA(3,2)=eV(8);   eA(3,3)=-1.0;

    Eigen::MatrixXd eCenter = -eA.topLeftCorner(3, 3).lu().solve(eV.segment(6, 3));
    Eigen::MatrixXd eT = Eigen::MatrixXd::Identity(4, 4);
    eT(3, 0) = eCenter(0);
    eT(3, 1) = eCenter(1);
    eT(3, 2) = eCenter(2);

    Eigen::MatrixXd eR = eT * eA * eT.transpose();

    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eEv(eR.topLeftCorner(3, 3) * (-1.0 / eR(3, 3)));
    Eigen::MatrixXd eVecs = eEv.eigenvectors();
    Eigen::MatrixXd eVals = eEv.eigenvalues();

    Eigen::MatrixXd eRadii(3, 1);
    eRadii(0) = sqrt(1.0 / eVals(0));
    eRadii(1) = sqrt(1.0 / eVals(1));
    eRadii(2) = sqrt(1.0 / eVals(2));

    Eigen::MatrixXd eScale = eRadii.asDiagonal().inverse() * eRadii.minCoeff();
    Eigen::MatrixXd eComp = eVecs * eScale * eVecs.transpose();

    mMagComp.resize(9);
    mMagComp[0] = eComp(0, 0);
    mMagComp[1] = eComp(0, 1);
    mMagComp[2] = eComp(0, 2);

    mMagComp[3] = eComp(1, 0);
    mMagComp[4] = eComp(1, 1);
    mMagComp[5] = eComp(1, 2);

    mMagComp[6] = eComp(2, 0);
    mMagComp[7] = eComp(2, 1);
    mMagComp[8] = eComp(2, 2);

    mMagCompCenter.resize(3);
    mMagCompCenter[0] = eCenter(0, 0);
    mMagCompCenter[1] = eCenter(1, 0);
    mMagCompCenter[2] = eCenter(2, 0);

    QVector<double> magX, magY, magZ;

    for (int i = 0;i < mMagSamples.size();i++) {
        double mx = mMagSamples.at(i).at(0);
        double my = mMagSamples.at(i).at(1);
        double mz = mMagSamples.at(i).at(2);

        mx -= mMagCompCenter.at(0);
        my -= mMagCompCenter.at(1);
        mz -= mMagCompCenter.at(2);

        magX.append(mx * mMagComp.at(0) + my * mMagComp.at(1) + mz * mMagComp.at(2));
        magY.append(mx * mMagComp.at(3) + my * mMagComp.at(4) + mz * mMagComp.at(5));
        magZ.append(mx * mMagComp.at(6) + my * mMagComp.at(7) + mz * mMagComp.at(8));
    }

    ui->magSampXyPlot->graph(1)->setData(magX, magY);
    ui->magSampXzPlot->graph(1)->setData(magX, magZ);
    ui->magSampYzPlot->graph(1)->setData(magY, magZ);

    updateMagPlots();
}