Commit 12132aac authored by Gabriele Buondonno's avatar Gabriele Buondonno
Browse files

[unittest] Simplify test_rpyToMatrix

parent 3beb5df2
......@@ -15,34 +15,26 @@ BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
BOOST_AUTO_TEST_CASE(test_rpyToMatrix)
{
#ifdef NDEBUG
const int n = 1e5;
#else
const int n = 1e2;
#endif
for(int k = 0; k < n ; ++k)
{
double r = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;
double p = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/M_PI)) - (M_PI/2);
double y = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;
Eigen::Matrix3d R = pinocchio::rpy::rpyToMatrix(r, p, y);
Eigen::Matrix3d Raa = (Eigen::AngleAxisd(y, Eigen::Vector3d::UnitZ())
* Eigen::AngleAxisd(p, Eigen::Vector3d::UnitY())
* Eigen::AngleAxisd(r, Eigen::Vector3d::UnitX())
).toRotationMatrix();
BOOST_CHECK(R.isApprox(Raa));
Eigen::Vector3d v;
v << r, p, y;
Eigen::Matrix3d Rv = pinocchio::rpy::rpyToMatrix(v);
BOOST_CHECK(Rv.isApprox(Raa));
BOOST_CHECK(Rv.isApprox(R));
}
double r = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;
double p = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/M_PI)) - (M_PI/2);
double y = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;
Eigen::Matrix3d R = pinocchio::rpy::rpyToMatrix(r, p, y);
Eigen::Matrix3d Raa = (Eigen::AngleAxisd(y, Eigen::Vector3d::UnitZ())
* Eigen::AngleAxisd(p, Eigen::Vector3d::UnitY())
* Eigen::AngleAxisd(r, Eigen::Vector3d::UnitX())
).toRotationMatrix();
BOOST_CHECK(R.isApprox(Raa));
Eigen::Vector3d v;
v << r, p, y;
Eigen::Matrix3d Rv = pinocchio::rpy::rpyToMatrix(v);
BOOST_CHECK(Rv.isApprox(Raa));
BOOST_CHECK(Rv.isApprox(R));
}
BOOST_AUTO_TEST_CASE(test_matrixToRpy)
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment