Verified Commit 182a650c authored by Justin Carpentier's avatar Justin Carpentier
Browse files

test: add multiprecision test

parent 25372a73
......@@ -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)
......
//
// 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()
Supports Markdown
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