value.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2015-2018 CNRS
3 //
4 
5 /*
6  * Compare the value obtained with the RNEA with the values obtained from
7  * RBDL. The test is not complete. It only validates the RNEA for the revolute
8  * joints. The free-flyer is not tested. It should be extended to account for
9  * the free flyer and for the other algorithms.
10  *
11  * Additionnal notes: the RNEA is an algorithm that can be used to validate
12  * many others (in particular, the mass matrix (CRBA) can be numerically
13  * validated from the RNEA, then the center-of-mass jacobian can be validated
14  * from the mass matrix, etc.
15  *
16  */
17 
18 #include <iostream>
19 #include <iomanip>
20 
21 #include "pinocchio/multibody/model.hpp"
22 #include "pinocchio/multibody/data.hpp"
23 #include "pinocchio/parsers/urdf.hpp"
24 #include "pinocchio/algorithm/rnea.hpp"
25 #include "pinocchio/algorithm/kinematics.hpp"
26 
27 #include <boost/test/unit_test.hpp>
28 
29 BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
30 
31 BOOST_AUTO_TEST_CASE ( test_000 )
32 {
33  std::string filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid.urdf");
34 
35  pinocchio::Model model;
36  pinocchio::urdf::buildModel(filename,pinocchio::JointModelFreeFlyer(),model);
37  model.gravity.linear( Eigen::Vector3d(0,0,-9.8));
38  pinocchio::Data data(model);
39 
40 
41  Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);
42  Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);
43  Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv);
44 
45  std::cout << std::setprecision(10);
46 
47  Eigen::VectorXd expected(model.nv);
48  expected << 0, 0, 1281.84, 0, -40.5132, 0, 4.4492, -1.5386, 0, -1.5386, -1.5386, 0, -4.4492, -1.5386, 0, -1.5386, -1.5386, 0, -37.436, 0, 0, -2.548, 0, 0, 0.392, 0, 0.392, 0, -2.548, 0, 0, 0.392, 0, 0.392, 0 ;
49  q = Eigen::VectorXd::Zero(model.nq);
50  v = Eigen::VectorXd::Zero(model.nv);
51  a = Eigen::VectorXd::Zero(model.nv);
52  rnea(model,data,q,v,a);
53  BOOST_CHECK (expected.isApprox(data.tau,1e-12));
54 }
55 
56 
57 BOOST_AUTO_TEST_CASE( test_0V0 )
58 {
59  std::string filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid.urdf");
60 
61  pinocchio::Model model;
62  pinocchio::urdf::buildModel(filename,pinocchio::JointModelFreeFlyer(),model);
63  model.gravity.linear( Eigen::Vector3d(0,0,-9.8));
64  pinocchio::Data data(model);
65 
66 
67  Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);
68  Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);
69  Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv);
70 
71  std::cout << std::setprecision(10);
72 
73  Eigen::VectorXd expected(model.nv);
74  expected << -48.10636, -73.218816, 1384.901025, -7.2292939, -186.342371, -20.6685852, -0.78887946, -1.869651075, 1.8889752, -2.036768175, -2.105948175, -1.023232, -18.3738505, -4.133954895, 9.0456456, -4.276615035, -4.143427035, -2.534896, -180.338765, 15.71570676, -29.8639164, -59.917862, -2.3307916, -2.7648728, -45.76782776, 3.4151272, -18.4320456, -4.5768072, -76.60945104, -0.5897908, -2.640844, -63.93417064, 5.359156, -25.8309196, -6.976116;
75  q = Eigen::VectorXd::Zero(model.nq);
76  for(int i=6;i<model.nv;++i) v[i] = i/10.;
77  a = Eigen::VectorXd::Zero(model.nv);
78  rnea(model,data,q,v,a);
79  //std::cout << (expected-data.tau).norm() << std::endl;
80  BOOST_CHECK (expected.isApprox(data.tau,1e-7));
81 }
82 
83 BOOST_AUTO_TEST_CASE( test_0VA )
84 {
85  std::string filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid.urdf");
86 
87  pinocchio::Model model;
88  pinocchio::urdf::buildModel(filename,pinocchio::JointModelFreeFlyer(),model);
89  model.gravity.linear( Eigen::Vector3d(0,0,-9.8));
90  pinocchio::Data data(model);
91 
92 
93  Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);
94  Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);
95  Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv);
96 
97  std::cout << std::setprecision(10);
98  Eigen::VectorXd expected(model.nv);
99  expected << -15.3331636, -0.67891816, 1273.80521, 102.9435113, 110.3509945, 81.52296995, 13.31476408, 14.26606068, 3.682505252, 9.048274318, 4.663303518, 2.05308568, 12.54347834, 25.92680911, 6.327105656, 16.71123385, 8.96650473, 3.54200704, 70.15812475, 77.02410963, 73.81994844, 41.73185754, 28.75786872, 28.94251127, 31.65847724, 20.40431127, 18.18579154, 6.838471928, 50.44193173, 34.07362801, 34.53507156, 38.33983417, 24.61507156, 22.2842788, 8.23435884;
100  q = Eigen::VectorXd::Zero(model.nq);
101  for(int i=6;i<model.nv;++i) v[i] = i/100.;
102  for(int i=6;i<model.nv;++i) a[i] = i/10.;
103  rnea(model,data,q,v,a);
104  BOOST_CHECK (expected.isApprox(data.tau,1e-6));
105 }
106 
107 BOOST_AUTO_TEST_CASE( test_Q00 )
108 {
109  std::string filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid.urdf");
110 
111  pinocchio::Model model;
112  pinocchio::urdf::buildModel(filename,pinocchio::JointModelFreeFlyer(),model);
113  model.gravity.linear( Eigen::Vector3d(0,0,-9.8));
114  pinocchio::Data data(model);
115 
116 
117  Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);
118  Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);
119  Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv);
120 
121  std::cout << std::setprecision(10);
122 
123  Eigen::VectorXd expected(model.nv);
124  expected << 5.367234435e-15, -2.587860481e-14, 1281.84, -133.3062501, 198.975587, -7.120345979e-16, 15.06850407, 58.39287139, -22.14971864, 17.14327289, 1.291543104, 0.7402017048, -4.386231387, 22.73949408, -19.01681794, 0.8839600793, -0.3197599308, -0.466827706, 65.47086697, 32.71449398, -4.250622066, -0.7937685568, -0.15349648, -1.070480752, -3.066302263, -0.3557903212, -0.2183951073, 0.182684221, -0.6648425468, -2.902772493, 0.1250340934, 0.4402877138, -0.3158584741, -0.0865162794, 0.3918733239;
125  for(int i=7;i<model.nq;++i) q[i] = (i-1)/10.;
126  v = Eigen::VectorXd::Zero(model.nv);
127  a = Eigen::VectorXd::Zero(model.nv);
128  rnea(model,data,q,v,a);
129 // std::cout << expected << "\n ---------------- \n"
130 // << data.tau << std::endl;
131 
132  BOOST_CHECK (expected.isApprox(data.tau,1e-6));
133 
134 }
135 
136 BOOST_AUTO_TEST_CASE( test_QVA )
137 {
138  std::string filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid.urdf");
139 
140  pinocchio::Model model;
141  pinocchio::urdf::buildModel(filename,pinocchio::JointModelFreeFlyer(),model);
142  model.gravity.linear( Eigen::Vector3d(0,0,-9.8));
143  pinocchio::Data data(model);
144 
145 
146  Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);
147  Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);
148  Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv);
149 
150  std::cout << std::setprecision(10);
151  Eigen::VectorXd expected(model.nv);
152  expected << -1.911650826, 1.250211406, 1284.82058, -139.0188156, 201.744449, 1.554847332, 15.1910084, 59.27339983, -21.70753738, 17.84339797, 1.754639468, 0.670280632, -2.968778954, 23.0776205, -17.56870284, 1.765761886, 0.2889992363, -0.392159764, 68.83598707, 34.59002827, -4.604435817, -0.3832225891, 1.085231916, -0.348635267, -2.831371037, -1.047616506, -0.228384161, 0.5656880079, 1.302869049, 0.8481280783, 0.7042182131, 1.554751317, -0.3908790552, -0.1294643218, 1.421077555;
153  for(int i=7;i<model.nq;++i) q[i] = (i-1)/10.;
154  for(int i=6;i<model.nv;++i) v[i] = i/100.;
155  for(int i=6;i<model.nv;++i) a[i] = i/100.;
156  rnea(model,data,q,v,a);
157  BOOST_CHECK (expected.isApprox(data.tau,1e-7));
158 }
159 
160 BOOST_AUTO_TEST_SUITE_END ()
pinocchio::DataTpl::tau
TangentVectorType tau
Vector of joint torques (dim model.nv).
Definition: src/multibody/data.hpp:130
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(test_000)
Definition: value.cpp:31
q
q
pinocchio::rnea
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...
Definition: parallel/rnea.hpp:32
pinocchio::MotionBase::linear
ConstLinearType linear() const
Definition: motion-base.hpp:22
PINOCCHIO_MODEL_DIR
#define PINOCCHIO_MODEL_DIR
Definition: build-reduced-model.cpp:11
pinocchio::ModelTpl::gravity
Motion gravity
Spatial gravity of the model.
Definition: src/multibody/model.hpp:158
pinocchio::urdf::buildModel
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...
a
Vec3f a
pinocchio::ModelTpl::nv
int nv
Dimension of the velocity vector space.
Definition: src/multibody/model.hpp:79
filename
filename
pinocchio::python::VectorXd
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > VectorXd
Definition: conversions.cpp:14
pinocchio::ModelTpl::nq
int nq
Dimension of the configuration vector representation.
Definition: src/multibody/model.hpp:76

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