Unverified Commit 39c2436e authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #1279 from aelkazdadi/mpfr-sincos

provide SINCOSAlgo specialization for boost mpfr float types
parents 6717a27f e20b2138
Pipeline #10777 passed with stage
in 213 minutes and 48 seconds
//
// Copyright (c) 2020 INRIA
//
#ifndef __pinocchio_math_mutliprecision_mpfr_hpp__
#define __pinocchio_math_mutliprecision_mpfr_hpp__
#include "pinocchio/math/multiprecision.hpp"
#include "pinocchio/math/sincos.hpp"
#include <boost/serialization/nvp.hpp>
#include <boost/multiprecision/mpfr.hpp>
namespace pinocchio
{
template <
unsigned S_digits10, boost::multiprecision::mpfr_allocation_type S_alloc,
boost::multiprecision::expression_template_option S_et, unsigned C_digits10,
boost::multiprecision::mpfr_allocation_type C_alloc,
boost::multiprecision::expression_template_option C_et, unsigned X_digits10,
boost::multiprecision::mpfr_allocation_type X_alloc,
boost::multiprecision::expression_template_option X_et>
struct SINCOSAlgo<
boost::multiprecision::number<
boost::multiprecision::mpfr_float_backend<X_digits10, X_alloc>, X_et>,
boost::multiprecision::number<
boost::multiprecision::mpfr_float_backend<S_digits10, S_alloc>, S_et>,
boost::multiprecision::number<
boost::multiprecision::mpfr_float_backend<C_digits10, C_alloc>, C_et>>
{
static void run(
boost::multiprecision::number<
boost::multiprecision::mpfr_float_backend<X_digits10, X_alloc>,
X_et> const& a,
boost::multiprecision::number<
boost::multiprecision::mpfr_float_backend<S_digits10, S_alloc>, S_et>*
sa,
boost::multiprecision::number<
boost::multiprecision::mpfr_float_backend<C_digits10, C_alloc>, C_et>*
ca)
{
mpfr_srcptr x_mpfr((a.backend().data()));
mpfr_ptr s_mpfr(sa->backend().data());
mpfr_ptr c_mpfr(ca->backend().data());
mpfr_sin_cos(s_mpfr, c_mpfr, x_mpfr, MPFR_RNDN);
}
};
} // namespace pinocchio
#endif // ifndef __pinocchio_math_mutliprecision_hpp__
......@@ -14,7 +14,7 @@ sudo apt update
sudo free -m -t
# Setup environment variables.
export APT_DEPENDENCIES="doxygen libboost-system-dev libboost-test-dev libboost-filesystem-dev libboost-program-options-dev libeigen3-dev liburdfdom-dev texlive-font-utils"
export APT_DEPENDENCIES="doxygen libboost-system-dev libboost-test-dev libboost-filesystem-dev libboost-program-options-dev libeigen3-dev liburdfdom-dev texlive-font-utils libmpfr-dev"
# Add Python dependency
echo "TRAVIS_PYTHON_VERSION=$TRAVIS_PYTHON_VERSION"
......
......@@ -32,6 +32,11 @@ MACRO(ADD_PINOCCHIO_UNIT_TEST NAME)
TARGET_LINK_LIBRARIES(${TEST_NAME} PRIVATE ${Boost_UNIT_TEST_FRAMEWORK_LIBRARY})
ENDMACRO(ADD_PINOCCHIO_UNIT_TEST)
MACRO(ADD_OPTIONAL_COMPILE_DEPENDENCY PKG_CONFIG_STRING)
_GET_PKG_CONFIG_DEBUG_STRING("${PKG_CONFIG_STRING}" ${ARGN})
ADD_DEPENDENCY(0 1 ${PKG_CONFIG_STRING} "${PKG_CONFIG_DEBUG_STRING}")
ENDMACRO()
# --- RULES -------------------------------------------------------------------
# --- RULES -------------------------------------------------------------------
# --- RULES -------------------------------------------------------------------
......@@ -135,6 +140,16 @@ ADD_PINOCCHIO_UNIT_TEST(contact-dynamics-derivatives)
IF(BUILD_ADVANCED_TESTING)
ADD_PINOCCHIO_UNIT_TEST(multiprecision)
SET_PROPERTY(TARGET test-cpp-multiprecision PROPERTY CXX_STANDARD 11)
ADD_OPTIONAL_COMPILE_DEPENDENCY(gmp)
ADD_OPTIONAL_COMPILE_DEPENDENCY(mpfr)
IF(MPFR_FOUND)
ADD_PINOCCHIO_UNIT_TEST(multiprecision-mpfr)
PKG_CONFIG_USE_DEPENDENCY(test-cpp-multiprecision-mpfr gmp)
PKG_CONFIG_USE_DEPENDENCY(test-cpp-multiprecision-mpfr mpfr)
SET_PROPERTY(TARGET test-cpp-multiprecision-mpfr PROPERTY CXX_STANDARD 11)
ENDIF(MPFR_FOUND)
ENDIF(BUILD_ADVANCED_TESTING)
# Automatic differentiation
......
//
// 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/parsers/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);
aba(model, data, q, v, tau);
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()
......@@ -121,8 +121,8 @@ BOOST_AUTO_TEST_CASE(test_mutliprecision)
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);
aba(model_multi,data_multi,q_multi,v_multi,tau_multi);
aba(model,data,q,v,tau);
BOOST_CHECK_IS_APPROX(data.ddq,data_multi.ddq,double);
......
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