C++ VectorXd::norm方法代码示例

void testSolverSpeed(const std::string inputFilePath,const std::string outputFilePath,const int sizeThreshold,std::string solverType,user_IndexTree & usrTree){
  Eigen::MatrixXd inputMatrix = readBinaryIntoMatrixXd(inputFilePath);
  int matrixSize = inputMatrix.rows();
  Eigen::VectorXd exactSoln = Eigen::VectorXd::LinSpaced(Eigen::Sequential,matrixSize,-2,2);
  Eigen::VectorXd inputF = inputMatrix * exactSoln;
  HODLR_Matrix test_HODLR(inputMatrix,sizeThreshold,usrTree);
  
  std::ofstream outputFile_Error;
  std::ofstream outputFile_Speed;
  std::string errorFileName = outputFilePath + "errorVsTolerance";
  std::string speedFileName = outputFilePath + "speedVsTolerance";
  outputFile_Error.open(errorFileName.c_str());
  outputFile_Speed.open(speedFileName.c_str());

  double accuracyVal[] = {1e-5,1e-6,1e-7,1e-8};
  std::vector<double> accuracy(accuracyVal, accuracyVal + sizeof(accuracyVal) / sizeof(double));
  for (unsigned int i = 0; i < accuracy.size();i++){
    test_HODLR.set_LRTolerance(accuracy[i]);
    Eigen::VectorXd solverSoln;
    if (solverType == "recLU"){
      solverSoln = test_HODLR.recLU_Solve(inputF);
      outputFile_Speed<<accuracy[i]<<" "<<test_HODLR.get_recLU_TotalTime()<<std::endl;
    }else if (solverType == "extendedSp"){
      solverSoln = test_HODLR.extendedSp_Solve(inputF);
      outputFile_Speed<<accuracy[i]<<" "<<test_HODLR.get_extendedSp_TotalTime()<<std::endl;
    }
    Eigen::VectorXd difference = solverSoln - exactSoln;
    double relError = difference.norm()/exactSoln.norm();
    outputFile_Error<<accuracy[i]<<" "<<relError<<std::endl;
  }
  outputFile_Error.close();
  outputFile_Speed.close();
}

bool UpperBodyPlanner::checkTrajectory(const moveit_msgs::RobotTrajectory& input_trajectory, moveit_msgs::RobotTrajectory& output_trajectory) {
    moveit_msgs::RobotTrajectory check_trajectory = input_trajectory;
    int input_traj_size = input_trajectory.joint_trajectory.points.size();
    if (input_traj_size == 0) {
        ROS_ERROR("No points in the trajectory.");
        singularity_check = true;
        return false;
    }
    else if (input_traj_size == 1) {
        ROS_INFO("1 point in the trajectory.");
        output_trajectory = check_trajectory;
        singularity_check = true;
        return true;
    }
    int i = 0;
    singularity_check = true;
    while (i < (check_trajectory.joint_trajectory.points.size() - 1)) {
        //std::cout << "Checking point number is: " << i << std::endl;
        Eigen::VectorXd front = stdVec2EigenXd(check_trajectory.joint_trajectory.points[i].positions);
        Eigen::VectorXd back = stdVec2EigenXd(check_trajectory.joint_trajectory.points[i + 1].positions);
        Eigen::VectorXd difference = front - back;
        if (difference.norm() == 0) {
            //std::cout << "The point " << i << " and " << i + 1 << " is too close." << std::endl;
            //std::cout << "Erase point " << i + 1 << " points" << std::endl;
            check_trajectory.joint_trajectory.points.erase(check_trajectory.joint_trajectory.points.begin() + (i + 1));
        }
        else {
            if (difference.norm() >= 0.5) {
                ROS_WARN("Hit singularity");
                singularity_check = false;
            }
            i++;
        }
    }
    //double fix_time = check_trajectory.joint_trajectory.points[0].time_from_start.toSec();
    double fix_time = path_step / move_velocity;
    Eigen::VectorXd velocity_cmd = Eigen::VectorXd::Zero(7);
    double start_t = 0;
    for (int j = 0; j < check_trajectory.joint_trajectory.points.size(); j++) {
        if (j == 0) {
            start_t = start_t + 2 * fix_time;
            velocity_cmd = Eigen::VectorXd::Zero(7);
        }
        else if (j == (check_trajectory.joint_trajectory.points.size() - 1)) {
            start_t = start_t + 2 * fix_time;
            velocity_cmd = Eigen::VectorXd::Zero(7);
        }
        else {
            Eigen::VectorXd current_joint_angle = stdVec2EigenXd(check_trajectory.joint_trajectory.points[j + 1].positions);
            Eigen::VectorXd last_joint_angle = stdVec2EigenXd(check_trajectory.joint_trajectory.points[j].positions);
            velocity_cmd = (current_joint_angle - last_joint_angle) / fix_time;
        }
        start_t = start_t + fix_time;
        ros::Duration start_time((start_t));
        check_trajectory.joint_trajectory.points[j].time_from_start = start_time;
        check_trajectory.joint_trajectory.points[j].velocities = EigenXd2stdVec(velocity_cmd);
    }
    output_trajectory = check_trajectory;
    return true;
}

