Debian Codegraph @

unittests/regression/test_Issue1184.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 <TestHelpers.hpp>
34 #include <dart/collision/bullet/BulletCollisionDetector.hpp>
35 #include <dart/dynamics/BoxShape.hpp>
36 #include <dart/dynamics/FreeJoint.hpp>
37 #include <dart/dynamics/PlaneShape.hpp>
38 #include <dart/dynamics/Skeleton.hpp>
39 #include <dart/dynamics/SphereShape.hpp>
40 #include <dart/dynamics/WeldJoint.hpp>
41 #include <dart/simulation/World.hpp>
42 #include <gtest/gtest.h>
43
44 //==============================================================================
45 TEST(Issue1184, Accuracy)
46 {
47 struct ShapeInfo
48 {
49 dart::dynamics::ShapePtr shape;
50 double offset;
51 };
52
53 std::function<ShapeInfo()> makePlaneGround = []() {
54 return ShapeInfo{
55 std::make_shared<dart::dynamics::PlaneShape>(
56 Eigen::Vector3d::UnitZ(), 0.0),
57 0.0};
58 };
59
60 std::function<ShapeInfo()> makeBoxGround = []() {
61 const double thickness = 0.1;
62 return ShapeInfo{
63 std::make_shared<dart::dynamics::BoxShape>(
64 Eigen::Vector3d(100.0, 100.0, thickness)),
65 -thickness / 2.0};
66 };
67
68 std::function<dart::dynamics::ShapePtr(double)> makeBoxObject
69 = [](const double s) -> dart::dynamics::ShapePtr {
70 return std::make_shared<dart::dynamics::BoxShape>(
71 Eigen::Vector3d::Constant(2 * s));
72 };
73
74 std::function<dart::dynamics::ShapePtr(double)> makeSphereObject
75 = [](const double s) -> dart::dynamics::ShapePtr {
76 return std::make_shared<dart::dynamics::SphereShape>(s);
77 };
78
79 #ifndef NDEBUG
80 const auto groundInfoFunctions = {makePlaneGround};
81 const auto objectShapeFunctions = {makeSphereObject};
82 const auto halfsizes = {10.0};
83 const auto fallingModes = {true};
84 const double dropHeight = 0.1;
85 const double tolerance = 1e-3;
86 #else
87 const auto groundInfoFunctions = {makePlaneGround, makeBoxGround};
88 const auto objectShapeFunctions = {makeBoxObject, makeSphereObject};
89 const auto halfsizes = {0.25, 1.0, 5.0, 10.0, 20.0};
90 const auto fallingModes = {true, false};
91 const double dropHeight = 1.0;
92 const double tolerance = 1e-3;
93 #endif
94
95 for (const auto& groundInfoFunction : groundInfoFunctions)
96 {
97 for (const auto& objectShapeFunction : objectShapeFunctions)
98 {
99 for (const double halfsize : halfsizes)
100 {
101 for (const bool falling : fallingModes)
102 {
103 auto world = dart::simulation::World::create("test");
104 world->getConstraintSolver()->setCollisionDetector(
105 dart::collision::BulletCollisionDetector::create());
106
107 Eigen::Isometry3d tf_object = Eigen::Isometry3d::Identity();
108 const double initialHeight
109 = falling ? dropHeight + halfsize : halfsize;
110 tf_object.translate(initialHeight * Eigen::Vector3d::UnitZ());
111
112 auto object = dart::dynamics::Skeleton::create("ball");
113 object->createJointAndBodyNodePair<dart::dynamics::FreeJoint>()
114 .first->setTransform(tf_object);
115
116 const auto objectShape = objectShapeFunction(halfsize);
117 object->getBodyNode(0)
118 ->createShapeNodeWith<
119 dart::dynamics::VisualAspect,
120 dart::dynamics::CollisionAspect,
121 dart::dynamics::DynamicsAspect>(objectShape);
122
123 world->addSkeleton(object);
124
125 const ShapeInfo groundInfo = groundInfoFunction();
126 auto ground = dart::dynamics::Skeleton::create("ground");
127 ground->createJointAndBodyNodePair<dart::dynamics::WeldJoint>()
128 .second->createShapeNodeWith<
129 dart::dynamics::VisualAspect,
130 dart::dynamics::CollisionAspect,
131 dart::dynamics::DynamicsAspect>(groundInfo.shape);
132
133 Eigen::Isometry3d tf_ground = Eigen::Isometry3d::Identity();
134 tf_ground.translate(groundInfo.offset * Eigen::Vector3d::UnitZ());
135 ground->getJoint(0)->setTransformFromParentBodyNode(tf_ground);
136
137 world->addSkeleton(ground);
138
139 // time until the object will strike
140 const double t_strike
141 = falling ? sqrt(-2.0 * dropHeight / world->getGravity()[2])
142 : 0.0;
143
144 // give the object some time to settle
145 const double min_time = 0.5;
146 const double t_limit = 30.0 * t_strike + min_time;
147
148 double lowestHeight = std::numeric_limits<double>::infinity();
149 double time = 0.0;
150 while (time < t_limit)
151 {
152 world->step();
153 const double currentHeight
154 = object->getBodyNode(0)->getTransform().translation()[2];
155
156 if (currentHeight < lowestHeight)
157 lowestHeight = currentHeight;
158
159 time = world->getTime();
160 }
161
162 // The simulation should have run for at least two seconds
163 ASSERT_LE(min_time, time);
164
165 EXPECT_GE(halfsize + tolerance, lowestHeight)
166 << "object type: " << objectShape->getType()
167 << "\nground type: " << groundInfo.shape->getType()
168 << "\nfalling: " << falling << "\n";
169
170 const double finalHeight
171 = object->getBodyNode(0)->getTransform().translation()[2];
172
173 EXPECT_NEAR(halfsize, finalHeight, tolerance)
174 << "object type: " << objectShape->getType()
175 << "\nground type: " << groundInfo.shape->getType()
176 << "\nfalling: " << falling << "\n";
177 }
178 }
179 }
180 }
181 }