Debian Codegraph @

tutorials/tutorial_collisions_finished/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 const double default_shape_density = 1000; // kg/m^3
39 const double default_shape_height = 0.1; // m
40 const double default_shape_width = 0.03; // m
41 const double default_skin_thickness = 1e-3; // m
42
43 const double default_start_height = 0.4; // m
44
45 const double minimum_start_v = 2.5; // m/s
46 const double maximum_start_v = 4.0; // m/s
47 const double default_start_v = 3.5; // m/s
48
49 const double minimum_launch_angle = dart::math::toRadian(30.0); // rad
50 const double maximum_launch_angle = dart::math::toRadian(70.0); // rad
51 const double default_launch_angle = dart::math::toRadian(45.0); // rad
52
53 const double maximum_start_w = 6 * dart::math::constantsd::pi(); // rad/s
54 const double default_start_w = 3 * dart::math::constantsd::pi(); // rad/s
55
56 const double ring_spring_stiffness = 0.5;
57 const double ring_damping_coefficient = 0.05;
58 const double default_damping_coefficient = 0.001;
59
60 const double default_ground_width = 2;
61 const double default_wall_thickness = 0.1;
62 const double default_wall_height = 1;
63 const double default_spawn_range = 0.9 * default_ground_width / 2;
64
65 const double default_restitution = 0.6;
66
67 const double default_vertex_stiffness = 1000.0;
68 const double default_edge_stiffness = 1.0;
69 const double default_soft_damping = 5.0;
70
71 using namespace dart::dynamics;
72 using namespace dart::simulation;
73
74 void setupRing(const SkeletonPtr& ring)
75 {
76 // Set the spring and damping coefficients for the degrees of freedom
77 for (std::size_t i = 6; i < ring->getNumDofs(); ++i)
78 {
79 DegreeOfFreedom* dof = ring->getDof(i);
80 dof->setSpringStiffness(ring_spring_stiffness);
81 dof->setDampingCoefficient(ring_damping_coefficient);
82 }
83
84 // Compute the joint angle needed to form a ring
85 std::size_t numEdges = ring->getNumBodyNodes();
86 double angle = 2 * dart::math::constantsd::pi() / numEdges;
87
88 // Set the BallJoints so that they have the correct rest position angle
89 for (std::size_t i = 1; i < ring->getNumJoints(); ++i)
90 {
91 Joint* joint = ring->getJoint(i);
92 Eigen::AngleAxisd rotation(angle, Eigen::Vector3d(0, 1, 0));
93 Eigen::Vector3d restPos
94 = BallJoint::convertToPositions(Eigen::Matrix3d(rotation));
95
96 for (std::size_t j = 0; j < 3; ++j)
97 joint->setRestPosition(j, restPos[j]);
98 }
99
100 // Set the Joints to be in their rest positions
101 for (std::size_t i = 6; i < ring->getNumDofs(); ++i)
102 {
103 DegreeOfFreedom* dof = ring->getDof(i);
104 dof->setPosition(dof->getRestPosition());
105 }
106 }
107
108 class MyWindow : public dart::gui::glut::SimWindow
109 {
110 public:
111 MyWindow(
112 const WorldPtr& world,
113 const SkeletonPtr& ball,
114 const SkeletonPtr& softBody,
115 const SkeletonPtr& hybridBody,
116 const SkeletonPtr& rigidChain,
117 const SkeletonPtr& rigidRing)
118 : mRandomize(true),
119 mRD(),
120 mMT(mRD()),
121 mDistribution(-1.0, std::nextafter(1.0, 2.0)),
122 mOriginalBall(ball),
123 mOriginalSoftBody(softBody),
124 mOriginalHybridBody(hybridBody),
125 mOriginalRigidChain(rigidChain),
126 mOriginalRigidRing(rigidRing),
127 mSkelCount(0)
128 {
129 setWorld(world);
130 }
131
132 void keyboard(unsigned char key, int x, int y) override
133 {
134 switch (key)
135 {
136 case '1':
137 addObject(mOriginalBall->cloneSkeleton());
138 break;
139
140 case '2':
141 addObject(mOriginalSoftBody->cloneSkeleton());
142 break;
143
144 case '3':
145 addObject(mOriginalHybridBody->cloneSkeleton());
146 break;
147
148 case '4':
149 addObject(mOriginalRigidChain->cloneSkeleton());
150 break;
151
152 case '5':
153 addRing(mOriginalRigidRing->cloneSkeleton());
154 break;
155
156 case 'd':
157 if (mWorld->getNumSkeletons() > 2)
158 removeSkeleton(mWorld->getSkeleton(2));
159 std::cout << "Remaining objects: " << mWorld->getNumSkeletons() - 2
160 << std::endl;
161 break;
162
163 case 'r':
164 mRandomize = !mRandomize;
165 std::cout << "Randomization: " << (mRandomize ? "on" : "off")
166 << std::endl;
167 break;
168
169 default:
170 SimWindow::keyboard(key, x, y);
171 }
172 }
173
174 void drawWorld() const override
175 {
176 // Make sure lighting is turned on and that polygons get filled in
177 glEnable(GL_LIGHTING);
178 glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
179
180 SimWindow::drawWorld();
181 }
182
183 void displayTimer(int _val) override
184 {
185 // We remove playback and baking, because we want to be able to add and
186 // remove objects during runtime
187 int numIter = mDisplayTimeout / (mWorld->getTimeStep() * 1000);
188 if (mSimulating)
189 {
190 for (int i = 0; i < numIter; i++)
191 timeStepping();
192 }
193 glutPostRedisplay();
194 glutTimerFunc(mDisplayTimeout, refreshTimer, _val);
195 }
196
197 protected:
198 /// Add an object to the world and toss it at the wall
199 bool addObject(const SkeletonPtr& object)
200 {
201 // Set the starting position for the object
202 Eigen::Vector6d positions(Eigen::Vector6d::Zero());
203
204 // If randomization is on, we will randomize the starting y-location
205 if (mRandomize)
206 positions[4] = default_spawn_range * mDistribution(mMT);
207
208 positions[5] = default_start_height;
209 object->getJoint(0)->setPositions(positions);
210
211 // Add the object to the world
212 object->setName(object->getName() + std::to_string(mSkelCount++));
213
214 // Look through the collisions to see if the new object would start in
215 // collision with something
216 auto collisionEngine
217 = mWorld->getConstraintSolver()->getCollisionDetector();
218 auto collisionGroup = mWorld->getConstraintSolver()->getCollisionGroup();
219 auto newGroup = collisionEngine->createCollisionGroup(object.get());
220
221 dart::collision::CollisionOption option;
222 dart::collision::CollisionResult result;
223 bool collision = collisionGroup->collide(newGroup.get(), option, &result);
224
225 // If the new object is not in collision
226 if (!collision)
227 {
228 mWorld->addSkeleton(object);
229 }
230 else
231 {
232 // or refuse to add the object if it is in collision
233 std::cout << "The new object spawned in a collision. "
234 << "It will not be added to the world." << std::endl;
235 return false;
236 }
237
238 // Create reference frames for setting the initial velocity
239 Eigen::Isometry3d centerTf(Eigen::Isometry3d::Identity());
240 centerTf.translation() = object->getCOM();
241 SimpleFrame center(Frame::World(), "center", centerTf);
242
243 // Set the velocities of the reference frames so that we can easily give the
244 // Skeleton the linear and angular velocities that we want
245 double angle = default_launch_angle;
246 double speed = default_start_v;
247 double angular_speed = default_start_w;
248 if (mRandomize)
249 {
250 angle = (mDistribution(mMT) + 1.0) / 2.0
251 * (maximum_launch_angle - minimum_launch_angle)
252 + minimum_launch_angle;
253
254 speed = (mDistribution(mMT) + 1.0) / 2.0
255 * (maximum_start_v - minimum_start_v)
256 + minimum_start_v;
257
258 angular_speed = mDistribution(mMT) * maximum_start_w;
259 }
260
261 Eigen::Vector3d v = speed * Eigen::Vector3d(cos(angle), 0.0, sin(angle));
262 Eigen::Vector3d w = angular_speed * Eigen::Vector3d::UnitY();
263 center.setClassicDerivatives(v, w);
264
265 SimpleFrame ref(&center, "root_reference");
266 ref.setRelativeTransform(object->getBodyNode(0)->getTransform(&center));
267
268 // Use the reference frames to set the velocity of the Skeleton's root
269 object->getJoint(0)->setVelocities(ref.getSpatialVelocity());
270
271 return true;
272 }
273
274 /// Add a ring to the world, and create a BallJoint constraint to ensure that
275 /// it stays in a ring shape
276 void addRing(const SkeletonPtr& ring)
277 {
278 setupRing(ring);
279
280 if (!addObject(ring))
281 return;
282
283 // Create a closed loop to turn the chain into a ring
284 BodyNode* head = ring->getBodyNode(0);
285 BodyNode* tail = ring->getBodyNode(ring->getNumBodyNodes() - 1);
286
287 // Compute the offset where the JointConstraint should be located
288 Eigen::Vector3d offset = Eigen::Vector3d(0, 0, default_shape_height / 2.0);
289 offset = tail->getWorldTransform() * offset;
290 auto constraint = std::make_shared<dart::constraint::BallJointConstraint>(
291 head, tail, offset);
292
293 mWorld->getConstraintSolver()->addConstraint(constraint);
294 mJointConstraints.push_back(constraint);
295 }
296
297 /// Remove a Skeleton and get rid of the constraint that was associated with
298 /// it, if one existed
299 void removeSkeleton(const SkeletonPtr& skel)
300 {
301 for (std::size_t i = 0; i < mJointConstraints.size(); ++i)
302 {
303 const dart::constraint::DynamicJointConstraintPtr& constraint
304 = mJointConstraints[i];
305
306 if (constraint->getBodyNode1()->getSkeleton() == skel
307 || constraint->getBodyNode2()->getSkeleton() == skel)
308 {
309 mWorld->getConstraintSolver()->removeConstraint(constraint);
310 mJointConstraints.erase(mJointConstraints.begin() + i);
311 break; // There should only be one constraint per skeleton
312 }
313 }
314
315 mWorld->removeSkeleton(skel);
316 }
317
318 /// Flag to keep track of whether or not we are randomizing the tosses
319 bool mRandomize;
320
321 // std library objects that allow us to generate high-quality random numbers
322 std::random_device mRD;
323 std::mt19937 mMT;
324 std::uniform_real_distribution<double> mDistribution;
325
326 /// History of the active JointConstraints so that we can properly delete them
327 /// when a Skeleton gets removed
328 std::vector<dart::constraint::DynamicJointConstraintPtr> mJointConstraints;
329
330 /// A blueprint Skeleton that we will use to spawn balls
331 SkeletonPtr mOriginalBall;
332
333 /// A blueprint Skeleton that we will use to spawn soft bodies
334 SkeletonPtr mOriginalSoftBody;
335
336 /// A blueprint Skeleton that we will use to spawn hybrid bodies
337 SkeletonPtr mOriginalHybridBody;
338
339 /// A blueprint Skeleton that we will use to spawn rigid chains
340 SkeletonPtr mOriginalRigidChain;
341
342 /// A blueprint Skeleton that we will use to spawn rigid rings
343 SkeletonPtr mOriginalRigidRing;
344
345 /// Keep track of how many Skeletons we spawn to ensure we can give them all
346 /// unique names
347 std::size_t mSkelCount;
348 };
349
350 /// Add a rigid body with the specified Joint type to a chain
351 template <class JointType>
352 BodyNode* addRigidBody(
353 const SkeletonPtr& chain,
354 const std::string& name,
355 Shape::ShapeType type,
356 BodyNode* parent = nullptr)
357 {
358 // Set the Joint properties
359 typename JointType::Properties properties;
360 properties.mName = name + "_joint";
361 if (parent)
362 {
363 // If the body has a parent, we should position the joint to be in the
364 // middle of the centers of the two bodies
365 Eigen::Isometry3d tf(Eigen::Isometry3d::Identity());
366 tf.translation() = Eigen::Vector3d(0, 0, default_shape_height / 2.0);
367 properties.mT_ParentBodyToJoint = tf;
368 properties.mT_ChildBodyToJoint = tf.inverse();
369 }
370
371 // Create the Joint and Body pair
372 BodyNode* bn = chain
373 ->createJointAndBodyNodePair<JointType>(
374 parent, properties, BodyNode::AspectProperties(name))
375 .second;
376
377 // Make the shape based on the requested Shape type
378 ShapePtr shape;
379 if (Shape::BOX == type)
380 {
381 shape = std::make_shared<BoxShape>(Eigen::Vector3d(
382 default_shape_width, default_shape_width, default_shape_height));
383 }
384 else if (Shape::CYLINDER == type)
385 {
386 shape = std::make_shared<CylinderShape>(
387 default_shape_width / 2.0, default_shape_height);
388 }
389 else if (Shape::ELLIPSOID == type)
390 {
391 shape = std::make_shared<EllipsoidShape>(
392 default_shape_height * Eigen::Vector3d::Ones());
393 }
394
395 auto shapeNode
396 = bn->createShapeNodeWith<VisualAspect, CollisionAspect, DynamicsAspect>(
397 shape);
398
399 // Setup the inertia for the body
400 Inertia inertia;
401 double mass = default_shape_density * shape->getVolume();
402 inertia.setMass(mass);
403 inertia.setMoment(shape->computeInertia(mass));
404 bn->setInertia(inertia);
405
406 // Set the coefficient of restitution to make the body more bouncy
407 shapeNode->getDynamicsAspect()->setRestitutionCoeff(default_restitution);
408
409 // Set damping to make the simulation more stable
410 if (parent)
411 {
412 Joint* joint = bn->getParentJoint();
413 for (std::size_t i = 0; i < joint->getNumDofs(); ++i)
414 joint->getDof(i)->setDampingCoefficient(default_damping_coefficient);
415 }
416
417 return bn;
418 }
419
420 enum SoftShapeType
421 {
422 SOFT_BOX = 0,
423 SOFT_CYLINDER,
424 SOFT_ELLIPSOID
425 };
426
427 /// Add a soft body with the specified Joint type to a chain
428 template <class JointType>
429 BodyNode* addSoftBody(
430 const SkeletonPtr& chain,
431 const std::string& name,
432 SoftShapeType type,
433 BodyNode* parent = nullptr)
434 {
435 // Set the Joint properties
436 typename JointType::Properties joint_properties;
437 joint_properties.mName = name + "_joint";
438 if (parent)
439 {
440 // If the body has a parent, we should position the joint to be in the
441 // middle of the centers of the two bodies
442 Eigen::Isometry3d tf(Eigen::Isometry3d::Identity());
443 tf.translation() = Eigen::Vector3d(0, 0, default_shape_height / 2.0);
444 joint_properties.mT_ParentBodyToJoint = tf;
445 joint_properties.mT_ChildBodyToJoint = tf.inverse();
446 }
447
448 // Set the properties of the soft body
449 SoftBodyNode::UniqueProperties soft_properties;
450 // Use the SoftBodyNodeHelper class to create the geometries for the
451 // SoftBodyNode
452 if (SOFT_BOX == type)
453 {
454 // Make a wide and short box
455 double width = default_shape_height, height = 2 * default_shape_width;
456 Eigen::Vector3d dims(width, width, height);
457
458 double mass
459 = 2 * dims[0] * dims[1] + 2 * dims[0] * dims[2] + 2 * dims[1] * dims[2];
460 mass *= default_shape_density * default_skin_thickness;
461 soft_properties = SoftBodyNodeHelper::makeBoxProperties(
462 dims, Eigen::Isometry3d::Identity(), Eigen::Vector3i(4, 4, 4), mass);
463 }
464 else if (SOFT_CYLINDER == type)
465 {
466 // Make a wide and short cylinder
467 double radius = default_shape_height / 2.0,
468 height = 2 * default_shape_width;
469
470 // Mass of center
471 double mass = default_shape_density * height * 2
472 * dart::math::constantsd::pi() * radius
473 * default_skin_thickness;
474 // Mass of top and bottom
475 mass += 2 * default_shape_density * dart::math::constantsd::pi()
476 * pow(radius, 2) * default_skin_thickness;
477 soft_properties = SoftBodyNodeHelper::makeCylinderProperties(
478 radius, height, 8, 3, 2, mass);
479 }
480 else if (SOFT_ELLIPSOID == type)
481 {
482 double radius = default_shape_height / 2.0;
483 Eigen::Vector3d dims = 2 * radius * Eigen::Vector3d::Ones();
484 double mass = default_shape_density * 4.0 * dart::math::constantsd::pi()
485 * pow(radius, 2) * default_skin_thickness;
486 soft_properties
487 = SoftBodyNodeHelper::makeEllipsoidProperties(dims, 6, 6, mass);
488 }
489 soft_properties.mKv = default_vertex_stiffness;
490 soft_properties.mKe = default_edge_stiffness;
491 soft_properties.mDampCoeff = default_soft_damping;
492
493 // Create the Joint and Body pair
494 SoftBodyNode::Properties body_properties(
495 BodyNode::AspectProperties(name), soft_properties);
496 SoftBodyNode* bn = chain
497 ->createJointAndBodyNodePair<JointType, SoftBodyNode>(
498 parent, joint_properties, body_properties)
499 .second;
500
501 // Zero out the inertia for the underlying BodyNode
502 Inertia inertia;
503 inertia.setMoment(1e-8 * Eigen::Matrix3d::Identity());
504 inertia.setMass(1e-8);
505 bn->setInertia(inertia);
506
507 // Make the shape transparent
508 bn->setAlpha(0.4);
509
510 return bn;
511 }
512
513 void setAllColors(const SkeletonPtr& object, const Eigen::Vector3d& color)
514 {
515 // Set the color of all the shapes in the object
516 object->setColor(color);
517 }
518
519 SkeletonPtr createBall()
520 {
521 SkeletonPtr ball = Skeleton::create("rigid_ball");
522
523 // Give the ball a body
524 addRigidBody<FreeJoint>(ball, "rigid ball", Shape::ELLIPSOID);
525
526 setAllColors(ball, dart::Color::Red());
527
528 return ball;
529 }
530
531 SkeletonPtr createRigidChain()
532 {
533 SkeletonPtr chain = Skeleton::create("rigid_chain");
534
535 // Add bodies to the chain
536 BodyNode* bn = addRigidBody<FreeJoint>(chain, "rigid box 1", Shape::BOX);
537 bn = addRigidBody<BallJoint>(chain, "rigid cyl 2", Shape::CYLINDER, bn);
538 bn = addRigidBody<BallJoint>(chain, "rigid box 3", Shape::BOX, bn);
539
540 setAllColors(chain, dart::Color::Orange());
541
542 return chain;
543 }
544
545 SkeletonPtr createRigidRing()
546 {
547 SkeletonPtr ring = Skeleton::create("rigid_ring");
548
549 // Add bodies to the ring
550 BodyNode* bn = addRigidBody<FreeJoint>(ring, "rigid box 1", Shape::BOX);
551 bn = addRigidBody<BallJoint>(ring, "rigid cyl 2", Shape::CYLINDER, bn);
552 bn = addRigidBody<BallJoint>(ring, "rigid box 3", Shape::BOX, bn);
553 bn = addRigidBody<BallJoint>(ring, "rigid cyl 4", Shape::CYLINDER, bn);
554 bn = addRigidBody<BallJoint>(ring, "rigid box 5", Shape::BOX, bn);
555 bn = addRigidBody<BallJoint>(ring, "rigid cyl 6", Shape::CYLINDER, bn);
556
557 setAllColors(ring, dart::Color::Blue());
558
559 return ring;
560 }
561
562 SkeletonPtr createSoftBody()
563 {
564 SkeletonPtr soft = Skeleton::create("soft");
565
566 // Add a soft body
567 BodyNode* bn = addSoftBody<FreeJoint>(soft, "soft box", SOFT_BOX);
568
569 // Add a rigid collision geometry and inertia
570 double width = default_shape_height, height = 2 * default_shape_width;
571 Eigen::Vector3d dims(width, width, height);
572 dims *= 0.6;
573 std::shared_ptr<BoxShape> box = std::make_shared<BoxShape>(dims);
574 bn->createShapeNodeWith<VisualAspect, CollisionAspect, DynamicsAspect>(box);
575
576 Inertia inertia;
577 inertia.setMass(default_shape_density * box->getVolume());
578 inertia.setMoment(box->computeInertia(inertia.getMass()));
579 bn->setInertia(inertia);
580
581 setAllColors(soft, dart::Color::Fuchsia());
582
583 return soft;
584 }
585
586 SkeletonPtr createHybridBody()
587 {
588 SkeletonPtr hybrid = Skeleton::create("hybrid");
589
590 // Add a soft body
591 BodyNode* bn = addSoftBody<FreeJoint>(hybrid, "soft sphere", SOFT_ELLIPSOID);
592
593 // Add a rigid body attached by a WeldJoint
594 bn = hybrid->createJointAndBodyNodePair<WeldJoint>(bn).second;
595 bn->setName("rigid box");
596
597 double box_shape_height = default_shape_height;
598 std::shared_ptr<BoxShape> box
599 = std::make_shared<BoxShape>(box_shape_height * Eigen::Vector3d::Ones());
600 bn->createShapeNodeWith<VisualAspect, CollisionAspect, DynamicsAspect>(box);
601
602 Eigen::Isometry3d tf(Eigen::Isometry3d::Identity());
603 tf.translation() = Eigen::Vector3d(box_shape_height / 2.0, 0, 0);
604 bn->getParentJoint()->setTransformFromParentBodyNode(tf);
605
606 Inertia inertia;
607 inertia.setMass(default_shape_density * box->getVolume());
608 inertia.setMoment(box->computeInertia(inertia.getMass()));
609 bn->setInertia(inertia);
610
611 setAllColors(hybrid, dart::Color::Green());
612
613 return hybrid;
614 }
615
616 SkeletonPtr createGround()
617 {
618 SkeletonPtr ground = Skeleton::create("ground");
619
620 BodyNode* bn = ground->createJointAndBodyNodePair<WeldJoint>().second;
621
622 std::shared_ptr<BoxShape> shape = std::make_shared<BoxShape>(Eigen::Vector3d(
623 default_ground_width, default_ground_width, default_wall_thickness));
624 auto shapeNode
625 = bn->createShapeNodeWith<VisualAspect, CollisionAspect, DynamicsAspect>(
626 shape);
627 shapeNode->getVisualAspect()->setColor(Eigen::Vector3d(1.0, 1.0, 1.0));
628
629 return ground;
630 }
631
632 SkeletonPtr createWall()
633 {
634 SkeletonPtr wall = Skeleton::create("wall");
635
636 BodyNode* bn = wall->createJointAndBodyNodePair<WeldJoint>().second;
637
638 std::shared_ptr<BoxShape> shape = std::make_shared<BoxShape>(Eigen::Vector3d(
639 default_wall_thickness, default_ground_width, default_wall_height));
640 auto shapeNode
641 = bn->createShapeNodeWith<VisualAspect, CollisionAspect, DynamicsAspect>(
642 shape);
643 shapeNode->getVisualAspect()->setColor(Eigen::Vector3d(0.8, 0.8, 0.8));
644
645 Eigen::Isometry3d tf(Eigen::Isometry3d::Identity());
646 tf.translation() = Eigen::Vector3d(
647 (default_ground_width + default_wall_thickness) / 2.0,
648 0.0,
649 (default_wall_height - default_wall_thickness) / 2.0);
650 bn->getParentJoint()->setTransformFromParentBodyNode(tf);
651
652 shapeNode->getDynamicsAspect()->setRestitutionCoeff(0.2);
653
654 return wall;
655 }
656
657 int main(int argc, char* argv[])
658 {
659 WorldPtr world = std::make_shared<World>();
660 world->addSkeleton(createGround());
661 world->addSkeleton(createWall());
662
663 MyWindow window(
664 world,
665 createBall(),
666 createSoftBody(),
667 createHybridBody(),
668 createRigidChain(),
669 createRigidRing());
670
671 std::cout << "space bar: simulation on/off" << std::endl;
672 std::cout << "'1': toss a rigid ball" << std::endl;
673 std::cout << "'2': toss a soft body" << std::endl;
674 std::cout << "'3': toss a hybrid soft/rigid body" << std::endl;
675 std::cout << "'4': toss a rigid chain" << std::endl;
676 std::cout << "'5': toss a ring of rigid bodies" << std::endl;
677
678 std::cout << "\n'd': delete the oldest object" << std::endl;
679 std::cout << "'r': toggle randomness" << std::endl;
680
681 std::cout << "\nWarning: Let objects settle before tossing a new one, or the "
682 "simulation could explode."
683 << std::endl;
684 std::cout << " If the simulation freezes, you may need to force quit "
685 "the application.\n"
686 << std::endl;
687
688 glutInit(&argc, argv);
689 window.initWindow(640, 480, "Collisions");
690 glutMainLoop();
691 }