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  Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);
41  Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);
42  Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv);
43 
44  std::cout << std::setprecision(10);
45 
46  Eigen::VectorXd expected(model.nv);
47  expected << 0, 0, 1281.84, 0, -40.5132, 0, 4.4492, -1.5386, 0, -1.5386, -1.5386, 0, -4.4492,
48  -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,
49  0.392, 0, 0.392, 0;
50  q = Eigen::VectorXd::Zero(model.nq);
51  v = Eigen::VectorXd::Zero(model.nv);
52  a = Eigen::VectorXd::Zero(model.nv);
53  rnea(model, data, q, v, a);
54  BOOST_CHECK(expected.isApprox(data.tau, 1e-12));
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  Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);
67  Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);
68  Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv);
69 
70  std::cout << std::setprecision(10);
71 
72  Eigen::VectorXd expected(model.nv);
73  expected << -48.10636, -73.218816, 1384.901025, -7.2292939, -186.342371, -20.6685852, -0.78887946,
74  -1.869651075, 1.8889752, -2.036768175, -2.105948175, -1.023232, -18.3738505, -4.133954895,
75  9.0456456, -4.276615035, -4.143427035, -2.534896, -180.338765, 15.71570676, -29.8639164,
76  -59.917862, -2.3307916, -2.7648728, -45.76782776, 3.4151272, -18.4320456, -4.5768072,
77  -76.60945104, -0.5897908, -2.640844, -63.93417064, 5.359156, -25.8309196, -6.976116;
78  q = Eigen::VectorXd::Zero(model.nq);
79  for (int i = 6; i < model.nv; ++i)
80  v[i] = i / 10.;
81  a = Eigen::VectorXd::Zero(model.nv);
82  rnea(model, data, q, v, a);
83  // std::cout << (expected-data.tau).norm() << std::endl;
84  BOOST_CHECK(expected.isApprox(data.tau, 1e-7));
85 }
86 
87 BOOST_AUTO_TEST_CASE(test_0VA)
88 {
89  std::string filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid.urdf");
90 
91  pinocchio::Model model;
92  pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(), model);
93  model.gravity.linear(Eigen::Vector3d(0, 0, -9.8));
94  pinocchio::Data data(model);
95 
96  Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);
97  Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);
98  Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv);
99 
100  std::cout << std::setprecision(10);
101  Eigen::VectorXd expected(model.nv);
102  expected << -15.3331636, -0.67891816, 1273.80521, 102.9435113, 110.3509945, 81.52296995,
103  13.31476408, 14.26606068, 3.682505252, 9.048274318, 4.663303518, 2.05308568, 12.54347834,
104  25.92680911, 6.327105656, 16.71123385, 8.96650473, 3.54200704, 70.15812475, 77.02410963,
105  73.81994844, 41.73185754, 28.75786872, 28.94251127, 31.65847724, 20.40431127, 18.18579154,
106  6.838471928, 50.44193173, 34.07362801, 34.53507156, 38.33983417, 24.61507156, 22.2842788,
107  8.23435884;
108  q = Eigen::VectorXd::Zero(model.nq);
109  for (int i = 6; i < model.nv; ++i)
110  v[i] = i / 100.;
111  for (int i = 6; i < model.nv; ++i)
112  a[i] = i / 10.;
113  rnea(model, data, q, v, a);
114  BOOST_CHECK(expected.isApprox(data.tau, 1e-6));
115 }
116 
117 BOOST_AUTO_TEST_CASE(test_Q00)
118 {
119  std::string filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid.urdf");
120 
121  pinocchio::Model model;
122  pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(), model);
123  model.gravity.linear(Eigen::Vector3d(0, 0, -9.8));
124  pinocchio::Data data(model);
125 
126  Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);
127  Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);
128  Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv);
129 
130  std::cout << std::setprecision(10);
131 
132  Eigen::VectorXd expected(model.nv);
133  expected << 5.367234435e-15, -2.587860481e-14, 1281.84, -133.3062501, 198.975587,
134  -7.120345979e-16, 15.06850407, 58.39287139, -22.14971864, 17.14327289, 1.291543104,
135  0.7402017048, -4.386231387, 22.73949408, -19.01681794, 0.8839600793, -0.3197599308,
136  -0.466827706, 65.47086697, 32.71449398, -4.250622066, -0.7937685568, -0.15349648, -1.070480752,
137  -3.066302263, -0.3557903212, -0.2183951073, 0.182684221, -0.6648425468, -2.902772493,
138  0.1250340934, 0.4402877138, -0.3158584741, -0.0865162794, 0.3918733239;
139  for (int i = 7; i < model.nq; ++i)
140  q[i] = (i - 1) / 10.;
141  v = Eigen::VectorXd::Zero(model.nv);
142  a = Eigen::VectorXd::Zero(model.nv);
143  rnea(model, data, q, v, a);
144  // std::cout << expected << "\n ---------------- \n"
145  // << data.tau << std::endl;
146 
147  BOOST_CHECK(expected.isApprox(data.tau, 1e-6));
148 }
149 
150 BOOST_AUTO_TEST_CASE(test_QVA)
151 {
152  std::string filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid.urdf");
153 
154  pinocchio::Model model;
155  pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(), model);
156  model.gravity.linear(Eigen::Vector3d(0, 0, -9.8));
157  pinocchio::Data data(model);
158 
159  Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);
160  Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);
161  Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv);
162 
163  std::cout << std::setprecision(10);
164  Eigen::VectorXd expected(model.nv);
165  expected << -1.911650826, 1.250211406, 1284.82058, -139.0188156, 201.744449, 1.554847332,
166  15.1910084, 59.27339983, -21.70753738, 17.84339797, 1.754639468, 0.670280632, -2.968778954,
167  23.0776205, -17.56870284, 1.765761886, 0.2889992363, -0.392159764, 68.83598707, 34.59002827,
168  -4.604435817, -0.3832225891, 1.085231916, -0.348635267, -2.831371037, -1.047616506,
169  -0.228384161, 0.5656880079, 1.302869049, 0.8481280783, 0.7042182131, 1.554751317, -0.3908790552,
170  -0.1294643218, 1.421077555;
171  for (int i = 7; i < model.nq; ++i)
172  q[i] = (i - 1) / 10.;
173  for (int i = 6; i < model.nv; ++i)
174  v[i] = i / 100.;
175  for (int i = 6; i < model.nv; ++i)
176  a[i] = i / 100.;
177  rnea(model, data, q, v, a);
178  BOOST_CHECK(expected.isApprox(data.tau, 1e-7));
179 }
180 
181 BOOST_AUTO_TEST_SUITE_END()
pinocchio::DataTpl
Definition: context/generic.hpp:25
inverse-kinematics.i
int i
Definition: inverse-kinematics.py:17
kinematics.hpp
model.hpp
setup.data
data
Definition: cmake/cython/setup.in.py:48
pinocchio::JointModelFreeFlyerTpl
Definition: multibody/joint/fwd.hpp:110
rnea.hpp
anymal-simulation.model
model
Definition: anymal-simulation.py:8
filename
filename
urdf.hpp
cartpole.v
v
Definition: cartpole.py:153
data.hpp
pinocchio::urdf::buildModel
ModelTpl< Scalar, Options, JointCollectionTpl > & buildModel(const std::string &filename, const typename ModelTpl< Scalar, Options, JointCollectionTpl >::JointModel &rootJoint, const std::string &rootJointName, 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...
q
q
a
Vec3f a
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::ModelTpl
Definition: context/generic.hpp:20
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(test_000)
Definition: value.cpp:31
PINOCCHIO_MODEL_DIR
#define PINOCCHIO_MODEL_DIR
Definition: build-reduced-model.cpp:11

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