static void testJacobian(const ExpressionTester<RotationExpression> & expressionTester) {
    auto rotExp = expressionTester.getExp();
    for (int i = 0; i < 3; i++) {
      Eigen::Vector3d p;
      p.setZero();
      p(i) = 1;
      RotationExpressionNodeFunctor functor(rotExp, p);

      sm::eigen::NumericalDiff<RotationExpressionNodeFunctor> numdiff(functor, expressionTester.getEps());

      /// Discern the size of the jacobian container
      Eigen::Matrix3d C = rotExp.toRotationMatrix();
      JacobianContainer Jc(3);
      rotExp.evaluateJacobians(Jc);
      Eigen::Matrix3d Cp_cross = sm::kinematics::crossMx(C * p);
      Jc.applyChainRule(Cp_cross);

      Eigen::VectorXd dp(Jc.cols());
      dp.setZero();
      Eigen::MatrixXd Jest = numdiff.estimateJacobian(dp);

      auto JcM = Jc.asSparseMatrix();
      sm::eigen::assertNear(Jc.asSparseMatrix(), Jest, expressionTester.getTolerance(), SM_SOURCE_FILE_POS, static_cast<std::stringstream&>(std::stringstream("Testing the RotationExpression's Jacobian (column=") << i <<")").str());
      if (expressionTester.getPrintResult()) {
        std::cout << "Jest=\n" << Jest << std::endl;
        std::cout << "Jc=\n" << JcM << std::endl;
      }
    }
  }
Exemple #2
0
/**
 * @function straightPath
 * @brief Builds a straight path -- easy
 */
bool goWSOrient::straightPath( const Eigen::VectorXd &_startConfig, 
		     	       const Eigen::Transform<double, 3, Eigen::Affine> &_goalPose,
	          	       std::vector<Eigen::VectorXd> &path )
{
  //-- Copy input 
  startConfig = _startConfig;
  goalPose = _goalPose;
  goalPos = _goalPose.translation();

  //-- Create needed variables
  Eigen::VectorXd q; Eigen::VectorXd wsPos;
  Eigen::Transform<double, 3, Eigen::Affine> T;
  Eigen::MatrixXd Jc(6, 7); Eigen::MatrixXd Jcinv(7, 6);

  Eigen::MatrixXd delta_q;
  double ws_dist;

  //-- Initialize variables
  q = startConfig; path.push_back( q );
  ws_dist = DBL_MAX;

  //-- Loop
  int trials = 0;
  while( ws_dist > goalThresh )
  {
    //-- Get ws position
    robinaLeftArm_fk( q, TWBase, Tee, T );
    wsPos = T.translation();

    Eigen::VectorXd wsOrient(3); 
    getRPY( T, wsOrient(0), wsOrient(1), wsOrient(2) );

    //-- Get the Jacobian

    robinaLeftArm_jc( q, TWBase, Tee, Jc );

    std::cout<< " JC: " << std::endl << Jc << std::endl;

    for( int i = 3; i < 6;i++ )
    { for( int j = 0; j < 7; j++ )
      {
        Jc(i,j) = Jc(i,j)*0.001;
      }
    }

    std::cout<< " JC switch: " << std::endl << Jc << std::endl;

    //-- Get the pseudo-inverse(easy way)
    pseudoInv( 6, 7, Jc, Jcinv );

    std::cout<< " JCW Inv: " << std::endl << Jcinv << std::endl;

    Eigen::VectorXd goalOrient(3);
    getRPY( goalPose, goalOrient(0), goalOrient(1), goalOrient(2) ); 

    //-- Get delta (orientation + position)
    Eigen::VectorXd delta(6);
    delta.head(3) = (goalPos - wsPos); delta.tail(3) = (goalOrient - wsOrient);

    //-- Get delta_q (jointspace)   
    delta_q = Jcinv*delta;


    //-- Scale
    double scal = stepSize/delta_q.norm();
    delta_q = delta_q*scal;

    //-- Add to q
    q = q + delta_q; 

    //-- Push it 
    path.push_back( q );

    //-- Check distance to goal
    ws_dist = (goalPos - wsPos).norm();
    printf(" -- Ws dist: %.3f \n", ws_dist );
    trials++;      
    
    if( trials > maxTrials )
     { break; }
  }  


  if( trials >= maxTrials )
   { path.clear();
     printf(" --(!) Did not get to the goal . Thresh: %.3f Trials: %d \n", ws_dist, trials ); 
     return false;}
  else
   { printf(" -- Got to the goal . Thresh: %.3f Trials: %d \n", ws_dist, trials ); 
     return true; }

}
Exemple #3
0
/**
 * @function balancePath
 * @brief Builds a straight path -- easy
 */
bool goWSOrient::balancePath( const Eigen::VectorXd &_startConfig, 
		     	      const Eigen::Transform<double, 3, Eigen::Affine> &_goalPose,
	          	      std::vector<Eigen::VectorXd> &path )
{

  srand( time(NULL) );
  //-- Copy input 
  startConfig = _startConfig;
  goalPose = _goalPose;
  goalPos = _goalPose.translation();

  //-- Create needed variables
  Eigen::VectorXd q; Eigen::VectorXd wsPos;
  Eigen::Transform<double, 3, Eigen::Affine> T;
  Eigen::MatrixXd Jc(3, 7); Eigen::MatrixXd Jcinv(7, 3);

  Eigen::MatrixXd delta_q;
  double ws_dist;

  //-- Initialize variables
  q = startConfig; path.push_back( q );
  ws_dist = DBL_MAX;

  //-- Loop
  int trials = 0;
    //-- Get ws position
    robinaLeftArm_fk( q, TWBase, Tee, T );
    wsPos = T.translation();

    Eigen::VectorXd wsOrient(3); 
    getRPY( T, wsOrient(0), wsOrient(1), wsOrient(2) );


    Eigen::VectorXd phi(7);
    Eigen::VectorXd dq(7);   
    Eigen::VectorXd qorig = q;   

      //-- Get the Jacobian
      robinaLeftArm_j( q, TWBase, Tee, Jc );

      //-- Get the pseudo-inverse(easy way)
      pseudoInv( 3, 7, Jc, Jcinv );
    int count = 0;
    for( int i = 0; i < 1000; i++ )
    {

     for( int j = 0; j < 7; j++ )
     { phi[j] = rand()%6400 / 10000.0 ; }

      //-- Get a null space projection displacement that does not affect the thing
      dq = ( Eigen::MatrixXd::Identity(7,7) - Jcinv*Jc )*phi;

      //-- Add to q 
      //q = qorig + dq;
      Eigen::VectorXd newq;
      newq = qorig + dq;

     //-- Calculate distance
     Eigen::Transform<double, 3, Eigen::Affine> Torig;
     Eigen::Transform<double, 3, Eigen::Affine> Tphi;
     robinaLeftArm_fk( newq, TWBase, Tee, Tphi );
     robinaLeftArm_fk( qorig, TWBase, Tee, Torig );
     double dist = ( Tphi.translation() - Torig.translation() ).norm();

     if( dist < 0.015 )
     { 
       count++;
       q = newq;
       printf("--> [%d] Distance to the original xyz is: %.3f \n", count, dist );
       //-- Push it 
       path.push_back( q );
     }

    }

  return true;
}