| 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 |
} |