examples/multiprecision.cpp
Go to the documentation of this file.
1 #include "pinocchio/math/multiprecision.hpp"
2 
3 #include "pinocchio/parsers/urdf.hpp"
4 
5 #include "pinocchio/algorithm/joint-configuration.hpp"
6 #include "pinocchio/algorithm/rnea.hpp"
7 
8 #include <boost/multiprecision/cpp_dec_float.hpp>
9 
10 #include <iostream>
11 
12 // PINOCCHIO_MODEL_DIR is defined by the CMake but you can define your own directory here.
13 #ifndef PINOCCHIO_MODEL_DIR
14  #define PINOCCHIO_MODEL_DIR "path_to_the_model_dir"
15 #endif
16 
17 int main(int argc, char ** argv)
18 {
19  using namespace pinocchio;
20 
21  // You should change here to set up your own URDF file or just pass it as an argument of this example.
22  const std::string urdf_filename = (argc<=1) ? PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/ur_description/urdf/ur5_robot.urdf") : argv[1];
23 
24  // Load the URDF model
25  Model model;
26  pinocchio::urdf::buildModel(urdf_filename,model);
27 
28  // Build a data related to model
29  Data data(model);
30 
31  // Define Model and Data for multiprecision types
32  typedef boost::multiprecision::cpp_dec_float_100 float_100;
33  typedef ModelTpl<float_100> ModelMulti;
34  typedef DataTpl<float_100> DataMulti;
35 
36  ModelMulti model_multi = model.cast<float_100>();
37  DataMulti data_multi(model_multi);
38 
39  // Sample a random joint configuration as well as random joint velocity and acceleration
40  ModelMulti::ConfigVectorType q_multi = randomConfiguration(model_multi);
41  ModelMulti::TangentVectorType v_multi = ModelMulti::TangentVectorType::Random(model.nv);
42  ModelMulti::TangentVectorType a_multi = ModelMulti::TangentVectorType::Random(model.nv);
43 
44  Model::ConfigVectorType q = q_multi.cast<double>();
45  Model::TangentVectorType v = v_multi.cast<double>();
46  Model::TangentVectorType a = a_multi.cast<double>();
47 
48  // Computes the inverse dynamics (aka RNEA)
49  rnea(model, data, q, v, a);
50  rnea(model_multi , data_multi , q_multi , v_multi , a_multi);
51 
52  // Get access to the joint torque with standard or multiprecision arithmetic and print sufficient decimals for both precisions
53  std::cout << "Joint torque standard arithmetic:\n" << std::setprecision(std::numeric_limits<float_100>::max_digits10) << data.tau << std::endl;
54  std::cout << "Joint torque multiprecision arithmetic:\n" << std::setprecision(std::numeric_limits<float_100>::max_digits10) << data_multi.tau << std::endl;
55 }
56 
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
TangentVectorType tau
Vector of joint torques (dim model.nv).
void rnea(const int num_threads, ModelPoolTpl< Scalar, Options, JointCollectionTpl > &pool, const Eigen::MatrixBase< ConfigVectorPool > &q, const Eigen::MatrixBase< TangentVectorPool1 > &v, const Eigen::MatrixBase< TangentVectorPool2 > &a, const Eigen::MatrixBase< TangentVectorPool3 > &tau)
The Recursive Newton-Euler algorithm. It computes the inverse dynamics, aka the joint torques accordi...
ModelTpl< NewScalar, Options, JointCollectionTpl > cast() const
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.
VectorXs TangentVectorType
Dense vectorized version of a joint tangent vector (e.g. velocity, acceleration, etc). It also handles the notion of co-tangent vector (e.g. torque, etc).
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
#define PINOCCHIO_MODEL_DIR
ModelTpl< Scalar, Options, JointCollectionTpl > & buildModel(const std::string &filename, const typename ModelTpl< Scalar, Options, JointCollectionTpl >::JointModel &rootJoint, ModelTpl< Scalar, Options, JointCollectionTpl > &model, const bool verbose=false)
Build the model from a URDF file with a particular joint as root of the model tree inside the model g...
Vec3f a
Main pinocchio namespace.
Definition: timings.cpp:28
int nv
Dimension of the velocity vector space.
int main(int argc, char **argv)
VectorXs ConfigVectorType
Dense vectorized version of a joint configuration vector.
JointCollectionTpl & model


pinocchio
Author(s):
autogenerated on Fri Jun 23 2023 02:38:32