Debian Codegraph @

tutorials/tutorial_collisions/main.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 <random>
34
35 #include <dart/dart.hpp>
36 #include <dart/gui/gui.hpp>
37
38 [[maybe_unused]] const double default_shape_density = 1000; // kg/m^3
39 [[maybe_unused]] const double default_shape_height = 0.1; // m
40 [[maybe_unused]] const double default_shape_width = 0.03; // m
41 [[maybe_unused]] const double default_skin_thickness = 1e-3; // m
42
43 [[maybe_unused]] const double default_start_height = 0.4; // m
44
45 [[maybe_unused]] const double minimum_start_v = 2.5; // m/s
46 [[maybe_unused]] const double maximum_start_v = 4.0; // m/s
47 [[maybe_unused]] const double default_start_v = 3.5; // m/s
48
49 [[maybe_unused]] const double minimum_launch_angle
50 = dart::math::toRadian(30.0); // rad
51 [[maybe_unused]] const double maximum_launch_angle
52 = dart::math::toRadian(70.0); // rad
53 [[maybe_unused]] const double default_launch_angle
54 = dart::math::toRadian(45.0); // rad
55
56 [[maybe_unused]] const double maximum_start_w
57 = 6 * dart::math::constantsd::pi(); // rad/s
58 [[maybe_unused]] const double default_start_w
59 = 3 * dart::math::constantsd::pi(); // rad/s
60
61 [[maybe_unused]] const double ring_spring_stiffness = 0.5;
62 [[maybe_unused]] const double ring_damping_coefficient = 0.05;
63 [[maybe_unused]] const double default_damping_coefficient = 0.001;
64
65 const double default_ground_width = 2;
66 const double default_wall_thickness = 0.1;
67 const double default_wall_height = 1;
68 [[maybe_unused]] const double default_spawn_range
69 = 0.9 * default_ground_width / 2;
70
71 [[maybe_unused]] const double default_restitution = 0.6;
72
73 [[maybe_unused]] const double default_vertex_stiffness = 1000.0;
74 [[maybe_unused]] const double default_edge_stiffness = 1.0;
75 [[maybe_unused]] const double default_soft_damping = 5.0;
76
77 using namespace dart::dynamics;
78 using namespace dart::simulation;
79 using namespace dart::gui;
80 using namespace dart::gui::glut;
81
82 void setupRing(const SkeletonPtr& /*ring*/)
83 {
84 // Set the spring and damping coefficients for the degrees of freedom
85 // Lesson 4a
86
87 // Compute the joint angle needed to form a ring
88 // Lesson 4b
89
90 // Set the BallJoints so that they have the correct rest position angle
91 // Lesson 4b
92
93 // Set the Joints to be in their rest positions
94 // Lesson 4c
95 }
96
97 class MyWindow : public SimWindow
98 {
99 public:
100 MyWindow(
101 const WorldPtr& world,
102 const SkeletonPtr& ball,
103 const SkeletonPtr& softBody,
104 const SkeletonPtr& hybridBody,
105 const SkeletonPtr& rigidChain,
106 const SkeletonPtr& rigidRing)
107 : mRandomize(true),
108 mRD(),
109 mMT(mRD()),
110 mDistribution(-1.0, std::nextafter(1.0, 2.0)),
111 mOriginalBall(ball),
112 mOriginalSoftBody(softBody),
113 mOriginalHybridBody(hybridBody),
114 mOriginalRigidChain(rigidChain),
115 mOriginalRigidRing(rigidRing),
116 mSkelCount(0)
117 {
118 setWorld(world);
119 }
120
121 void keyboard(unsigned char key, int x, int y) override
122 {
123 switch (key)
124 {
125 case '1':
126 addObject(mOriginalBall->cloneSkeleton());
127 break;
128
129 case '2':
130 addObject(mOriginalSoftBody->cloneSkeleton());
131 break;
132
133 case '3':
134 addObject(mOriginalHybridBody->cloneSkeleton());
135 break;
136
137 case '4':
138 addObject(mOriginalRigidChain->cloneSkeleton());
139 break;
140
141 case '5':
142 addRing(mOriginalRigidRing->cloneSkeleton());
143 break;
144
145 case 'd':
146 if (mWorld->getNumSkeletons() > 2)
147 removeSkeleton(mWorld->getSkeleton(2));
148 std::cout << "Remaining objects: " << mWorld->getNumSkeletons() - 2
149 << std::endl;
150 break;
151
152 case 'r':
153 mRandomize = !mRandomize;
154 std::cout << "Randomization: " << (mRandomize ? "on" : "off")
155 << std::endl;
156 break;
157
158 default:
159 SimWindow::keyboard(key, x, y);
160 }
161 }
162
163 void drawWorld() const override
164 {
165 // Make sure lighting is turned on and that polygons get filled in
166 glEnable(GL_LIGHTING);
167 glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
168
169 SimWindow::drawWorld();
170 }
171
172 void displayTimer(int _val) override
173 {
174 // We remove playback and baking, because we want to be able to add and
175 // remove objects during runtime
176 int numIter = mDisplayTimeout / (mWorld->getTimeStep() * 1000);
177 if (mSimulating)
178 {
179 for (int i = 0; i < numIter; i++)
180 timeStepping();
181 }
182 glutPostRedisplay();
183 glutTimerFunc(mDisplayTimeout, refreshTimer, _val);
184 }
185
186 protected:
187 /// Add an object to the world and toss it at the wall
188 bool addObject(const SkeletonPtr& /*object*/)
189 {
190 // Set the starting position for the object
191 // Lesson 3a
192
193 // Add the object to the world
194 // Lesson 3b
195
196 // Compute collisions
197 // Lesson 3c
198
199 // Refuse to add the object if it is in collision
200 // Lesson 3c
201
202 // Create reference frames for setting the initial velocity
203 // Lesson 3d
204
205 // Set the velocities of the reference frames so that we can easily give the
206 // Skeleton the linear and angular velocities that we want
207 // Lesson 3e
208
209 // Use the reference frames to set the velocity of the Skeleton's root
210 // Lesson 3f
211
212 return true;
213 }
214
215 /// Add a ring to the world, and create a BallJoint constraint to ensure that
216 /// it stays in a ring shape
217 void addRing(const SkeletonPtr& ring)
218 {
219 setupRing(ring);
220
221 if (!addObject(ring))
222 return;
223
224 // Create a closed loop to turn the chain into a ring
225 // Lesson 5
226 }
227
228 /// Remove a Skeleton and get rid of the constraint that was associated with
229 /// it, if one existed
230 void removeSkeleton(const SkeletonPtr& skel)
231 {
232 for (std::size_t i = 0; i < mJointConstraints.size(); ++i)
233 {
234 const dart::constraint::DynamicJointConstraintPtr& constraint
235 = mJointConstraints[i];
236
237 if (constraint->getBodyNode1()->getSkeleton() == skel
238 || constraint->getBodyNode2()->getSkeleton() == skel)
239 {
240 mWorld->getConstraintSolver()->removeConstraint(constraint);
241 mJointConstraints.erase(mJointConstraints.begin() + i);
242 break; // There should only be one constraint per skeleton
243 }
244 }
245
246 mWorld->removeSkeleton(skel);
247 }
248
249 /// Flag to keep track of whether or not we are randomizing the tosses
250 bool mRandomize;
251
252 // std library objects that allow us to generate high-quality random numbers
253 std::random_device mRD;
254 std::mt19937 mMT;
255 std::uniform_real_distribution<double> mDistribution;
256
257 /// History of the active JointConstraints so that we can properly delete them
258 /// when a Skeleton gets removed
259 std::vector<dart::constraint::DynamicJointConstraintPtr> mJointConstraints;
260
261 /// A blueprint Skeleton that we will use to spawn balls
262 SkeletonPtr mOriginalBall;
263
264 /// A blueprint Skeleton that we will use to spawn soft bodies
265 SkeletonPtr mOriginalSoftBody;
266
267 /// A blueprint Skeleton that we will use to spawn hybrid bodies
268 SkeletonPtr mOriginalHybridBody;
269
270 /// A blueprint Skeleton that we will use to spawn rigid chains
271 SkeletonPtr mOriginalRigidChain;
272
273 /// A blueprint Skeleton that we will use to spawn rigid rings
274 SkeletonPtr mOriginalRigidRing;
275
276 /// Keep track of how many Skeletons we spawn to ensure we can give them all
277 /// unique names
278 std::size_t mSkelCount;
279 };
280
281 /// Add a rigid body with the specified Joint type to a chain
282 template <class JointType>
283 BodyNode* addRigidBody(
284 const SkeletonPtr& /*chain*/,
285 const std::string& /*name*/,
286 Shape::ShapeType /*type*/,
287 BodyNode* /*parent*/ = nullptr)
288 {
289 // Set the Joint properties
290 // Lesson 1a
291
292 // Create the Joint and Body pair
293 // Lesson 1b
294 BodyNode* bn = nullptr;
295
296 // Make the shape based on the requested Shape type
297 // Lesson 1c
298
299 // Setup the inertia for the body
300 // Lesson 1d
301
302 // Set the coefficient of restitution to make the body more bouncy
303 // Lesson 1e
304
305 // Set damping to make the simulation more stable
306 // Lesson 1f
307
308 return bn;
309 }
310
311 enum SoftShapeType
312 {
313 SOFT_BOX = 0,
314 SOFT_CYLINDER,
315 SOFT_ELLIPSOID
316 };
317
318 /// Add a soft body with the specified Joint type to a chain
319 template <class JointType>
320 BodyNode* addSoftBody(
321 const SkeletonPtr& /*chain*/,
322 const std::string& /*name*/,
323 SoftShapeType /*type*/,
324 BodyNode* /*parent*/ = nullptr)
325 {
326 // Set the Joint properties
327 // Lesson 2a
328
329 // Set the properties of the soft body
330 // Lesson 2b
331
332 // Create the Joint and Body pair
333 // Lesson 2c
334 SoftBodyNode* bn = nullptr;
335
336 // Zero out the inertia for the underlying BodyNode
337 // Lesson 2d
338
339 // Make the shape transparent
340 // Lesson 2e
341
342 return bn;
343 }
344
345 void setAllColors(const SkeletonPtr& object, const Eigen::Vector3d& color)
346 {
347 // Set the color of all the shapes in the object
348 object->setColor(color);
349 }
350
351 SkeletonPtr createBall()
352 {
353 SkeletonPtr ball = Skeleton::create("rigid_ball");
354
355 // Give the ball a body
356 addRigidBody<FreeJoint>(ball, "rigid ball", Shape::ELLIPSOID);
357
358 setAllColors(ball, dart::Color::Red());
359
360 return ball;
361 }
362
363 SkeletonPtr createRigidChain()
364 {
365 SkeletonPtr chain = Skeleton::create("rigid_chain");
366
367 // Add bodies to the chain
368 BodyNode* bn = addRigidBody<FreeJoint>(chain, "rigid box 1", Shape::BOX);
369 bn = addRigidBody<BallJoint>(chain, "rigid cyl 2", Shape::CYLINDER, bn);
370 bn = addRigidBody<BallJoint>(chain, "rigid box 3", Shape::BOX, bn);
371
372 setAllColors(chain, dart::Color::Orange());
373
374 return chain;
375 }
376
377 SkeletonPtr createRigidRing()
378 {
379 SkeletonPtr ring = Skeleton::create("rigid_ring");
380
381 // Add bodies to the ring
382 BodyNode* bn = addRigidBody<FreeJoint>(ring, "rigid box 1", Shape::BOX);
383 bn = addRigidBody<BallJoint>(ring, "rigid cyl 2", Shape::CYLINDER, bn);
384 bn = addRigidBody<BallJoint>(ring, "rigid box 3", Shape::BOX, bn);
385 bn = addRigidBody<BallJoint>(ring, "rigid cyl 4", Shape::CYLINDER, bn);
386 bn = addRigidBody<BallJoint>(ring, "rigid box 5", Shape::BOX, bn);
387 bn = addRigidBody<BallJoint>(ring, "rigid cyl 6", Shape::CYLINDER, bn);
388
389 setAllColors(ring, dart::Color::Blue());
390
391 return ring;
392 }
393
394 SkeletonPtr createSoftBody()
395 {
396 SkeletonPtr soft = Skeleton::create("soft");
397
398 // Add a soft body
399 /*BodyNode* bn =*/addSoftBody<FreeJoint>(soft, "soft box", SOFT_BOX);
400
401 // Add a rigid collision geometry and inertia
402 // Lesson 2f
403
404 setAllColors(soft, dart::Color::Fuchsia());
405
406 return soft;
407 }
408
409 SkeletonPtr createHybridBody()
410 {
411 SkeletonPtr hybrid = Skeleton::create("hybrid");
412
413 // Add a soft body
414 /*BodyNode* bn =*/addSoftBody<FreeJoint>(
415 hybrid, "soft sphere", SOFT_ELLIPSOID);
416
417 // Add a rigid body attached by a WeldJoint
418 // Lesson 2g
419
420 setAllColors(hybrid, dart::Color::Green());
421
422 return hybrid;
423 }
424
425 SkeletonPtr createGround()
426 {
427 SkeletonPtr ground = Skeleton::create("ground");
428
429 BodyNode* bn = ground->createJointAndBodyNodePair<WeldJoint>().second;
430
431 std::shared_ptr<BoxShape> shape = std::make_shared<BoxShape>(Eigen::Vector3d(
432 default_ground_width, default_ground_width, default_wall_thickness));
433 auto shapeNode
434 = bn->createShapeNodeWith<VisualAspect, CollisionAspect, DynamicsAspect>(
435 shape);
436 shapeNode->getVisualAspect()->setColor(Eigen::Vector3d(1.0, 1.0, 1.0));
437
438 return ground;
439 }
440
441 SkeletonPtr createWall()
442 {
443 SkeletonPtr wall = Skeleton::create("wall");
444
445 BodyNode* bn = wall->createJointAndBodyNodePair<WeldJoint>().second;
446
447 std::shared_ptr<BoxShape> shape = std::make_shared<BoxShape>(Eigen::Vector3d(
448 default_wall_thickness, default_ground_width, default_wall_height));
449 auto shapeNode
450 = bn->createShapeNodeWith<VisualAspect, CollisionAspect, DynamicsAspect>(
451 shape);
452 shapeNode->getVisualAspect()->setColor(Eigen::Vector3d(0.8, 0.8, 0.8));
453
454 Eigen::Isometry3d tf(Eigen::Isometry3d::Identity());
455 tf.translation() = Eigen::Vector3d(
456 (default_ground_width + default_wall_thickness) / 2.0,
457 0.0,
458 (default_wall_height - default_wall_thickness) / 2.0);
459 bn->getParentJoint()->setTransformFromParentBodyNode(tf);
460
461 shapeNode->getDynamicsAspect()->setRestitutionCoeff(0.2);
462
463 return wall;
464 }
465
466 int main(int argc, char* argv[])
467 {
468 WorldPtr world = std::make_shared<World>();
469 world->addSkeleton(createGround());
470 world->addSkeleton(createWall());
471
472 MyWindow window(
473 world,
474 createBall(),
475 createSoftBody(),
476 createHybridBody(),
477 createRigidChain(),
478 createRigidRing());
479
480 std::cout << "space bar: simulation on/off" << std::endl;
481 std::cout << "'1': toss a rigid ball" << std::endl;
482 std::cout << "'2': toss a soft body" << std::endl;
483 std::cout << "'3': toss a hybrid soft/rigid body" << std::endl;
484 std::cout << "'4': toss a rigid chain" << std::endl;
485 std::cout << "'5': toss a ring of rigid bodies" << std::endl;
486
487 std::cout << "\n'd': delete the oldest object" << std::endl;
488 std::cout << "'r': toggle randomness" << std::endl;
489
490 std::cout << "\nWarning: Let objects settle before tossing a new one, or the "
491 "simulation could explode."
492 << std::endl;
493 std::cout << " If the simulation freezes, you may need to force quit "
494 "the application.\n"
495 << std::endl;
496
497 glutInit(&argc, argv);
498 window.initWindow(640, 480, "Collisions");
499 glutMainLoop();
500 }