test: add multiprecision test

unittest/CMakeLists.txt

@@ -123,6 +123,9 @@ ADD_PINOCCHIO_UNIT_TEST(centroidal-derivatives)
 ADD_PINOCCHIO_UNIT_TEST(center-of-mass-derivatives)
 ADD_PINOCCHIO_UNIT_TEST(contact-dynamics-derivatives)
 
+# Multiprecision arithmetic
+ADD_PINOCCHIO_UNIT_TEST(boost-multiprecision)
+
 # Automatic differentiation
 IF(CPPAD_FOUND AND BUILD_WITH_AUTODIFF_SUPPORT)
   ADD_PINOCCHIO_UNIT_TEST(cppad-basic cppad)

unittest/boost-multiprecision.cpp

+//
+// Copyright (c) 2020 INRIA
+//
+
+#include "pinocchio/parsers/urdf.hpp"
+#include "pinocchio/algorithm/rnea.hpp"
+#include "pinocchio/algorithm/aba.hpp"
+#include "pinocchio/algorithm/jacobian.hpp"
+#include "pinocchio/algorithm/center-of-mass.hpp"
+#include "pinocchio/algorithm/joint-configuration.hpp"
+#include "pinocchio/algorithm/crba.hpp"
+#include "pinocchio/algorithm/centroidal.hpp"
+#include "pinocchio/parsers/sample-models.hpp"
+
+#include "pinocchio/math/boost-multiprecision.hpp"
+
+#include <boost/multiprecision/cpp_bin_float.hpp>
+#include <boost/math/special_functions/gamma.hpp>
+
+#include <iostream>
+
+#include <boost/test/unit_test.hpp>
+#include <boost/utility/binary.hpp>
+
+BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
+
+BOOST_AUTO_TEST_CASE(test_basic)
+{
+  using namespace boost::multiprecision;
+
+  // Operations at fixed precision and full numeric_limits support:
+  cpp_bin_float_100 b = 2;
+  std::cout << std::numeric_limits<cpp_bin_float_100>::digits << std::endl;
+  std::cout << std::numeric_limits<cpp_bin_float_100>::digits10 << std::endl;
+  // We can use any C++ std lib function, lets print all the digits as well:
+  std::cout << std::setprecision(std::numeric_limits<cpp_bin_float_100>::max_digits10)
+  << log(b) << std::endl; // print log(2)
+                          // We can also use any function from Boost.Math:
+  std::cout << boost::math::tgamma(b) << std::endl;
+  // These even work when the argument is an expression template:
+  std::cout << boost::math::tgamma(b * b) << std::endl;
+  // And since we have an extended exponent range we can generate some really large
+  // numbers here (4.0238726007709377354370243e+2564):
+  std::cout << boost::math::tgamma(cpp_bin_float_100(1000)) << std::endl;
+}
+
+BOOST_AUTO_TEST_CASE(test_cast)
+{
+  typedef boost::multiprecision::cpp_bin_float_100 float_100;
+  
+  // Test Scalar cast
+  double initial_value = boost::math::constants::pi<double>();
+  float_100 value_100(initial_value);
+  double value_cast = value_100.convert_to<double>();
+  BOOST_CHECK(initial_value == value_cast);
+  
+  typedef Eigen::Matrix<float_100,Eigen::Dynamic,1> VectorFloat100;
+  static const Eigen::DenseIndex dim = 100;
+  Eigen::VectorXd initial_vec = Eigen::VectorXd::Random(dim);
+  VectorFloat100 vec_float_100 = initial_vec.cast<float_100>();
+  Eigen::VectorXd vec = vec_float_100.cast<double>();
+  
+  BOOST_CHECK(vec == initial_vec);
+}
+
+#define BOOST_CHECK_IS_APPROX(double_field,multires_field,Scalar) \
+  BOOST_CHECK(double_field.isApprox(multires_field.cast<Scalar>()))
+
+BOOST_AUTO_TEST_CASE(test_mutliprecision)
+{
+  using namespace pinocchio;
+  const std::string urdf_filename = PINOCCHIO_MODEL_DIR + std::string("/others/robots/talos_data/urdf/talos_reduced.urdf");
+  
+  Model model;
+  pinocchio::urdf::buildModel(urdf_filename, model);
+  Data data(model);
+  
+  model.lowerPositionLimit.head<3>().fill(-1.);
+  model.upperPositionLimit.head<3>().fill( 1.);
+  
+  typedef boost::multiprecision::cpp_bin_float_100 float_100;
+  typedef ModelTpl<float_100> ModelMulti;
+  typedef DataTpl<float_100> DataMulti;
+  
+  ModelMulti model_multi = model.cast<float_100>();
+  DataMulti data_multi(model_multi);
+  
+  ModelMulti::ConfigVectorType q_multi = randomConfiguration(model_multi);
+  ModelMulti::TangentVectorType v_multi = ModelMulti::TangentVectorType::Random(model_multi.nv);
+  ModelMulti::TangentVectorType a_multi = ModelMulti::TangentVectorType::Random(model_multi.nv);
+  ModelMulti::TangentVectorType tau_multi = ModelMulti::TangentVectorType::Random(model_multi.nv);
+    
+  //  Model::ConfigVectorType q = randomConfiguration(model);
+  //  Model::TangentVectorType v = Model::TangentVectorType::Random(model.nv);
+  //  Model::TangentVectorType a = Model::TangentVectorType::Random(model.nv);
+  //  Model::TangentVectorType tau = Model::TangentVectorType::Random(model.nv);
+  
+  Model::ConfigVectorType q = q_multi.cast<double>();
+  Model::TangentVectorType v = v_multi.cast<double>();
+  Model::TangentVectorType a = a_multi.cast<double>();
+  Model::TangentVectorType tau = tau_multi.cast<double>();
+  
+  forwardKinematics(model_multi,data_multi,q_multi,v_multi,a_multi);
+  forwardKinematics(model,data,q,v,a);
+  
+  for(JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id)
+  {
+    BOOST_CHECK_IS_APPROX(data.oMi[joint_id],data_multi.oMi[joint_id],double);
+    BOOST_CHECK_IS_APPROX(data.v[joint_id],data_multi.v[joint_id],double);
+    BOOST_CHECK_IS_APPROX(data.a[joint_id],data_multi.a[joint_id],double);
+  }
+  
+  // Jacobians
+  computeJointJacobians(model_multi,data_multi,q_multi);
+  computeJointJacobians(model,data,q);
+  
+  BOOST_CHECK_IS_APPROX(data.J,data_multi.J,double);
+  
+  // Inverse Dynamics
+  rnea(model_multi,data_multi,q_multi,v_multi,a_multi);
+  rnea(model,data,q,v,a);
+  
+  BOOST_CHECK_IS_APPROX(data.tau,data_multi.tau,double);
+  
+  // Forward Dynamics
+  aba(model_multi,data_multi,q_multi,v_multi,a_multi);
+  aba(model,data,q,v,a);
+  
+  BOOST_CHECK_IS_APPROX(data.ddq,data_multi.ddq,double);
+  
+  // Mass matrix
+  crba(model_multi,data_multi,q_multi);
+  data_multi.M.triangularView<Eigen::StrictlyLower>()
+    = data_multi.M.transpose().triangularView<Eigen::StrictlyLower>();
+
+  crba(model,data,q);
+  data.M.triangularView<Eigen::StrictlyLower>()
+    = data.M.transpose().triangularView<Eigen::StrictlyLower>();
+
+  BOOST_CHECK_IS_APPROX(data.M,data_multi.M,double);
+}
+
+BOOST_AUTO_TEST_SUITE_END()