Debian Codegraph @

unittests/integration/test_SoftDynamics.cpp

raw
1 /*
2 * Copyright (c) 2011-2022, The DART development contributors
3 * All rights reserved.
4 *
5 * The list of contributors can be found at:
6 * https://github.com/dartsim/dart/blob/master/LICENSE
7 *
8 * This file is provided under the following "BSD-style" License:
9 * Redistribution and use in source and binary forms, with or
10 * without modification, are permitted provided that the following
11 * conditions are met:
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
19 * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
20 * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
21 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
23 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
24 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
25 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
26 * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30 * POSSIBILITY OF SUCH DAMAGE.
31 */
32
33 #include <string>
34 #include <vector>
35
36 #include <Eigen/Dense>
37 #include <gtest/gtest.h>
38
39 #include "dart/common/Console.hpp"
40 #include "dart/dynamics/Joint.hpp"
41 #include "dart/dynamics/PointMass.hpp"
42 #include "dart/dynamics/Skeleton.hpp"
43 #include "dart/dynamics/SoftBodyNode.hpp"
44 #include "dart/math/Constants.hpp"
45 #include "dart/simulation/World.hpp"
46 #include "dart/utils/SkelParser.hpp"
47
48 using namespace std;
49 using namespace Eigen;
50 using namespace dart;
51
52 //==============================================================================
53 /// Returns true if the two matrices are equal within the given bound
54 template <class MATRIX>
55 bool equals(
56 const DenseBase<MATRIX>& A, const DenseBase<MATRIX>& B, double tol = 1e-5)
57 {
58 // Get the matrix sizes and sanity check the call
59 const std::size_t n1 = A.cols(), m1 = A.rows();
60 const std::size_t n2 = B.cols(), m2 = B.rows();
61 if (m1 != m2 || n1 != n2)
62 return false;
63
64 // Check each index
65 for (std::size_t i = 0; i < m1; i++)
66 {
67 for (std::size_t j = 0; j < n1; j++)
68 {
69 if (std::isnan(A(i, j)) ^ std::isnan(B(i, j)))
70 return false;
71 else if (std::abs(A(i, j) - B(i, j)) > tol)
72 return false;
73 }
74 }
75
76 // If no problems, the two matrices are equal
77 return true;
78 }
79
80 //==============================================================================
81 class SoftDynamicsTest : public ::testing::Test
82 {
83 public:
84 // Get Skel file list to test.
85 const std::vector<std::string>& getList();
86
87 // Get mass matrix of _skel using Jacobians and inertias of each body
88 // in _skel.
89 MatrixXd getMassMatrix(dynamics::SkeletonPtr _skel);
90
91 // Get augmented mass matrix of _skel using Jacobians and inertias of
92 // each body in _skel.
93 MatrixXd getAugMassMatrix(dynamics::SkeletonPtr _skel);
94
95 // TODO(JS): Not implemented yet.
96 // Compare velocities computed by recursive method, Jacobian, and finite
97 // difference.
98 // void compareVelocities(const std::string& _fileName) {}
99
100 // TODO(JS): Not implemented yet.
101 // Compare accelerations computed by recursive method, Jacobian, and finite
102 // difference.
103 // void compareAccelerations(const std::string& _fileName) {}
104
105 // Compare dynamics terms in equations of motion such as mass matrix, mass
106 // inverse matrix, Coriolis force vector, gravity force vector, and external
107 // force vector.
108 void compareEquationsOfMotion(const std::string& _fileName);
109
110 protected:
111 // Sets up the test fixture.
112 void SetUp() override;
113
114 // Skel file list.
115 std::vector<std::string> list;
116 };
117
118 //==============================================================================
119 void SoftDynamicsTest::SetUp()
120 {
121 list.push_back("dart://sample/skel/test/test_drop_box.skel");
122 }
123
124 //==============================================================================
125 const std::vector<std::string>& SoftDynamicsTest::getList()
126 {
127 return list;
128 }
129
130 //==============================================================================
131 MatrixXd SoftDynamicsTest::getMassMatrix(dynamics::SkeletonPtr _skel)
132 {
133 int skelDof = _skel->getNumDofs();
134
135 MatrixXd skelM = MatrixXd::Zero(skelDof, skelDof); // Mass matrix of skeleton
136 MatrixXd M; // Body mass
137 Eigen::Matrix6d I; // Body inertia
138 math::Jacobian J; // Body Jacobian
139
140 for (std::size_t i = 0; i < _skel->getNumBodyNodes(); ++i)
141 {
142 dynamics::BodyNode* body = _skel->getBodyNode(i);
143
144 int dof = body->getNumDependentGenCoords();
145 I = body->getSpatialInertia();
146 J = body->getJacobian();
147
148 EXPECT_EQ(I.rows(), 6);
149 EXPECT_EQ(I.cols(), 6);
150 EXPECT_EQ(J.rows(), 6);
151 EXPECT_EQ(J.cols(), dof);
152
153 M = J.transpose() * I * J; // (dof x dof) matrix
154
155 for (int j = 0; j < dof; ++j)
156 {
157 int jIdx = body->getDependentGenCoordIndex(j);
158
159 for (int k = 0; k < dof; ++k)
160 {
161 int kIdx = body->getDependentGenCoordIndex(k);
162
163 skelM(jIdx, kIdx) += M(j, k);
164 }
165 }
166 }
167
168 return skelM;
169 }
170
171 //==============================================================================
172 MatrixXd SoftDynamicsTest::getAugMassMatrix(dynamics::SkeletonPtr _skel)
173 {
174 int dof = _skel->getNumDofs();
175 double dt = _skel->getTimeStep();
176
177 MatrixXd M = getMassMatrix(_skel);
178 MatrixXd D = MatrixXd::Zero(dof, dof);
179 MatrixXd K = MatrixXd::Zero(dof, dof);
180 MatrixXd AugM;
181
182 // Compute diagonal matrices of joint damping and joint stiffness
183 for (std::size_t i = 0; i < _skel->getNumBodyNodes(); ++i)
184 {
185 dynamics::BodyNode* body = _skel->getBodyNode(i);
186 dynamics::Joint* joint = body->getParentJoint();
187
188 EXPECT_TRUE(body != nullptr);
189 EXPECT_TRUE(joint != nullptr);
190
191 int dof = joint->getNumDofs();
192
193 for (int j = 0; j < dof; ++j)
194 {
195 int idx = joint->getIndexInSkeleton(j);
196
197 D(idx, idx) = joint->getDampingCoefficient(j);
198 K(idx, idx) = joint->getSpringStiffness(j);
199 }
200 }
201
202 // dynamics::SkeletonPtr softSkel
203 // = dynamic_cast<dynamics::SkeletonPtr>(_skel);
204
205 // if (softSkel != nullptr)
206 // {
207 // for (int i = 0; i < softSkel->getNumSoftBodyNodes(); ++i)
208 // {
209 // dynamics::SoftBodyNode* softBody = softSkel->getSoftBodyNode(i);
210
211 // for (int j = 0; j < softBody->getNumPointMasses(); ++j)
212 // {
213 // dynamics::PointMass* pm = softBody->getPointMass(j);
214
215 // int dof = 3;
216
217 // for (int k = 0; k < dof; ++k)
218 // {
219 // int idx = pm->getIndexInSkeleton(k);
220
221 // D(idx, idx) = softBody->getDampingCoefficient();
222 // K(idx, idx) = softBody->getVertexSpringStiffness();
223 // }
224 // }
225 // }
226 // }
227
228 AugM = M + (dt * D) + (dt * dt * K);
229
230 return AugM;
231 }
232
233 //==============================================================================
234 void SoftDynamicsTest::compareEquationsOfMotion(const std::string& _fileName)
235 {
236 using namespace std;
237 using namespace Eigen;
238 using namespace dart;
239 using namespace math;
240 using namespace dynamics;
241 using namespace simulation;
242 using namespace utils;
243
244 //---------------------------- Settings --------------------------------------
245 // Number of random state tests for each skeletons
246 #ifndef NDEBUG // Debug mode
247 std::size_t nRandomItr = 1;
248 #else
249 std::size_t nRandomItr = 1;
250 #endif
251
252 // Lower and upper bound of configuration for system
253 double lb = -1.5 * constantsd::pi();
254 double ub = 1.5 * constantsd::pi();
255
256 // Lower and upper bound of joint damping and stiffness
257 double lbD = 0.0;
258 double ubD = 10.0;
259 double lbK = 0.0;
260 double ubK = 10.0;
261
262 simulation::WorldPtr myWorld;
263
264 //----------------------------- Tests ----------------------------------------
265 // Check whether multiplication of mass matrix and its inverse is identity
266 // matrix.
267 myWorld = utils::SkelParser::readWorld(_fileName);
268 EXPECT_TRUE(myWorld != nullptr);
269
270 for (std::size_t i = 0; i < myWorld->getNumSkeletons(); ++i)
271 {
272 dynamics::SkeletonPtr softSkel = myWorld->getSkeleton(i);
273
274 int dof = softSkel->getNumDofs();
275 // int nBodyNodes = skel->getNumBodyNodes();
276 int nSoftBodyNodes = 0;
277 if (softSkel != nullptr)
278 nSoftBodyNodes = softSkel->getNumSoftBodyNodes();
279
280 if (dof == 0)
281 {
282 dtmsg << "Skeleton [" << softSkel->getName()
283 << "] is skipped since it has "
284 << "0 DOF." << endl;
285 continue;
286 }
287
288 for (std::size_t j = 0; j < nRandomItr; ++j)
289 {
290 // Random joint stiffness and damping coefficient
291 for (std::size_t k = 0; k < softSkel->getNumBodyNodes(); ++k)
292 {
293 BodyNode* body = softSkel->getBodyNode(k);
294 Joint* joint = body->getParentJoint();
295 int localDof = joint->getNumDofs();
296
297 for (int l = 0; l < localDof; ++l)
298 {
299 joint->setDampingCoefficient(l, Random::uniform(lbD, ubD));
300 joint->setSpringStiffness(l, Random::uniform(lbK, ubK));
301
302 double lbRP = joint->getPositionLowerLimit(l);
303 double ubRP = joint->getPositionUpperLimit(l);
304 joint->setRestPosition(l, Random::uniform(lbRP, ubRP));
305 }
306 }
307
308 // Set random states
309 Skeleton::Configuration x = softSkel->getConfiguration();
310 for (auto k = 0u; k < softSkel->getNumDofs(); ++k)
311 {
312 x.mPositions[k] = Random::uniform(lb, ub);
313 x.mVelocities[k] = Random::uniform(lb, ub);
314 }
315 softSkel->setConfiguration(x);
316
317 //------------------------ Mass Matrix Test ----------------------------
318 // Get matrices
319 MatrixXd M = softSkel->getMassMatrix();
320 MatrixXd M2 = getMassMatrix(softSkel);
321 MatrixXd InvM = softSkel->getInvMassMatrix();
322 MatrixXd M_InvM = M * InvM;
323 MatrixXd InvM_M = InvM * M;
324
325 MatrixXd AugM = softSkel->getAugMassMatrix();
326 MatrixXd AugM2 = getAugMassMatrix(softSkel);
327 MatrixXd InvAugM = softSkel->getInvAugMassMatrix();
328 MatrixXd AugM_InvAugM = AugM * InvAugM;
329 MatrixXd InvAugM_AugM = InvAugM * AugM;
330
331 MatrixXd I = MatrixXd::Identity(dof, dof);
332
333 // Check if the number of generalized coordinates and dimension of mass
334 // matrix are same.
335 EXPECT_EQ(M.rows(), dof);
336 EXPECT_EQ(M.cols(), dof);
337
338 // Check mass matrix
339 EXPECT_TRUE(equals(M, M2, 1e-6));
340 if (!equals(M, M2, 1e-6))
341 {
342 cout << "M :" << endl << M << endl << endl;
343 cout << "M2:" << endl << M2 << endl << endl;
344 }
345
346 // Check augmented mass matrix
347 EXPECT_TRUE(equals(AugM, AugM2, 1e-6));
348 if (!equals(AugM, AugM2, 1e-6))
349 {
350 cout << "AugM :" << endl << AugM << endl << endl;
351 cout << "AugM2:" << endl << AugM2 << endl << endl;
352 }
353
354 // Check if both of (M * InvM) and (InvM * M) are identity.
355 EXPECT_TRUE(equals(M_InvM, I, 1e-6));
356 if (!equals(M_InvM, I, 1e-6))
357 {
358 cout << "InvM :" << endl << InvM << endl << endl;
359 }
360 EXPECT_TRUE(equals(InvM_M, I, 1e-6));
361 if (!equals(InvM_M, I, 1e-6))
362 {
363 cout << "InvM_M:" << endl << InvM_M << endl << endl;
364 }
365
366 // Check if both of (M * InvM) and (InvM * M) are identity.
367 EXPECT_TRUE(equals(AugM_InvAugM, I, 1e-6));
368 if (!equals(AugM_InvAugM, I, 1e-6))
369 {
370 cout << "InvAugM :" << endl << InvAugM << endl << endl;
371 cout << "InvAugM2 :" << endl << AugM.inverse() << endl << endl;
372 cout << "AugM_InvAugM :" << endl << AugM_InvAugM << endl << endl;
373 }
374 EXPECT_TRUE(equals(InvAugM_AugM, I, 1e-6));
375 if (!equals(InvAugM_AugM, I, 1e-6))
376 {
377 cout << "InvAugM_AugM:" << endl << InvAugM_AugM << endl << endl;
378 }
379
380 //------- Coriolis Force Vector and Combined Force Vector Tests --------
381 // Get C1, Coriolis force vector using recursive method
382 VectorXd C = softSkel->getCoriolisForces();
383 VectorXd Cg = softSkel->getCoriolisAndGravityForces();
384
385 // Get C2, Coriolis force vector using inverse dynamics algorithm
386 Vector3d oldGravity = softSkel->getGravity();
387 VectorXd oldTau = softSkel->getForces();
388 VectorXd oldDdq = softSkel->getAccelerations();
389 // TODO(JS): Save external forces of body nodes
390 vector<double> oldKv(nSoftBodyNodes, 0.0);
391 vector<double> oldKe(nSoftBodyNodes, 0.0);
392 vector<double> oldD(nSoftBodyNodes, 0.0);
393 for (int k = 0; k < nSoftBodyNodes; ++k)
394 {
395 assert(softSkel != nullptr);
396 dynamics::SoftBodyNode* sbn = softSkel->getSoftBodyNode(k);
397 oldKv[k] = sbn->getVertexSpringStiffness();
398 oldKe[k] = sbn->getEdgeSpringStiffness();
399 oldD[k] = sbn->getDampingCoefficient();
400 }
401
402 softSkel->resetGeneralizedForces();
403 softSkel->clearExternalForces();
404 softSkel->setAccelerations(VectorXd::Zero(dof));
405 for (int k = 0; k < nSoftBodyNodes; ++k)
406 {
407 assert(softSkel != nullptr);
408 dynamics::SoftBodyNode* sbn = softSkel->getSoftBodyNode(k);
409 sbn->setVertexSpringStiffness(0.0);
410 sbn->setEdgeSpringStiffness(0.0);
411 sbn->setDampingCoefficient(0.0);
412 }
413
414 EXPECT_TRUE(softSkel->getForces() == VectorXd::Zero(dof));
415 EXPECT_TRUE(softSkel->getExternalForces() == VectorXd::Zero(dof));
416 EXPECT_TRUE(softSkel->getAccelerations() == VectorXd::Zero(dof));
417
418 softSkel->setGravity(Vector3d::Zero());
419 EXPECT_TRUE(softSkel->getGravity() == Vector3d::Zero());
420 softSkel->computeInverseDynamics(false, false);
421 VectorXd C2 = softSkel->getForces();
422
423 softSkel->setGravity(oldGravity);
424 EXPECT_TRUE(softSkel->getGravity() == oldGravity);
425 softSkel->computeInverseDynamics(false, false);
426 VectorXd Cg2 = softSkel->getForces();
427
428 EXPECT_TRUE(equals(C, C2, 1e-6));
429 if (!equals(C, C2, 1e-6))
430 {
431 cout << "C :" << C.transpose() << endl;
432 cout << "C2:" << C2.transpose() << endl;
433 }
434
435 EXPECT_TRUE(equals(Cg, Cg2, 1e-6));
436 if (!equals(Cg, Cg2, 1e-6))
437 {
438 cout << "Cg :" << Cg.transpose() << endl;
439 cout << "Cg2:" << Cg2.transpose() << endl;
440 }
441
442 softSkel->setForces(oldTau);
443 softSkel->setAccelerations(oldDdq);
444 // TODO(JS): Restore external forces of body nodes
445 }
446 }
447 }
448
449 //==============================================================================
450 TEST_F(SoftDynamicsTest, compareEquationsOfMotion)
451 {
452 // TODO(JS): Equations of motion for softbody skeleton is not done yet
453
454 // for (int i = 0; i < getList().size(); ++i)
455 // {
456 //#ifndef NDEBUG
457 // dtdbg << getList()[i] << std::endl;
458 //#endif
459 // compareEquationsOfMotion(getList()[i]);
460 // }
461 }