diff --git a/unittest/CMakeLists.txt b/unittest/CMakeLists.txt index 4fc59455da1b5f60431c71178582216057e13582..c5d1460732ca3e42779be78e14e5c59e8ca66e41 100644 --- a/unittest/CMakeLists.txt +++ b/unittest/CMakeLists.txt @@ -122,6 +122,7 @@ ADD_PINOCCHIO_UNIT_TEST(aba-derivatives eigen3) # Automatic differentiation IF(CPPAD_FOUND) ADD_PINOCCHIO_UNIT_TEST(cppad-basic "eigen3;cppad") + ADD_PINOCCHIO_UNIT_TEST(cppad-spatial "eigen3;cppad") ADD_PINOCCHIO_UNIT_TEST(cppad-joints "eigen3;cppad") ADD_PINOCCHIO_UNIT_TEST(cppad-algo "eigen3;cppad") ENDIF(CPPAD_FOUND) diff --git a/unittest/cppad-spatial.cpp b/unittest/cppad-spatial.cpp new file mode 100644 index 0000000000000000000000000000000000000000..33e1c2123fe101baa4e0afee736315707f96cbd7 --- /dev/null +++ b/unittest/cppad-spatial.cpp @@ -0,0 +1,298 @@ +// +// Copyright (c) 2018 CNRS +// +// This file is part of Pinocchio +// Pinocchio is free software: you can redistribute it +// and/or modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation, either version +// 3 of the License, or (at your option) any later version. +// +// Pinocchio is distributed in the hope that it will be +// useful, but WITHOUT ANY WARRANTY; without even the implied warranty +// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Lesser Public License for more details. You should have +// received a copy of the GNU Lesser General Public License along with +// Pinocchio If not, see +// <http://www.gnu.org/licenses/>. + +#include "pinocchio/fwd.hpp" +#include "pinocchio/spatial/se3.hpp" +#include "pinocchio/spatial/motion.hpp" +#include "pinocchio/spatial/explog.hpp" + +#include <iostream> + +#include <boost/test/unit_test.hpp> +#include <boost/utility/binary.hpp> + +BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) + +template<typename Vector3Like> +Eigen::Matrix<typename Vector3Like::Scalar,3,3,0> +computeV(const Eigen::MatrixBase<Vector3Like> & v3) +{ + typedef typename Vector3Like::Scalar Scalar; + typedef Eigen::Matrix<Scalar,3,3,0> ReturnType; + typedef ReturnType Matrix3; + + Scalar t2 = v3.squaredNorm(); + const Scalar t = se3::math::sqrt(t2); + Scalar alpha, beta, zeta; + + if (t < 1e-4) + { + alpha = Scalar(1) + t2/Scalar(6) - t2*t2/Scalar(120); + beta = Scalar(1)/Scalar(2) - t2/Scalar(24); + zeta = Scalar(1)/Scalar(6) - t2/Scalar(120); + } + else + { + Scalar st,ct; se3::SINCOS(t,&st,&ct); + alpha = st/t; + beta = (1-ct)/t2; + zeta = (1 - alpha)/(t2); + } + + Matrix3 V + = alpha * Matrix3::Identity() + + beta * se3::skew(v3) + + zeta * v3 * v3.transpose(); + + return V; +} + +template<typename Vector3Like> +Eigen::Matrix<typename Vector3Like::Scalar,3,3,0> +computeVinv(const Eigen::MatrixBase<Vector3Like> & v3) +{ + typedef typename Vector3Like::Scalar Scalar; + typedef Eigen::Matrix<Scalar,3,3,0> ReturnType; + typedef ReturnType Matrix3; + + Scalar t2 = v3.squaredNorm(); + const Scalar t = se3::math::sqrt(t2); + + Scalar alpha, beta; + if (t < 1e-4) + { + alpha = Scalar(1) - t2/Scalar(12) - t2*t2/Scalar(720); + beta = Scalar(1)/Scalar(12) + t2/Scalar(720); + } + else + { + Scalar st,ct; se3::SINCOS(t,&st,&ct); + alpha = t*st/(Scalar(2)*(Scalar(1)-ct)); + beta = Scalar(1)/t2 - st/(Scalar(2)*t*(Scalar(1)-ct)); + } + + Matrix3 Vinv + = alpha * Matrix3::Identity() + - 0.5 * se3::skew(v3) + + beta * v3 * v3.transpose(); + + return Vinv; +} + +BOOST_AUTO_TEST_CASE(test_log3) +{ + using CppAD::AD; + using CppAD::NearEqual; + + typedef double Scalar; + typedef AD<Scalar> ADScalar; + + typedef se3::SE3Tpl<Scalar> SE3; + typedef se3::MotionTpl<Scalar> Motion; + typedef se3::SE3Tpl<ADScalar> ADSE3; + typedef se3::MotionTpl<ADScalar> ADMotion; + + Motion v(Motion::Zero()); + SE3 M(SE3::Random()); M.translation().setZero(); + + SE3::Matrix3 rot_next = M.rotation() * se3::exp3(v.angular()); + + SE3::Matrix3 Jlog3; + se3::Jlog3(M.rotation(), Jlog3); + + typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic> Matrix; + typedef Eigen::Matrix<ADScalar,Eigen::Dynamic,1> ADVector; + + ADMotion ad_v(v.cast<ADScalar>()); + ADSE3 ad_M(M.cast<ADScalar>()); + ADSE3::Matrix3 rot = ad_M.rotation(); + + ADVector X(3); + + X = ad_v.angular(); + + CppAD::Independent(X); + ADMotion::Vector3 X_ = X; + + ADSE3::Matrix3 ad_rot_next = rot * se3::exp3(X_); + ADMotion::Vector3 log_R_next = se3::log3(ad_rot_next); + + ADVector Y(3); + Y = log_R_next; + + CppAD::ADFun<Scalar> map(X,Y); + + CPPAD_TESTVECTOR(Scalar) x(3); + Eigen::Map<Motion::Vector3>(x.data()).setZero(); + + CPPAD_TESTVECTOR(Scalar) nu_next_vec = map.Forward(0,x); + Motion::Vector3 nu_next(Eigen::Map<Motion::Vector3>(nu_next_vec.data())); + + SE3::Matrix3 rot_next_from_map = se3::exp3(nu_next); + + CPPAD_TESTVECTOR(Scalar) jac = map.Jacobian(x); + + Matrix jacobian = Eigen::Map<EIGEN_PLAIN_ROW_MAJOR_TYPE(Matrix)>(jac.data(),3,3); + + BOOST_CHECK(rot_next_from_map.isApprox(rot_next)); + BOOST_CHECK(jacobian.isApprox(Jlog3)); +} + +BOOST_AUTO_TEST_CASE(test_explog_translation) +{ + using CppAD::AD; + using CppAD::NearEqual; + + typedef double Scalar; + typedef AD<Scalar> ADScalar; + + typedef se3::SE3Tpl<Scalar> SE3; + typedef se3::MotionTpl<Scalar> Motion; + typedef se3::SE3Tpl<ADScalar> ADSE3; + typedef se3::MotionTpl<ADScalar> ADMotion; + + Motion v(Motion::Zero()); + SE3 M(SE3::Random()); //M.rotation().setIdentity(); + + { + Motion::Vector3 v3_test; v3_test.setRandom(); + SE3::Matrix3 V = computeV(v3_test); + SE3::Matrix3 Vinv = computeVinv(v3_test); + + BOOST_CHECK((V*Vinv).isIdentity()); + } + + SE3 M_next = M * se3::exp6(v); +// BOOST_CHECK(M_next.rotation().isIdentity()); + + typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic> Matrix; + typedef Eigen::Matrix<ADScalar,Eigen::Dynamic,1> ADVector; + + ADMotion ad_v(v.cast<ADScalar>()); + ADSE3 ad_M(M.cast<ADScalar>()); + + ADVector X(6); + + X = ad_v.toVector(); + + CppAD::Independent(X); + ADMotion::Vector6 X_ = X; + + se3::MotionRef<ADMotion::Vector6> ad_v_ref(X_); + ADSE3 ad_M_next = ad_M * se3::exp6(ad_v_ref); + + ADVector Y(6); + Y.head<3>() = ad_M_next.translation(); + Y.tail<3>() = ad_M.translation() + ad_M.rotation() * computeV(ad_v_ref.angular()) * ad_v_ref.linear(); + + CppAD::ADFun<Scalar> map(X,Y); + + CPPAD_TESTVECTOR(Scalar) x((size_t)X.size()); + Eigen::Map<Motion::Vector6>(x.data()).setZero(); + + CPPAD_TESTVECTOR(Scalar) translation_vec = map.Forward(0,x); + Motion::Vector3 translation1(Eigen::Map<Motion::Vector3>(translation_vec.data())); + Motion::Vector3 translation2(Eigen::Map<Motion::Vector3>(translation_vec.data()+3)); + BOOST_CHECK(translation1.isApprox(M_next.translation())); + BOOST_CHECK(translation2.isApprox(M_next.translation())); + + CPPAD_TESTVECTOR(Scalar) jac = map.Jacobian(x); + + Matrix jacobian = Eigen::Map<EIGEN_PLAIN_ROW_MAJOR_TYPE(Matrix)>(jac.data(),Y.size(),X.size()); + + BOOST_CHECK(jacobian.topLeftCorner(3,3).isApprox(M.rotation())); + +} + + +BOOST_AUTO_TEST_CASE(test_explog) +{ + using CppAD::AD; + using CppAD::NearEqual; + + typedef double Scalar; + typedef AD<Scalar> ADScalar; + + typedef se3::SE3Tpl<Scalar> SE3; + typedef se3::MotionTpl<Scalar> Motion; + typedef se3::SE3Tpl<ADScalar> ADSE3; + typedef se3::MotionTpl<ADScalar> ADMotion; + + Motion v(Motion::Zero()); + SE3 M(SE3::Random()); //M.translation().setZero(); + + SE3::Matrix6 Jlog6; + se3::Jlog6(M, Jlog6); + + typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic> Matrix; + typedef Eigen::Matrix<ADScalar,Eigen::Dynamic,1> ADVector; + + ADMotion ad_v(v.cast<ADScalar>()); + ADSE3 ad_M(M.cast<ADScalar>()); + + ADVector X(6); + + X.segment<3>(Motion::LINEAR) = ad_v.linear(); + X.segment<3>(Motion::ANGULAR) = ad_v.angular(); + + CppAD::Independent(X); + ADMotion::Vector6 X_ = X; + se3::MotionRef< ADMotion::Vector6> v_X(X_); + + ADSE3 ad_M_next = ad_M * se3::exp6(v_X); + ADMotion ad_log_M_next = se3::log6(ad_M_next); + + ADVector Y(6); + Y.segment<3>(Motion::LINEAR) = ad_log_M_next.linear(); + Y.segment<3>(Motion::ANGULAR) = ad_log_M_next.angular(); + + CppAD::ADFun<Scalar> map(X,Y); + + CPPAD_TESTVECTOR(Scalar) x(6); + Eigen::Map<Motion::Vector6>(x.data()).setZero(); + + CPPAD_TESTVECTOR(Scalar) nu_next_vec = map.Forward(0,x); + Motion nu_next(Eigen::Map<Motion::Vector6>(nu_next_vec.data())); + + CPPAD_TESTVECTOR(Scalar) jac = map.Jacobian(x); + + Matrix jacobian = Eigen::Map<EIGEN_PLAIN_ROW_MAJOR_TYPE(Matrix)>(jac.data(),6,6); + + // Check using finite differencies + Motion dv(Motion::Zero()); + typedef Eigen::Matrix<Scalar,6,6> Matrix6; + Matrix6 Jlog6_fd(Matrix6::Zero()); + Motion v_plus, v0(log6(M)); + + const Scalar eps = 1e-8; + for(int k = 0; k < 6; ++k) + { + dv.toVector()[k] = eps; + SE3 M_plus = M * exp6(dv); + v_plus = log6(M_plus); + Jlog6_fd.col(k) = (v_plus-v0).toVector()/eps; + dv.toVector()[k] = 0; + } + + SE3::Matrix6 Jlog6_analytic; + se3::Jlog6(M, Jlog6_analytic); + + BOOST_CHECK(Jlog6.isApprox(Jlog6_analytic)); + BOOST_CHECK(Jlog6_fd.isApprox(Jlog6,se3::math::sqrt(eps))); +} + +BOOST_AUTO_TEST_SUITE_END()