com.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2015-2019 CNRS INRIA
3 //
4 
15 
16 #include <iostream>
17 
18 #include <boost/test/unit_test.hpp>
19 #include <boost/utility/binary.hpp>
20 
21 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
22 
24 {
25  using namespace Eigen;
26  using namespace pinocchio;
27 
31 
32  VectorXd q = VectorXd::Ones(model.nq);
33  q.middleRows<4>(3).normalize();
34  VectorXd v = VectorXd::Ones(model.nv);
35  VectorXd a = VectorXd::Ones(model.nv);
36 
38 
39  /* Test COM against CRBA*/
40  Vector3d com = centerOfMass(model, data, q);
41  BOOST_CHECK(data.com[0].isApprox(getComFromCrba(model, data), 1e-12));
42 
43  /* Test COM against Jcom (both use different way to compute the COM). */
46  BOOST_CHECK(com.isApprox(data.com[0], 1e-12));
47 
48  /* Test COM against Jcom (both use different way to compute the COM). */
49  centerOfMass(model, data, q, v, a);
50  BOOST_CHECK(com.isApprox(data.com[0], 1e-12));
51 
52  /* Test vCoM against nle algorithm without gravity field */
53  a.setZero();
54  model.gravity.setZero();
55  centerOfMass(model, data, q, v, a);
57 
58  pinocchio::SE3::Vector3 acom_from_nle(data.nle.head<3>() / data.mass[0]);
59  BOOST_CHECK((data.liMi[1].rotation() * acom_from_nle).isApprox(data.acom[0], 1e-12));
60 
61  /* Test Jcom against CRBA */
62  Eigen::MatrixXd Jcom = jacobianCenterOfMass(model, data, q);
63  BOOST_CHECK(data.Jcom.isApprox(getJacobianComFromCrba(model, data), 1e-12));
64 
65  /* Test CoM velocity againt jacobianCenterOfMass */
66  BOOST_CHECK((Jcom * v).isApprox(data.vcom[0], 1e-12));
67 
69  /* Test CoM velocity againt jacobianCenterOfMass */
70  BOOST_CHECK((Jcom * v).isApprox(data.vcom[0], 1e-12));
71 
72  // std::cout << "com = [ " << data.com[0].transpose() << " ];" << std::endl;
73  // std::cout << "mass = [ " << data.mass[0] << " ];" << std::endl;
74  // std::cout << "Jcom = [ " << data.Jcom << " ];" << std::endl;
75  // std::cout << "M3 = [ " << data.M.topRows<3>() << " ];" << std::endl;
76 }
77 
79 {
80  using namespace Eigen;
81  using namespace pinocchio;
82 
85 
86  double mass = computeTotalMass(model);
87 
88  BOOST_CHECK(mass == mass); // checking it is not NaN
89 
90  double mass_check = 0.0;
91  for (size_t i = 1; i < (size_t)(model.njoints); ++i)
92  mass_check += model.inertias[i].mass();
93 
94  BOOST_CHECK_CLOSE(mass, mass_check, 1e-12);
95 
96  pinocchio::Data data1(model);
97 
98  double mass_data = computeTotalMass(model, data1);
99 
100  BOOST_CHECK(mass_data == mass_data); // checking it is not NaN
101  BOOST_CHECK_CLOSE(mass, mass_data, 1e-12);
102  BOOST_CHECK_CLOSE(data1.mass[0], mass_data, 1e-12);
103 
104  pinocchio::Data data2(model);
105  VectorXd q = VectorXd::Ones(model.nq);
106  q.middleRows<4>(3).normalize();
107  centerOfMass(model, data2, q);
108 
109  BOOST_CHECK_CLOSE(data2.mass[0], mass, 1e-12);
110 }
111 
112 BOOST_AUTO_TEST_CASE(test_subtree_masses)
113 {
114  using namespace Eigen;
115  using namespace pinocchio;
116 
119 
120  pinocchio::Data data1(model);
121 
122  computeSubtreeMasses(model, data1);
123 
124  pinocchio::Data data2(model);
125  VectorXd q = VectorXd::Ones(model.nq);
126  q.middleRows<4>(3).normalize();
127  centerOfMass(model, data2, q);
128 
129  for (size_t i = 0; i < (size_t)(model.njoints); ++i)
130  {
131  BOOST_CHECK_CLOSE(data1.mass[i], data2.mass[i], 1e-12);
132  }
133 }
134 
135 // BOOST_AUTO_TEST_CASE ( test_timings )
136 //{
137 // using namespace Eigen;
138 // using namespace pinocchio;
139 //
140 // pinocchio::Model model;
141 // pinocchio::buildModels::humanoidRandom(model);
142 // pinocchio::Data data(model);
143 //
144 // long flag = BOOST_BINARY(1111);
145 // PinocchioTicToc timer(PinocchioTicToc::US);
146 // #ifdef NDEBUG
147 // #ifdef _INTENSE_TESTING_
148 // const size_t NBT = 1000*1000;
149 // #else
150 // const size_t NBT = 10;
151 // #endif
152 // #else
153 // const size_t NBT = 1;
154 // std::cout << "(the time score in debug mode is not relevant) " ;
155 // #endif
156 //
157 // bool verbose = flag & (flag-1) ; // True is two or more binaries of the flag are 1.
158 // if(verbose) std::cout <<"--" << std::endl;
159 // Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);
160 //
161 // if( flag >> 0 & 1 )
162 // {
163 // timer.tic();
164 // SMOOTH(NBT)
165 // {
166 // centerOfMass(model,data,q);
167 // }
168 // if(verbose) std::cout << "COM =\t";
169 // timer.toc(std::cout,NBT);
170 // }
171 //
172 // if( flag >> 1 & 1 )
173 // {
174 // timer.tic();
175 // SMOOTH(NBT)
176 // {
177 // centerOfMass(model,data,q,false);
178 // }
179 // if(verbose) std::cout << "Without sub-tree =\t";
180 // timer.toc(std::cout,NBT);
181 // }
182 //
183 // if( flag >> 2 & 1 )
184 // {
185 // timer.tic();
186 // SMOOTH(NBT)
187 // {
188 // jacobianCenterOfMass(model,data,q);
189 // }
190 // if(verbose) std::cout << "Jcom =\t";
191 // timer.toc(std::cout,NBT);
192 // }
193 // }
194 
195 BOOST_AUTO_TEST_CASE(test_subtree_com_jacobian)
196 {
197  using namespace Eigen;
198  using namespace pinocchio;
199 
200  Model model;
202  Data data(model);
203 
204  model.upperPositionLimit.head<3>().fill(1000);
205  model.lowerPositionLimit.head<3>() = -model.upperPositionLimit.head<3>();
207  VectorXd v = VectorXd::Random(model.nv);
208 
209  Data data_ref(model);
210  jacobianCenterOfMass(model, data_ref, q, true);
211 
212  centerOfMass(model, data, q, v);
213  Data::Matrix3x Jcom1(3, model.nv);
214  Jcom1.setZero();
216 
217  BOOST_CHECK(Jcom1.isApprox(data_ref.Jcom));
218 
219  centerOfMass(model, data_ref, q, v, true);
220  computeJointJacobians(model, data_ref, q);
221  Data::Matrix3x Jcom_extracted(3, model.nv), Jcom_fd(3, model.nv);
222  Data data_extracted(model), data_fd(model);
223  const double eps = 1e-8;
224  jacobianCenterOfMass(model, data_extracted, q);
225 
226  // Get subtree jacobian and check that it is consistent with the com velocity
227  Data::Matrix3x Jcom(3, model.nv);
228  Jcom.setZero();
229  for (JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; joint_id++)
230  {
231  SE3::Vector3 subtreeComVelocityInWorld_ref =
232  data_ref.oMi[joint_id].rotation() * data_ref.vcom[joint_id];
233  Jcom.setZero();
234  data.J.setZero();
236 
237  BOOST_CHECK(
238  data.J.middleCols(model.joints[joint_id].idx_v(), data.nvSubtree[joint_id])
239  .isApprox(data_ref.J.middleCols(model.joints[joint_id].idx_v(), data.nvSubtree[joint_id])));
240  SE3::Vector3 subtreeComVelocityInWorld = Jcom * v;
241 
242  Jcom_extracted.setZero();
243  getJacobianSubtreeCenterOfMass(model, data_extracted, joint_id, Jcom_extracted);
244 
245  // Check with finite differences
246  Eigen::VectorXd v_plus(model.nv);
247  v_plus.setZero();
248  centerOfMass(model, data_fd, q);
249  const SE3::Vector3 com = data_fd.oMi[joint_id].act(data_fd.com[joint_id]);
250  Jcom_fd.setZero();
251  for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
252  {
253  v_plus[k] = eps;
254  Eigen::VectorXd q_plus = integrate(model, q, v_plus);
255  centerOfMass(model, data_fd, q_plus);
256  const SE3::Vector3 com_plus = data_fd.oMi[joint_id].act(data_fd.com[joint_id]);
257  Jcom_fd.col(k) = (com_plus - com) / eps;
258  v_plus[k] = 0.;
259  }
260 
261  // Eigen::VectorXd q_plus = integrate(model,q,v*eps);
262  // centerOfMass(model,data_fd,q_plus);
263  // const SE3::Vector3 com_plus = data_fd.oMi[joint_id].act(data_fd.com[joint_id]);
264  //
265  // const SE3::Vector3 vcom_subtree_fd = (com_plus - com)/eps;
266 
267  BOOST_CHECK(Jcom.isApprox(Jcom_fd, sqrt(eps)));
268  BOOST_CHECK(Jcom_extracted.isApprox(Jcom_fd, sqrt(eps)));
269  BOOST_CHECK(Jcom_extracted.isApprox(Jcom));
270 
271  BOOST_CHECK(std::fabs(data.mass[joint_id] - data_ref.mass[joint_id]) <= 1e-12);
272  BOOST_CHECK(data.com[joint_id].isApprox(data_ref.oMi[joint_id].act(data_ref.com[joint_id])));
273  BOOST_CHECK(subtreeComVelocityInWorld.isApprox(subtreeComVelocityInWorld_ref));
274  }
275 
276  // Other signatures
277  Data data_other_signature(model);
278  Data::Matrix3x Jcom2(3, model.nv);
279  Jcom2.setZero();
280  jacobianSubtreeCenterOfMass(model, data_other_signature, q, 0, Jcom2);
281 
282  BOOST_CHECK(Jcom2.isApprox(Jcom1));
283 
285  Data::Matrix3x Jcom3(3, model.nv);
286  Jcom3.setZero();
288 
289  BOOST_CHECK(Jcom3.isApprox(Jcom1));
290 }
291 
292 BOOST_AUTO_TEST_SUITE_END()
pinocchio::WORLD
@ WORLD
Definition: multibody/fwd.hpp:48
Eigen
pinocchio::SE3Tpl< context::Scalar, context::Options >::Vector3
traits< SE3Tpl >::Vector3 Vector3
Definition: spatial/se3-tpl.hpp:55
pinocchio::getComFromCrba
const DataTpl< Scalar, Options, JointCollectionTpl >::Vector3 & getComFromCrba(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
Extracts the center of mass position from the joint space inertia matrix (also called the mass matrix...
pinocchio::DataTpl
Definition: context/generic.hpp:25
pinocchio::DataTpl::mass
std::vector< Scalar > mass
Vector of subtree mass. In other words, mass[j] is the mass of the subtree supported by joint ....
Definition: multibody/data.hpp:444
compute-all-terms.hpp
inverse-kinematics.i
int i
Definition: inverse-kinematics.py:17
model.hpp
setup.data
data
Definition: cmake/cython/setup.in.py:48
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(test_com)
Definition: com.cpp:23
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::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
pinocchio::computeTotalMass
Scalar computeTotalMass(const ModelTpl< Scalar, Options, JointCollectionTpl > &model)
Compute the total mass of the model and return it.
rnea.hpp
timer.hpp
anymal-simulation.model
model
Definition: anymal-simulation.py:8
center-of-mass.hpp
joint-configuration.hpp
pinocchio::DataTpl::Matrix3x
Eigen::Matrix< Scalar, 3, Eigen::Dynamic, Options > Matrix3x
The 3d jacobian type (temporary)
Definition: multibody/data.hpp:94
pinocchio::integrate
void integrate(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v, const Eigen::MatrixBase< ReturnType > &qout)
Integrate a configuration vector for the specified model for a tangent vector during one unit time.
Definition: joint-configuration.hpp:70
pinocchio::jacobianCenterOfMass
const DataTpl< Scalar, Options, JointCollectionTpl >::Matrix3x & jacobianCenterOfMass(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const bool computeSubtreeComs=true)
Computes both the jacobian and the the center of mass position of a given model according to a partic...
pinocchio::computeSubtreeMasses
void computeSubtreeMasses(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
Compute the mass of each kinematic subtree and store it in data.mass. The element mass[0] corresponds...
cartpole.v
v
Definition: cartpole.py:153
data.hpp
pinocchio::DataTpl::Jcom
Matrix3x Jcom
Jacobian of center of mass.
Definition: multibody/data.hpp:450
pinocchio::getJacobianSubtreeCenterOfMass
void getJacobianSubtreeCenterOfMass(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex &rootSubtreeId, const Eigen::MatrixBase< Matrix3xLike > &res)
Retrieves the Jacobian of the center of mass of the given subtree according to the current value stor...
pinocchio::nonLinearEffects
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & nonLinearEffects(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v)
Computes the non-linear effects (Corriolis, centrifual and gravitationnal effects),...
q
q
anymal-simulation.mass
mass
Definition: anymal-simulation.py:62
pinocchio::getJacobianComFromCrba
const DataTpl< Scalar, Options, JointCollectionTpl >::Matrix3x & getJacobianComFromCrba(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
Extracts both the jacobian of the center of mass (CoM), the total mass of the system and the CoM posi...
a
Vec3f a
pinocchio::DataTpl::J
Matrix6x J
Jacobian of joint placements.
Definition: multibody/data.hpp:360
fill
fill
dcrba.eps
int eps
Definition: dcrba.py:458
pinocchio::JointIndex
Index JointIndex
Definition: multibody/fwd.hpp:26
pinocchio::jacobianSubtreeCenterOfMass
void jacobianSubtreeCenterOfMass(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const JointIndex &rootSubtreeId, const Eigen::MatrixBase< Matrix3xLike > &res)
Computes the Jacobian of the center of mass of the given subtree according to a particular joint conf...
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
robot-wrapper-viewer.com
com
Definition: robot-wrapper-viewer.py:45
pinocchio::normalize
void normalize(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &qout)
Normalize a configuration vector.
Definition: joint-configuration.hpp:887
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:27
isApprox
bool isApprox(const Box &s1, const Box &s2, const FCL_REAL tol)
pinocchio::centerOfMass
const DataTpl< Scalar, Options, JointCollectionTpl >::Vector3 & centerOfMass(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const bool computeSubtreeComs=true)
Computes the center of mass position of a given model according to a particular joint configuration....


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