/**
 * @function heuristicCost
 * @brief
 */
double M_RRT::heuristicCost( Eigen::VectorXd node )
{

  Eigen::Transform<double, 3, Eigen::Affine> T;

  // Calculate the EE position
  robinaLeftArm_fk( node, TWBase, Tee, T );

  Eigen::VectorXd trans_ee = T.translation();
  Eigen::VectorXd x_ee = T.rotation().col(0);
  Eigen::VectorXd y_ee = T.rotation().col(1);
  Eigen::VectorXd z_ee = T.rotation().col(2);

  Eigen::VectorXd GH = ( goalPosition - trans_ee );

  double fx1 = GH.norm() ;

  GH = GH/GH.norm();

  double fx2 = abs( GH.dot( x_ee ) - 1 );

  double fx3 = abs( GH.dot( z_ee ) );

  double heuristic = w1*fx1 + w2*fx2 + w3*fx3;     

  return heuristic;
}

/**
 * @function GoToXYZ
 */
bool JTFollower::GoToXYZ( Eigen::VectorXd &_q, 
			  Eigen::VectorXd _targetXYZ, 
			  std::vector<Eigen::VectorXd> &_workspacePath ) {

  Eigen::VectorXd dXYZ;
  Eigen::VectorXd dConfig;
  int iter;
  mWorld->getRobot(mRobotId)->update();
  
  //-- Initialize
  dXYZ = ( _targetXYZ - GetXYZ(_q) ); // GetXYZ also updates the config to _q, so Jaclin use an updated value
  iter = 0;
  //printf("New call to GoToXYZ: dXYZ: %f  \n", dXYZ.norm() );
  while( dXYZ.norm() > mWorkspaceThresh && iter < mMaxIter ) {
    //printf("XYZ Error: %f \n", dXYZ.norm() );
    Eigen::MatrixXd Jt = GetPseudoInvJac(_q);
    dConfig = Jt*dXYZ;
    //printf("dConfig : %.3f \n", dConfig.norm() );
    if( dConfig.norm() > mConfigStep ) {
      double n = dConfig.norm();
      dConfig = dConfig *(mConfigStep/n);
      //printf("NEW dConfig : %.3f \n", dConfig.norm() );
    }
    _q = _q + dConfig;
    _workspacePath.push_back( _q );
    
    dXYZ = (_targetXYZ - GetXYZ(_q) );
    iter++;
  }
  
  if( iter >= mMaxIter ) { return false; }
  else { return true; }
  
}

/* Function: testACASolverConv1DUniformPts
 * ---------------------------------------
 * This function creates a convergence plot of solver relative error vs ACA tolerance for a dense self interaction matrix.
 * The test dense matrix, is an interaction matrix arising from the self interaction of uniform points on a 1D interval.
 * intervalMin : Lower bound of the 1D interval.
 * intervalMax : Upper bound of the 1D interval.
 * numPts : Number of interacting points (matrix size).
 * diagValue : The diagonal entry value of the dense matrix.
 * exactSoln : The test right hand side of the linear system.
 * outputFileName : Path of the output file.
 * kernel : Pointer to the kernel function. 
 * solverType : Type of HODLR solver. Default is recLU.
 */ 
