Debian Codegraph @

unittests/integration/test_World.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 <iostream>
34
35 #include <gtest/gtest.h>
36
37 #include "dart/collision/collision.hpp"
38 #include "dart/dynamics/BodyNode.hpp"
39 #include "dart/dynamics/RevoluteJoint.hpp"
40 #include "dart/dynamics/Skeleton.hpp"
41 #include "dart/math/Geometry.hpp"
42 #include "dart/utils/SkelParser.hpp"
43
44 #include "TestHelpers.hpp"
45 #if HAVE_BULLET
46 #include "dart/collision/bullet/bullet.hpp"
47 #endif
48 #include "dart/constraint/BallJointConstraint.hpp"
49 #include "dart/constraint/BoxedLcpConstraintSolver.hpp"
50 #include "dart/constraint/DantzigBoxedLcpSolver.hpp"
51 #include "dart/constraint/PgsBoxedLcpSolver.hpp"
52 #include "dart/simulation/World.hpp"
53
54 using namespace dart;
55 using namespace math;
56 using namespace dynamics;
57 using namespace simulation;
58
59 //==============================================================================
60 TEST(World, AddingAndRemovingSkeletons)
61 {
62 // World
63 WorldPtr world = World::create();
64
65 //-------------------- Test World::removeSkeleton() ------------------------
66 SkeletonPtr skeleton1 = createThreeLinkRobot(
67 Eigen::Vector3d(1.0, 1.0, 1.0),
68 DOF_X,
69 Eigen::Vector3d(1.0, 1.0, 1.0),
70 DOF_Y,
71 Eigen::Vector3d(1.0, 1.0, 1.0),
72 DOF_Z,
73 false,
74 false);
75
76 SkeletonPtr skeleton2 = createThreeLinkRobot(
77 Eigen::Vector3d(1.0, 1.0, 1.0),
78 DOF_X,
79 Eigen::Vector3d(1.0, 1.0, 1.0),
80 DOF_Y,
81 Eigen::Vector3d(1.0, 1.0, 1.0),
82 DOF_Z,
83 false,
84 false);
85
86 SkeletonPtr skeleton3 = createThreeLinkRobot(
87 Eigen::Vector3d(1.0, 1.0, 1.0),
88 DOF_X,
89 Eigen::Vector3d(1.0, 1.0, 1.0),
90 DOF_Y,
91 Eigen::Vector3d(1.0, 1.0, 1.0),
92 DOF_Z,
93 false,
94 false);
95
96 SkeletonPtr skeleton4 = createThreeLinkRobot(
97 Eigen::Vector3d(1.0, 1.0, 1.0),
98 DOF_X,
99 Eigen::Vector3d(1.0, 1.0, 1.0),
100 DOF_Y,
101 Eigen::Vector3d(1.0, 1.0, 1.0),
102 DOF_Z,
103 false,
104 false);
105
106 int nSteps = 20;
107
108 // Empty world
109 for (int i = 0; i < nSteps; ++i)
110 world->step();
111
112 EXPECT_FALSE(world->hasSkeleton(skeleton1));
113 EXPECT_FALSE(world->hasSkeleton(skeleton2));
114 EXPECT_FALSE(world->hasSkeleton(skeleton3));
115 EXPECT_FALSE(world->hasSkeleton(skeleton4));
116
117 // Add skeleton1, skeleton2
118 world->addSkeleton(skeleton1);
119 EXPECT_TRUE(world->hasSkeleton(skeleton1));
120 world->addSkeleton(skeleton2);
121 EXPECT_TRUE(world->hasSkeleton(skeleton2));
122 EXPECT_TRUE(world->getNumSkeletons() == 2);
123 for (int i = 0; i < nSteps; ++i)
124 world->step();
125
126 std::string s1name = skeleton1->getName();
127 std::string s2name = skeleton2->getName();
128 EXPECT_TRUE(skeleton1 == world->getSkeleton(s1name));
129 EXPECT_TRUE(skeleton2 == world->getSkeleton(s2name));
130
131 // Remove skeleton2
132 world->removeSkeleton(skeleton2);
133 EXPECT_TRUE(world->getNumSkeletons() == 1);
134 for (int i = 0; i < nSteps; ++i)
135 world->step();
136
137 EXPECT_TRUE(skeleton1 == world->getSkeleton(s1name));
138 EXPECT_FALSE(skeleton2 == world->getSkeleton(s2name));
139 EXPECT_TRUE(world->getSkeleton(s2name) == nullptr);
140
141 // Add skeleton3, skeleton4
142 world->addSkeleton(skeleton3);
143 EXPECT_TRUE(world->hasSkeleton(skeleton3));
144 world->addSkeleton(skeleton4);
145 EXPECT_TRUE(world->hasSkeleton(skeleton4));
146 EXPECT_TRUE(world->getNumSkeletons() == 3);
147 for (int i = 0; i < nSteps; ++i)
148 world->step();
149
150 std::string s3name = skeleton3->getName();
151 std::string s4name = skeleton4->getName();
152
153 EXPECT_TRUE(s3name == s2name);
154 EXPECT_TRUE(skeleton3 == world->getSkeleton(s3name));
155 EXPECT_TRUE(skeleton4 == world->getSkeleton(s4name));
156
157 skeleton4->setName(skeleton1->getName());
158 EXPECT_FALSE(skeleton4->getName() == skeleton1->getName());
159
160 // Remove skeleton1
161 world->removeSkeleton(skeleton1);
162 EXPECT_TRUE(world->getNumSkeletons() == 2);
163 for (int i = 0; i < nSteps; ++i)
164 world->step();
165
166 EXPECT_FALSE(skeleton1 == world->getSkeleton(s1name));
167 EXPECT_TRUE(world->getSkeleton(s1name) == nullptr);
168
169 // Remove all the skeletons
170 world->removeAllSkeletons();
171 EXPECT_EQ((int)world->getNumSkeletons(), 0);
172 for (int i = 0; i < nSteps; ++i)
173 world->step();
174
175 EXPECT_FALSE(skeleton3 == world->getSkeleton(s3name));
176 EXPECT_TRUE(world->getSkeleton(s3name) == nullptr);
177
178 EXPECT_FALSE(skeleton4 == world->getSkeleton(s4name));
179 EXPECT_TRUE(world->getSkeleton(s4name) == nullptr);
180
181 // An error will be thrown here if Skeletons are not being removed correctly
182 skeleton1->setName(skeleton4->getName());
183 }
184
185 //==============================================================================
186 TEST(World, Cloning)
187 {
188 // Create a list of skel files to test with
189 std::vector<common::Uri> fileList;
190 fileList.push_back("dart://sample/skel/test/chainwhipa.skel");
191 fileList.push_back("dart://sample/skel/test/single_pendulum.skel");
192 fileList.push_back(
193 "dart://sample/skel/test/single_pendulum_euler_joint.skel");
194 fileList.push_back("dart://sample/skel/test/single_pendulum_ball_joint.skel");
195 fileList.push_back("dart://sample/skel/test/double_pendulum.skel");
196 fileList.push_back(
197 "dart://sample/skel/test/double_pendulum_euler_joint.skel");
198 fileList.push_back("dart://sample/skel/test/double_pendulum_ball_joint.skel");
199 fileList.push_back(
200 "dart://sample/skel/test/serial_chain_revolute_joint.skel");
201 fileList.push_back(
202 "dart://sample/skel/test/serial_chain_eulerxyz_joint.skel");
203 fileList.push_back("dart://sample/skel/test/serial_chain_ball_joint.skel");
204 fileList.push_back("dart://sample/skel/test/serial_chain_ball_joint_20.skel");
205 fileList.push_back("dart://sample/skel/test/serial_chain_ball_joint_40.skel");
206 fileList.push_back("dart://sample/skel/test/simple_tree_structure.skel");
207 fileList.push_back(
208 "dart://sample/skel/test/simple_tree_structure_euler_joint.skel");
209 fileList.push_back(
210 "dart://sample/skel/test/simple_tree_structure_ball_joint.skel");
211 fileList.push_back("dart://sample/skel/test/tree_structure.skel");
212 fileList.push_back("dart://sample/skel/test/tree_structure_euler_joint.skel");
213 fileList.push_back("dart://sample/skel/test/tree_structure_ball_joint.skel");
214 fileList.push_back("dart://sample/skel/fullbody1.skel");
215
216 std::vector<dart::simulation::WorldPtr> worlds;
217 for (std::size_t i = 0; i < fileList.size(); ++i)
218 worlds.push_back(utils::SkelParser::readWorld(fileList[i]));
219
220 for (std::size_t i = 0; i < worlds.size(); ++i)
221 {
222 dart::simulation::WorldPtr original = worlds[i];
223 std::vector<dart::simulation::WorldPtr> clones;
224 clones.push_back(original);
225 for (std::size_t j = 1; j < 5; ++j)
226 clones.push_back(clones[j - 1]->clone());
227
228 #ifndef NDEBUG // Debug mode
229 std::size_t numIterations = 3;
230 #else
231 std::size_t numIterations = 500;
232 #endif
233
234 for (std::size_t j = 0; j < numIterations; ++j)
235 {
236 for (std::size_t k = 0; k < original->getNumSkeletons(); ++k)
237 {
238 dart::dynamics::SkeletonPtr skel = original->getSkeleton(k);
239
240 // Generate a random command vector
241 Eigen::VectorXd commands = skel->getCommands();
242 for (int q = 0; q < commands.size(); ++q)
243 commands[q] = Random::uniform(-0.1, 0.1);
244
245 // Assign the command vector to each clone of the kth skeleton
246 for (std::size_t c = 0; c < clones.size(); ++c)
247 {
248 dart::dynamics::SkeletonPtr skelClone = clones[c]->getSkeleton(k);
249 skelClone->setCommands(commands);
250 }
251 }
252
253 // Step each clone forward
254 for (std::size_t c = 0; c < clones.size(); ++c)
255 clones[c]->step(false);
256 }
257
258 for (std::size_t c = 0; c < clones.size(); ++c)
259 {
260 for (std::size_t k = 0; k < original->getNumSkeletons(); ++k)
261 {
262 dart::dynamics::SkeletonPtr skel = original->getSkeleton(k);
263 dart::dynamics::SkeletonPtr clone = clones[c]->getSkeleton(k);
264
265 EXPECT_TRUE(equals(skel->getPositions(), clone->getPositions(), 0));
266 EXPECT_TRUE(equals(skel->getVelocities(), clone->getVelocities(), 0));
267 EXPECT_TRUE(
268 equals(skel->getAccelerations(), clone->getAccelerations(), 0));
269 EXPECT_TRUE(equals(skel->getForces(), clone->getForces(), 0));
270 }
271 }
272 }
273 }
274
275 //==============================================================================
276 TEST(World, ValidatingClones)
277 {
278 // Create a list of skel files to test with
279 std::vector<common::Uri> fileList;
280 fileList.push_back("dart://sample/skel/test/chainwhipa.skel");
281 fileList.push_back("dart://sample/skel/test/single_pendulum.skel");
282 fileList.push_back(
283 "dart://sample/skel/test/single_pendulum_euler_joint.skel");
284 fileList.push_back("dart://sample/skel/test/single_pendulum_ball_joint.skel");
285 fileList.push_back("dart://sample/skel/test/double_pendulum.skel");
286 fileList.push_back(
287 "dart://sample/skel/test/double_pendulum_euler_joint.skel");
288 fileList.push_back("dart://sample/skel/test/double_pendulum_ball_joint.skel");
289 fileList.push_back(
290 "dart://sample/skel/test/serial_chain_revolute_joint.skel");
291 fileList.push_back(
292 "dart://sample/skel/test/serial_chain_eulerxyz_joint.skel");
293 fileList.push_back("dart://sample/skel/test/serial_chain_ball_joint.skel");
294 fileList.push_back("dart://sample/skel/test/serial_chain_ball_joint_20.skel");
295 fileList.push_back("dart://sample/skel/test/serial_chain_ball_joint_40.skel");
296 fileList.push_back("dart://sample/skel/test/simple_tree_structure.skel");
297 fileList.push_back(
298 "dart://sample/skel/test/simple_tree_structure_euler_joint.skel");
299 fileList.push_back(
300 "dart://sample/skel/test/simple_tree_structure_ball_joint.skel");
301 fileList.push_back("dart://sample/skel/test/tree_structure.skel");
302 fileList.push_back("dart://sample/skel/test/tree_structure_euler_joint.skel");
303 fileList.push_back("dart://sample/skel/test/tree_structure_ball_joint.skel");
304 fileList.push_back("dart://sample/skel/fullbody1.skel");
305
306 std::vector<dart::simulation::WorldPtr> worlds;
307 for (std::size_t i = 0; i < fileList.size(); ++i)
308 {
309 worlds.push_back(utils::SkelParser::readWorld(fileList[i]));
310
311 // Set non default collision detector
312 #if HAVE_BULLET
313 worlds.back()->getConstraintSolver()->setCollisionDetector(
314 collision::BulletCollisionDetector::create());
315 #else
316 worlds.back()->getConstraintSolver()->setCollisionDetector(
317 collision::DARTCollisionDetector::create());
318 #endif
319 }
320
321 for (std::size_t i = 0; i < worlds.size(); ++i)
322 {
323 dart::simulation::WorldPtr original = worlds[i];
324 std::vector<dart::simulation::WorldPtr> clones;
325 clones.push_back(original);
326 for (std::size_t j = 1; j < 5; ++j)
327 {
328 clones.push_back(clones[j - 1]->clone());
329
330 auto originalCD = original->getConstraintSolver()->getCollisionDetector();
331 auto cloneCD
332 = clones.back()->getConstraintSolver()->getCollisionDetector();
333
334 std::string originalCDType = originalCD->getType();
335 std::string cloneCDType = cloneCD->getType();
336
337 EXPECT_EQ(originalCDType, cloneCDType);
338 }
339 }
340 }
341
342 //==============================================================================
343 simulation::WorldPtr createWorld()
344 {
345 // Create and initialize the world
346 simulation::WorldPtr world
347 = utils::SkelParser::readWorld("dart://sample/skel/chain.skel");
348 assert(world != nullptr);
349
350 // Create and initialize the world
351 world->setGravity(Eigen::Vector3d(0.0, -9.81, 0.0));
352 world->setTimeStep(1.0 / 2000);
353
354 const auto dof = world->getSkeleton(0)->getNumDofs();
355 Eigen::VectorXd initPose = Eigen::VectorXd::Zero(static_cast<int>(dof));
356 initPose[20] = 3.14159 * 0.4;
357 initPose[23] = 3.14159 * 0.4;
358 initPose[26] = 3.14159 * 0.4;
359 initPose[29] = 3.14159 * 0.4;
360 world->getSkeleton(0)->setPositions(initPose);
361
362 // Create a ball joint constraint
363 BodyNode* bd1 = world->getSkeleton(0)->getBodyNode("link 6");
364 BodyNode* bd2 = world->getSkeleton(0)->getBodyNode("link 10");
365 EXPECT_TRUE(bd1 != nullptr);
366 EXPECT_TRUE(bd2 != nullptr);
367 const Eigen::Vector3d offset(0.0, 0.025, 0.0);
368 const Eigen::Vector3d jointPos = bd1->getTransform() * offset;
369 auto cl
370 = std::make_shared<constraint::BallJointConstraint>(bd1, bd2, jointPos);
371 world->getConstraintSolver()->addConstraint(cl);
372
373 return world;
374 }
375
376 //==============================================================================
377 TEST(World, SetNewConstraintSolver)
378 {
379 auto world = createWorld();
380 EXPECT_TRUE(world->getConstraintSolver()->getSkeletons().size() == 1);
381 EXPECT_TRUE(world->getConstraintSolver()->getNumConstraints() == 1);
382
383 auto solver1 = std::make_unique<constraint::BoxedLcpConstraintSolver>(
384 std::make_shared<constraint::DantzigBoxedLcpSolver>());
385 EXPECT_TRUE(solver1->getSkeletons().size() == 0);
386 EXPECT_TRUE(solver1->getNumConstraints() == 0);
387
388 world->setConstraintSolver(std::move(solver1));
389 EXPECT_TRUE(world->getConstraintSolver()->getSkeletons().size() == 1);
390 EXPECT_TRUE(world->getConstraintSolver()->getNumConstraints() == 1);
391
392 auto solver2 = std::make_unique<constraint::BoxedLcpConstraintSolver>(
393 std::make_shared<constraint::PgsBoxedLcpSolver>());
394 EXPECT_TRUE(solver2->getSkeletons().size() == 0);
395 EXPECT_TRUE(solver2->getNumConstraints() == 0);
396
397 world->setConstraintSolver(std::move(solver2));
398 EXPECT_TRUE(world->getConstraintSolver()->getSkeletons().size() == 1);
399 EXPECT_TRUE(world->getConstraintSolver()->getNumConstraints() == 1);
400 }