multiprecision-mpfr.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2020 INRIA
3 //
4 
6 
7 #include <boost/math/special_functions/gamma.hpp>
8 #include <boost/multiprecision/cpp_dec_float.hpp>
9 #include <boost/test/unit_test.hpp>
10 #include <boost/utility/binary.hpp>
11 #include <iostream>
12 
22 
23 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
24 
26 {
27  using namespace boost::multiprecision;
28 
29  // Operations at fixed precision and full numeric_limits support:
30  mpfr_float_100 b = 2;
31  std::cout << std::numeric_limits<mpfr_float_100>::digits << std::endl;
32  std::cout << std::numeric_limits<mpfr_float_100>::digits10 << std::endl;
33  // We can use any C++ std lib function, lets print all the digits as well:
34  std::cout << std::setprecision(std::numeric_limits<mpfr_float_100>::max_digits10) << log(b)
35  << std::endl; // print log(2)
36  // We can also use any function from Boost.Math:
37  std::cout << boost::math::tgamma(b) << std::endl;
38  // These even work when the argument is an expression template:
39  std::cout << boost::math::tgamma(b * b) << std::endl;
40  // And since we have an extended exponent range we can generate some really
41  // large numbers here (4.0238726007709377354370243e+2564):
42  std::cout << boost::math::tgamma(mpfr_float_100(1000)) << std::endl;
43 }
44 
46 {
47  using namespace boost::multiprecision;
48  typedef mpfr_float_100 heap_float_100;
49  typedef number<mpfr_float_backend<100, allocate_stack>> stack_float_100;
50  {
51  heap_float_100 x;
52  heap_float_100 s;
53  heap_float_100 c;
54  x = 100;
55  pinocchio::SINCOS(x, &s, &c);
56  BOOST_CHECK(s == sin(x));
57  BOOST_CHECK(c == cos(x));
58  }
59  {
60  stack_float_100 x;
61  stack_float_100 s;
62  stack_float_100 c;
63  x = 100;
64  pinocchio::SINCOS(x, &s, &c);
65  BOOST_CHECK(s == sin(x));
66  BOOST_CHECK(c == cos(x));
67  }
68 }
69 
71 {
72  typedef boost::multiprecision::mpfr_float_100 float_100;
73 
74  // Test Scalar cast
75  double initial_value = boost::math::constants::pi<double>();
76  float_100 value_100(initial_value);
77  double value_cast = value_100.convert_to<double>();
78  BOOST_CHECK(initial_value == value_cast);
79 
80  typedef Eigen::Matrix<float_100, Eigen::Dynamic, 1> VectorFloat100;
81  static const Eigen::DenseIndex dim = 100;
82  Eigen::VectorXd initial_vec = Eigen::VectorXd::Random(dim);
83  VectorFloat100 vec_float_100 = initial_vec.cast<float_100>();
84  Eigen::VectorXd vec = vec_float_100.cast<double>();
85 
86  BOOST_CHECK(vec == initial_vec);
87 }
88 
89 #define BOOST_CHECK_IS_APPROX(double_field, multires_field, Scalar) \
90  BOOST_CHECK(double_field.isApprox(multires_field.cast<Scalar>()))
91 
92 BOOST_AUTO_TEST_CASE(test_mutliprecision)
93 {
94  using namespace pinocchio;
95 
96  Model model;
98  Data data(model);
99 
100  model.lowerPositionLimit.head<3>().fill(-1.);
101  model.upperPositionLimit.head<3>().fill(1.);
102 
103  typedef boost::multiprecision::mpfr_float_100 float_100;
104  typedef ModelTpl<float_100> ModelMulti;
105  typedef DataTpl<float_100> DataMulti;
106 
107  ModelMulti model_multi = model.cast<float_100>();
108  DataMulti data_multi(model_multi);
109 
110  ModelMulti::ConfigVectorType q_multi = randomConfiguration(model_multi);
111  ModelMulti::TangentVectorType v_multi = ModelMulti::TangentVectorType::Random(model_multi.nv);
112  ModelMulti::TangentVectorType a_multi = ModelMulti::TangentVectorType::Random(model_multi.nv);
113  ModelMulti::TangentVectorType tau_multi = ModelMulti::TangentVectorType::Random(model_multi.nv);
114 
115  // Model::ConfigVectorType q = randomConfiguration(model);
116  // Model::TangentVectorType v = Model::TangentVectorType::Random(model.nv);
117  // Model::TangentVectorType a = Model::TangentVectorType::Random(model.nv);
118  // Model::TangentVectorType tau = Model::TangentVectorType::Random(model.nv);
119 
120  Model::ConfigVectorType q = q_multi.cast<double>();
121  Model::TangentVectorType v = v_multi.cast<double>();
122  Model::TangentVectorType a = a_multi.cast<double>();
123  Model::TangentVectorType tau = tau_multi.cast<double>();
124 
125  forwardKinematics(model_multi, data_multi, q_multi, v_multi, a_multi);
127 
128  for (JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id)
129  {
130  BOOST_CHECK_IS_APPROX(data.oMi[joint_id], data_multi.oMi[joint_id], double);
131  BOOST_CHECK_IS_APPROX(data.v[joint_id], data_multi.v[joint_id], double);
132  BOOST_CHECK_IS_APPROX(data.a[joint_id], data_multi.a[joint_id], double);
133  }
134 
135  // Jacobians
136  computeJointJacobians(model_multi, data_multi, q_multi);
138 
139  BOOST_CHECK_IS_APPROX(data.J, data_multi.J, double);
140 
141  // Inverse Dynamics
142  rnea(model_multi, data_multi, q_multi, v_multi, a_multi);
143  rnea(model, data, q, v, a);
144 
145  BOOST_CHECK_IS_APPROX(data.tau, data_multi.tau, double);
146 
147  // Forward Dynamics
148  aba(model_multi, data_multi, q_multi, v_multi, tau_multi, Convention::WORLD);
150 
151  BOOST_CHECK_IS_APPROX(data.ddq, data_multi.ddq, double);
152 
153  // Mass matrix
154  crba(model_multi, data_multi, q_multi, Convention::WORLD);
155  data_multi.M.triangularView<Eigen::StrictlyLower>() =
156  data_multi.M.transpose().triangularView<Eigen::StrictlyLower>();
157 
159  data.M.triangularView<Eigen::StrictlyLower>() =
160  data.M.transpose().triangularView<Eigen::StrictlyLower>();
161 
162  BOOST_CHECK_IS_APPROX(data.M, data_multi.M, double);
163 }
164 
165 BOOST_AUTO_TEST_SUITE_END()
pinocchio::DataTpl
Definition: context/generic.hpp:25
pinocchio::forwardKinematics
void forwardKinematics(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
Update the joint placements according to the current joint configuration.
c
Vec3f c
pinocchio::Convention::WORLD
@ WORLD
setup.data
data
Definition: cmake/cython/setup.in.py:48
vec
vec
pinocchio::buildModels::humanoidRandom
void humanoidRandom(ModelTpl< Scalar, Options, JointCollectionTpl > &model, bool usingFF=true)
Create a humanoid kinematic tree with 6-DOF limbs and random joint placements.
pinocchio::crba
const DataTpl< Scalar, Options, JointCollectionTpl >::MatrixXs & crba(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Convention convention=Convention::LOCAL)
Computes the upper triangular part of the joint space inertia matrix M by using the Composite Rigid B...
pinocchio::aba
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & aba(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType1 > &v, const Eigen::MatrixBase< TangentVectorType2 > &tau, const Convention convention=Convention::LOCAL)
The Articulated-Body algorithm. It computes the forward dynamics, aka the joint accelerations given t...
pinocchio::computeJointJacobians
const DataTpl< Scalar, Options, JointCollectionTpl >::Matrix6x & computeJointJacobians(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
Computes the full model Jacobian, i.e. the stack of all motion subspace expressed in the world frame....
pinocchio::SINCOS
void SINCOS(const S1 &a, S2 *sa, S3 *ca)
Computes sin/cos values of a given input scalar.
Definition: sincos.hpp:27
pinocchio::randomConfiguration
void randomConfiguration(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &lowerLimits, const Eigen::MatrixBase< ConfigVectorIn2 > &upperLimits, const Eigen::MatrixBase< ReturnType > &qout)
Generate a configuration vector uniformly sampled among provided limits.
Definition: joint-configuration.hpp:315
rnea.hpp
b
Vec3f b
BOOST_CHECK_IS_APPROX
#define BOOST_CHECK_IS_APPROX(double_field, multires_field, Scalar)
Definition: multiprecision-mpfr.cpp:89
aba.hpp
forward-dynamics-derivatives.tau
tau
Definition: forward-dynamics-derivatives.py:25
center-of-mass.hpp
joint-configuration.hpp
pinocchio::ModelTpl::TangentVectorType
VectorXs TangentVectorType
Dense vectorized version of a joint tangent vector (e.g. velocity, acceleration, etc)....
Definition: multibody/model.hpp:95
x
x
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(test_basic)
Definition: multiprecision-mpfr.cpp:25
pinocchio::q
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
Definition: joint-configuration.hpp:1083
a
Vec3f a
pinocchio::v
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
Definition: joint-configuration.hpp:1084
multiprecision.hpp
fill
fill
centroidal.hpp
pinocchio::rnea
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & rnea(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType1 > &v, const Eigen::MatrixBase< TangentVectorType2 > &a)
The Recursive Newton-Euler algorithm. It computes the inverse dynamics, aka the joint torques accordi...
pinocchio.explog.log
def log(x)
Definition: bindings/python/pinocchio/explog.py:29
pinocchio::JointIndex
Index JointIndex
Definition: multibody/fwd.hpp:26
pinocchio::ModelTpl::ConfigVectorType
VectorXs ConfigVectorType
Dense vectorized version of a joint configuration vector.
Definition: multibody/model.hpp:87
dim
int dim
sample-models.hpp
append-urdf-model-with-another-model.joint_id
joint_id
Definition: append-urdf-model-with-another-model.py:34
jacobian.hpp
pinocchio::ModelTpl
Definition: context/generic.hpp:20
crba.hpp
multiprecision-mpfr.hpp
pinocchio::model
JointCollectionTpl & model
Definition: joint-configuration.hpp:1082
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:27


pinocchio
Author(s):
autogenerated on Sun Dec 22 2024 03:41:12