void testACASolverConv1DUnifromPts(const double intervalMin,const double intervalMax, const int numPts, const double diagValue, Eigen::VectorXd exactSoln, std::string outputFileName, double (*kernel)(const double r),std::string solverType){
  assert(intervalMax > intervalMin);
  assert(numPts > 0);
  assert(exactSoln.rows() == numPts);
  int minTol = -5;
  int maxTol = -10;
  int sizeThreshold = 30;
  
  Eigen::MatrixXd denseMatrix = makeMatrix1DUniformPts (intervalMin, intervalMax, intervalMin, intervalMax, numPts, numPts, diagValue, kernel);
  Eigen::VectorXd RHS = denseMatrix * exactSoln;
  HODLR_Matrix denseHODLR(denseMatrix, sizeThreshold);
  std::ofstream outputFile;
  outputFile.open(outputFileName.c_str());
  for (int i = minTol; i >= maxTol; i--){
    double tol = pow(10,i);
    denseHODLR.set_LRTolerance(tol);
    Eigen::VectorXd solverSoln;
    if (solverType == "recLU")
      solverSoln = denseHODLR.recLU_Solve(RHS);
    if (solverType == "extendedSp")
      solverSoln = denseHODLR.extendedSp_Solve(RHS);
    Eigen::VectorXd residual = solverSoln-exactSoln;
    double relError = residual.norm()/exactSoln.norm();
    outputFile<<tol<<"       "<<relError<<std::endl;
  }
  outputFile.close();
}

 Residuals(const Problem& problem, const Omega& omega, const Psi& psi)
     : normOfC{problem.c.norm()},
       normOfB{problem.b.norm()},
       AXPlusS{problem.A * omega.x + psi.s},
       ATransposeY{problem.A.transpose() * omega.y},
       cTransposeX{problem.c.transpose() * omega.x},
       bTranspoesY{problem.b.transpose() * omega.y},
       primal{getPrimal(problem, omega, psi)},
       dual{getDual(problem, omega)},
       primalDualGap{getGap(problem, omega)},
       unbounded{cTransposeX < 0 ? AXPlusS.norm() * normOfC / -cTransposeX
                                 : NAN},
       infeasible{bTranspoesY < 0 ? ATransposeY.norm() * normOfB / -bTranspoesY
                                  : NAN} {}

/**
 * @function tryStepGoal
 */
M_RRT::StepResult M_RRT::tryStepGoal( const Eigen::VectorXd &qtry, int NNidx )
{
  VectorXd qnear( ndim );
  VectorXd qnew( ndim );
  qnear = configVector[NNidx];

  Eigen::VectorXd diff = ( qtry - qnear );
  double edist = diff.norm();

  // If the new node is nearer than the stepSize, don't add it

  if( edist < stepSize )
   { return STEP_REACHED; } 

  // Scale it in order to add it
  double scale = stepSize / edist;
  for( int i = 0; i < ndim; i++ )
  { qnew[i] = qnear[i] + diff[i]*scale; }

  double qnearCost = heuristicCost( qnear );
  double qnewCost = heuristicCost(  qnew );

  //-- Check if there are not collisions are if the heuristic distance decreases
  if( checkCollisions(qnew) )
    { return STEP_COLLISION; }
  else if( qnewCost > qnearCost )
    { return STEP_NO_NEARER;}
  else  
    { addNode( qnew, NNidx );
      return STEP_PROGRESS; }
}

