unittest/multiprecision.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2020 INRIA
3 //
4 
13 
15 
16 #include <boost/multiprecision/cpp_dec_float.hpp>
17 #include <boost/math/special_functions/gamma.hpp>
18 
19 #include <iostream>
20 
21 #include <boost/test/unit_test.hpp>
22 #include <boost/utility/binary.hpp>
23 
24 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
25 
27 {
28  using namespace boost::multiprecision;
29 
30  // Operations at fixed precision and full numeric_limits support:
31  cpp_dec_float_100 b = 2;
32  std::cout << std::numeric_limits<cpp_dec_float_100>::digits << std::endl;
33  std::cout << std::numeric_limits<cpp_dec_float_100>::digits10 << std::endl;
34  // We can use any C++ std lib function, lets print all the digits as well:
35  std::cout << std::setprecision(std::numeric_limits<cpp_dec_float_100>::max_digits10) << log(b)
36  << std::endl; // print log(2)
37  // We can also use any function from Boost.Math:
38  std::cout << boost::math::tgamma(b) << std::endl;
39  // These even work when the argument is an expression template:
40  std::cout << boost::math::tgamma(b * b) << std::endl;
41  // And since we have an extended exponent range we can generate some really large
42  // numbers here (4.0238726007709377354370243e+2564):
43  std::cout << boost::math::tgamma(cpp_dec_float_100(1000)) << std::endl;
44 }
45 
47 {
48  typedef boost::multiprecision::cpp_dec_float_100 float_100;
49 
50  // Test Scalar cast
51  double initial_value = boost::math::constants::pi<double>();
52  float_100 value_100(initial_value);
53  double value_cast = value_100.convert_to<double>();
54  BOOST_CHECK(initial_value == value_cast);
55 
56  typedef Eigen::Matrix<float_100, Eigen::Dynamic, 1> VectorFloat100;
57  static const Eigen::DenseIndex dim = 100;
58  Eigen::VectorXd initial_vec = Eigen::VectorXd::Random(dim);
59  VectorFloat100 vec_float_100 = initial_vec.cast<float_100>();
60  Eigen::VectorXd vec = vec_float_100.cast<double>();
61 
62  BOOST_CHECK(vec == initial_vec);
63 }
64 
65 #define BOOST_CHECK_IS_APPROX(double_field, multires_field, Scalar) \
66  BOOST_CHECK(double_field.isApprox(multires_field.cast<Scalar>()))
67 
68 BOOST_AUTO_TEST_CASE(test_mutliprecision)
69 {
70  using namespace pinocchio;
71 
72  Model model;
74  Data data(model);
75 
76  model.lowerPositionLimit.head<3>().fill(-1.);
77  model.upperPositionLimit.head<3>().fill(1.);
78 
79  typedef boost::multiprecision::cpp_dec_float_100 float_100;
80  typedef ModelTpl<float_100> ModelMulti;
81  typedef DataTpl<float_100> DataMulti;
82 
83  ModelMulti model_multi = model.cast<float_100>();
84  DataMulti data_multi(model_multi);
85 
86  ModelMulti::ConfigVectorType q_multi = randomConfiguration(model_multi);
87  ModelMulti::TangentVectorType v_multi = ModelMulti::TangentVectorType::Random(model_multi.nv);
88  ModelMulti::TangentVectorType a_multi = ModelMulti::TangentVectorType::Random(model_multi.nv);
89  ModelMulti::TangentVectorType tau_multi = ModelMulti::TangentVectorType::Random(model_multi.nv);
90 
91  // Model::ConfigVectorType q = randomConfiguration(model);
92  // Model::TangentVectorType v = Model::TangentVectorType::Random(model.nv);
93  // Model::TangentVectorType a = Model::TangentVectorType::Random(model.nv);
94  // Model::TangentVectorType tau = Model::TangentVectorType::Random(model.nv);
95 
96  Model::ConfigVectorType q = q_multi.cast<double>();
97  Model::TangentVectorType v = v_multi.cast<double>();
98  Model::TangentVectorType a = a_multi.cast<double>();
99  Model::TangentVectorType tau = tau_multi.cast<double>();
100 
101  forwardKinematics(model_multi, data_multi, q_multi, v_multi, a_multi);
103 
104  for (JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id)
105  {
106  BOOST_CHECK_IS_APPROX(data.oMi[joint_id], data_multi.oMi[joint_id], double);
107  BOOST_CHECK_IS_APPROX(data.v[joint_id], data_multi.v[joint_id], double);
108  BOOST_CHECK_IS_APPROX(data.a[joint_id], data_multi.a[joint_id], double);
109  }
110 
111  // Jacobians
112  computeJointJacobians(model_multi, data_multi, q_multi);
114 
115  BOOST_CHECK_IS_APPROX(data.J, data_multi.J, double);
116 
117  // Inverse Dynamics
118  rnea(model_multi, data_multi, q_multi, v_multi, a_multi);
119  rnea(model, data, q, v, a);
120 
121  BOOST_CHECK_IS_APPROX(data.tau, data_multi.tau, double);
122 
123  // Forward Dynamics
124  aba(model_multi, data_multi, q_multi, v_multi, tau_multi, Convention::WORLD);
126 
127  BOOST_CHECK_IS_APPROX(data.ddq, data_multi.ddq, double);
128 
129  // Mass matrix
130  crba(model_multi, data_multi, q_multi, Convention::WORLD);
131  data_multi.M.triangularView<Eigen::StrictlyLower>() =
132  data_multi.M.transpose().triangularView<Eigen::StrictlyLower>();
133 
135  data.M.triangularView<Eigen::StrictlyLower>() =
136  data.M.transpose().triangularView<Eigen::StrictlyLower>();
137 
138  BOOST_CHECK_IS_APPROX(data.M, data_multi.M, double);
139 }
140 
141 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.
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::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
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
pinocchio::q
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
Definition: joint-configuration.hpp:1083
a
Vec3f a
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(test_basic)
Definition: unittest/multiprecision.cpp:26
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...
BOOST_CHECK_IS_APPROX
#define BOOST_CHECK_IS_APPROX(double_field, multires_field, Scalar)
Definition: unittest/multiprecision.cpp:65
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
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