Commit cf1789f1 authored by Olivier Stasse's avatar Olivier Stasse

Merge branch 'devel' of https://github.com/jmirabel/sot-core into cmake-export

parents b46f49da 51f1d3d3
......@@ -480,6 +480,23 @@ struct MatrixToRPY : public UnaryOpHeader<MatrixRotation, VectorRollPitchYaw> {
};
REGISTER_UNARY_OP(MatrixToRPY, MatrixToRPY);
struct RPYToQuaternion : public UnaryOpHeader<VectorRollPitchYaw, VectorQuaternion> {
void operator()(const VectorRollPitchYaw &r, VectorQuaternion &res) {
res = (Eigen::AngleAxisd(r(2), Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd(r(1), Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(r(0), Eigen::Vector3d::UnitX()))
.toRotationMatrix();
}
};
REGISTER_UNARY_OP(RPYToQuaternion, RPYToQuaternion);
struct QuaternionToRPY : public UnaryOpHeader<VectorQuaternion, VectorRollPitchYaw> {
void operator()(const VectorQuaternion &r, VectorRollPitchYaw &res) {
res = (r.toRotationMatrix().eulerAngles(2, 1, 0)).reverse();
}
};
REGISTER_UNARY_OP(QuaternionToRPY, QuaternionToRPY);
struct QuaternionToMatrix
: public UnaryOpHeader<VectorQuaternion, MatrixRotation> {
void operator()(const VectorQuaternion &r, MatrixRotation &res) {
......
......@@ -23,5 +23,8 @@ template <> DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SwitchVector, "SwitchVector");
typedef Switch<bool, int> SwitchBool;
template <> DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SwitchBool, "SwitchBoolean");
typedef Switch<MatrixHomogeneous, int> SwitchMatrixHomogeneous;
template <> DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SwitchMatrixHomogeneous, "SwitchMatrixHomogeneous");
} // namespace sot
} // namespace dynamicgraph
......@@ -141,3 +141,54 @@ BOOST_AUTO_TEST_CASE(test_matrix_selector) {
output << aMatrixSelector->getClassName();
BOOST_CHECK(output.is_equal("Selec_of_matrix"));
}
BOOST_AUTO_TEST_SUITE(test_rotation_conversions)
template<typename type> type random();
template<> VectorRollPitchYaw random<VectorRollPitchYaw>()
{ return VectorRollPitchYaw::Random(); }
template<> VectorQuaternion random<VectorQuaternion>()
{ return VectorQuaternion(Eigen::Vector4d::Random().normalized()); }
template<> MatrixRotation random<MatrixRotation>()
{ return MatrixRotation(random<VectorQuaternion>()); }
template<typename type> bool compare(const type& a, const type& b)
{
return a.isApprox (b);
}
template<> bool compare<VectorQuaternion>(const VectorQuaternion& a, const VectorQuaternion& b)
{
return a.isApprox (b) || a.coeffs().isApprox(-b.coeffs());
}
template<typename AtoB, typename BtoA>
void test_impl ()
{
typedef typename AtoB::Tin A;
typedef typename AtoB::Tout B;
AtoB a2b;
BtoA b2a;
for (std::size_t i = 1; i < 10; ++i) {
A ain = random<A>(), aout;
B b;
a2b (ain, b);
b2a (b, aout);
BOOST_CHECK(compare(ain, aout));
}
}
BOOST_AUTO_TEST_CASE(matrix_rpy) {
test_impl<MatrixToRPY, RPYToMatrix>();
}
BOOST_AUTO_TEST_CASE(quaternion_rpy) {
test_impl<QuaternionToRPY, RPYToQuaternion>();
}
BOOST_AUTO_TEST_CASE(matrix_quaternion) {
test_impl<MatrixToQuaternion, QuaternionToMatrix>();
}
BOOST_AUTO_TEST_SUITE_END()
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