void FindClosestQuad:: find
(
  mesh::Quad& quad )
{
  TRACE(quad.vertex(0).getCoords(), quad.vertex(1).getCoords(), quad.vertex(2).getCoords() , quad.vertex(3).getCoords());

  // Methodology of book "Computational Geometry", Joseph O' Rourke, Chapter 7.2
  auto& a = quad.vertex(0).getCoords();
  auto& b = quad.vertex(1).getCoords();
  auto& c = quad.vertex(2).getCoords();
  auto& d = quad.vertex(3).getCoords();
  auto& norm = quad.getNormal();

  auto ret = math::barycenter::calcBarycentricCoordsForQuad(a, b, c, d, norm, _searchPoint);
  assertion(ret.barycentricCoords.size() == 4);

  const bool inside = not (ret.barycentricCoords.array() < - math::NUMERICAL_ZERO_DIFFERENCE).any();

  // if valid, compute distance to triangle and evtl. store distance
  if (inside) {
    Eigen::VectorXd distanceVector = ret.projected;
    distanceVector -= _searchPoint;
    double distance = distanceVector.norm();
    if ( _shortestDistance > distance ) {
      _shortestDistance = distance;
      _vectorToProjectionPoint = distanceVector;
      _parametersProjectionPoint[0] = ret.barycentricCoords[0];
      _parametersProjectionPoint[1] = ret.barycentricCoords[1];
      _parametersProjectionPoint[2] = ret.barycentricCoords[2];
      _parametersProjectionPoint[3] = ret.barycentricCoords[3];
      _closestQuad = &quad;
    }
  }
}

/**
 * @function tryStep
 */
JG_RRT::StepResult JG_RRT::tryStep( const Eigen::VectorXd &qtry, int NNidx )
{
    Eigen::VectorXd qnear( ndim );
    Eigen::VectorXd qnew( ndim );
    qnear = configVector[NNidx];

    Eigen::VectorXd diff = ( qtry - qnear );
    double edist = diff.norm();

    // If the new node is nearer than the stepSize, don't add it

    if( edist < stepSize )
    {
        return STEP_REACHED;
    }

    // Scale it in order to add it
    double scale = stepSize / edist;
    for( int i = 0; i < ndim; i++ )
    {
        qnew[i] = qnear[i] + diff[i]*scale;
    }

    if( !checkCollisions(qnew) )
    {
        addNode( qnew, NNidx );
        return STEP_PROGRESS;
    }
    else
    {
        return STEP_COLLISION;
    }
}

void dampnewton(const FuncType &F, const JacType &DF,
                Eigen::VectorXd &x, double rtol = 1e-4,double atol = 1e-6)
{
    const int n = x.size();
    const double lmin = 1E-3; // Minimal damping factor
    double lambda = 1.0; // Initial and actual damping factor
    Eigen::VectorXd s(n),st(n); // Newton corrections
    Eigen::VectorXd xn(n);      // Tentative new iterate
    double sn,stn;    // Norms of Newton corrections
    
    do {
        auto jacfac = DF(x).lu(); // LU-factorize Jacobian
        
        s = jacfac.solve(F(x));   // Newton correction
        sn = s.norm();            // Norm of Newton correction
        lambda *= 2.0;
        do {
            lambda /= 2;
            if (lambda < lmin) throw "No convergence: lambda -> 0";
            xn = x-lambda*s;           // {\bf Tentative next iterate}
            st = jacfac.solve(F(xn));  // Simplified Newton correction
            stn = st.norm();
        }
        while (stn > (1-lambda/2)*sn); // {\bf Natural monotonicity test}
        x = xn; // Now: xn accepted as new iterate
        lambda = std::min(2.0*lambda,1.0); // Try to mitigate damping
    }
    // Termination based on simplified Newton correction
    while ((stn > rtol*x.norm()) && (stn > atol));
}

