multiprecision-mpfr.cpp

Go to the documentation of this file.

//
// Copyright (c) 2020 INRIA
//
 
#include "pinocchio/math/multiprecision-mpfr.hpp"
 
#include <boost/math/special_functions/gamma.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>
#include <iostream>
 
#include "pinocchio/algorithm/aba.hpp"
#include "pinocchio/algorithm/center-of-mass.hpp"
#include "pinocchio/algorithm/centroidal.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/math/multiprecision.hpp"
#include "pinocchio/multibody/sample-models.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:
  mpfr_float_100 b = 2;
  std::cout << std::numeric_limits<mpfr_float_100>::digits << std::endl;
  std::cout << std::numeric_limits<mpfr_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<mpfr_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(mpfr_float_100(1000)) << std::endl;
}
 
BOOST_AUTO_TEST_CASE(test_sincos)
{
  using namespace boost::multiprecision;
  typedef mpfr_float_100 heap_float_100;
  typedef number<mpfr_float_backend<100, allocate_stack>> stack_float_100;
  {
    heap_float_100 x;
    heap_float_100 s;
    heap_float_100 c;
    x = 100;
    pinocchio::SINCOS(x, &s, &c);
    BOOST_CHECK(s == sin(x));
    BOOST_CHECK(c == cos(x));
  }
  {
    stack_float_100 x;
    stack_float_100 s;
    stack_float_100 c;
    x = 100;
    pinocchio::SINCOS(x, &s, &c);
    BOOST_CHECK(s == sin(x));
    BOOST_CHECK(c == cos(x));
  }
}
 
BOOST_AUTO_TEST_CASE(test_cast)
{
  typedef boost::multiprecision::mpfr_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;
 
  Model model;
  pinocchio::buildModels::humanoidRandom(model);
  Data data(model);
 
  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill(1.);
 
  typedef boost::multiprecision::mpfr_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, tau_multi, Convention::WORLD);
  aba(model, data, q, v, tau, Convention::WORLD);
 
  BOOST_CHECK_IS_APPROX(data.ddq, data_multi.ddq, double);
 
  // Mass matrix
  crba(model_multi, data_multi, q_multi, Convention::WORLD);
  data_multi.M.triangularView<Eigen::StrictlyLower>() =
    data_multi.M.transpose().triangularView<Eigen::StrictlyLower>();
 
  crba(model, data, q, Convention::WORLD);
  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()