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;
}
}
}
/**
* @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; }
}
/**
* @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;
}