/* ********************************************************************************************* */
Vector Factors::RestPlate::errorProxy(const LieVector& a0, const LieVector& a1, 
		const LieVector& b0, const LieVector& b1) const {

	// Get the two lines
	Eigen::VectorXd a0v = a0.vector(), a1v = a1.vector(), b0v = b0.vector(), b1v = b1.vector();
	Eigen::VectorXd aLine = a1v - a0v, bLine = b1v - b0v;
	double aLength = aLine.norm(), bLength = bLine.norm();
	aLine = aLine / aLength, bLine = bLine / bLength;
	Eigen::Vector2d aPerp(-aLine(1), aLine(0)), bPerp(-bLine(1), bLine(0));

	// Get the two projections (perpendicular projection should be 0.0).
	Eigen::VectorXd va = b0v - a0v, vb = a1v - b0v;
	double aParaProj = aLine(0) * va(0) + aLine(1) * va(1);
	double bParaProj = bLine(0) * vb(0) + bLine(1) * vb(1);
	double aPerpError = aPerp(0) * va(0) + aPerp(1) * va(1);
	double bPerpError = bPerp(0) * vb(0) + bPerp(1) * vb(1);

	// Get the parallel errors
	double aParaError = 0.0, bParaError = 0.0;
	if(aParaProj > aLength) aParaError = aParaProj - aLength;
	else if(aParaProj < 0.0) aParaError = aParaProj;
	if(bParaProj > bLength / 2) bParaError = bParaProj - bLength / 2;
	else if(bParaProj < 0.0) bParaError = bParaProj;

	// Send the minimal error
	if(fabs(aPerpError) < fabs(bPerpError)) return Vector_(2, aPerpError, aParaError);
	else return Vector_(2, bPerpError, bParaError);
}

void randomUnitSphere(Eigen::VectorXd &sp) {
    static boost::random::mt19937 rng(time(NULL));
    static boost::random::normal_distribution<> normal;

    do {
        sp(0) = normal(rng);
        sp(1) = normal(rng);
        sp(2) = normal(rng);
    } while (sp.norm() < 0.00001);
    sp.normalize();
}

void randomUnitQuaternion(Eigen::VectorXd &quat) {
    static boost::random::mt19937 rng(time(NULL));
    static boost::random::normal_distribution<> normal;

    do {
        quat(0) = normal(rng);
        quat(1) = normal(rng);
        quat(2) = normal(rng);
        quat(3) = normal(rng);
    } while (quat.norm() < 0.00001);
    quat.normalize();
}

// Convert difference in desired and sensed forces to cartesian velocity.
// Implemented as constant step (equal to force_tracking_gain) in the direction of force error.
// This works with the integration (bug) in the low level controller. It relies on the low level
// controller integrating the constant error until it reaches the desired force
void forceErrorToVelocity(const Eigen::VectorXd& force_err, Eigen::VectorXd& cart_vel) {

	double force_err_nrm;

	if(bOrientCtrl) {
		force_err_nrm = force_err.norm();
	} else {
		double force_error = force_err[0]*force_err[0]+force_err[1]*force_err[1]+force_err[2]*force_err[2];
		force_err_nrm = sqrt(force_error);
	}

	if(bOrientCtrl) {
		for(int i=0; i<6; ++i) {
			cart_vel[i] = force_err[i]/force_err_nrm*force_tracking_gain;
		}
	} else {
		for(int i=0; i<3; ++i) {
			cart_vel[i] = force_err[i]/force_err_nrm*force_tracking_gain;
		}
	}
}

/**
 * @function tryStepFromNode
 * @brief Tries to extend tree towards provided sample (must be overridden for MBP ) 
 */
B1RRT::StepResult B1RRT::tryStepFromNode( const Eigen::VectorXd &_qtry, int _NNIdx ) {
    
    /// Calculates a new node to grow towards _qtry, check for collisions and adds to tree 
    Eigen::VectorXd qnear = configVector[_NNIdx];

    /// Compute direction and magnitude
    Eigen::VectorXd diff = _qtry - qnear;
    double dist = diff.norm();
 
    if( dist < stepSize ) { 
        return STEP_REACHED; 
    }

    /// Scale this vector to stepSize and add to end of qnear
    Eigen::VectorXd qnew = qnear + diff*(stepSize/dist);

    if( !checkCollisions(qnew) ) {
        addNode( qnew, _NNIdx );
        return STEP_PROGRESS;
    } else {
        return STEP_COLLISION;
    }

}