| 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/dart.hpp> |
| 35 |
#include <gtest/gtest.h> |
| 36 |
|
| 37 |
dart::dynamics::SkeletonPtr create_box( |
| 38 |
const Eigen::Vector3d& dims, |
| 39 |
double mass, |
| 40 |
const Eigen::Vector4d& color, |
| 41 |
const std::string& box_name) |
| 42 |
{ |
| 43 |
dart::dynamics::SkeletonPtr box_skel |
| 44 |
= dart::dynamics::Skeleton::create(box_name); |
| 45 |
|
| 46 |
// Give the box a body |
| 47 |
dart::dynamics::BodyNodePtr body; |
| 48 |
body |
| 49 |
= box_skel->createJointAndBodyNodePair<dart::dynamics::FreeJoint>(nullptr) |
| 50 |
.second; |
| 51 |
body->setName(box_name); |
| 52 |
|
| 53 |
// Give the body a shape |
| 54 |
auto box = std::make_shared<dart::dynamics::BoxShape>(dims); |
| 55 |
auto box_node = body->createShapeNodeWith< |
| 56 |
dart::dynamics::VisualAspect, |
| 57 |
dart::dynamics::CollisionAspect, |
| 58 |
dart::dynamics::DynamicsAspect>(box); |
| 59 |
box_node->getVisualAspect()->setColor(color); |
| 60 |
// Set up inertia |
| 61 |
dart::dynamics::Inertia inertia; |
| 62 |
inertia.setMass(mass); |
| 63 |
inertia.setMoment(box->computeInertia(mass)); |
| 64 |
body->setInertia(inertia); |
| 65 |
|
| 66 |
return box_skel; |
| 67 |
} |
| 68 |
|
| 69 |
dart::dynamics::SkeletonPtr create_floor() |
| 70 |
{ |
| 71 |
double floor_width = 10.; |
| 72 |
double floor_height = 0.1; |
| 73 |
|
| 74 |
dart::dynamics::SkeletonPtr floor_skel |
| 75 |
= dart::dynamics::Skeleton::create("floor"); |
| 76 |
|
| 77 |
// Give the floor a body |
| 78 |
dart::dynamics::BodyNodePtr body |
| 79 |
= floor_skel |
| 80 |
->createJointAndBodyNodePair<dart::dynamics::WeldJoint>(nullptr) |
| 81 |
.second; |
| 82 |
|
| 83 |
// Give the body a shape |
| 84 |
auto box = std::make_shared<dart::dynamics::BoxShape>( |
| 85 |
Eigen::Vector3d(floor_width, floor_width, floor_height)); |
| 86 |
auto box_node = body->createShapeNodeWith< |
| 87 |
dart::dynamics::VisualAspect, |
| 88 |
dart::dynamics::CollisionAspect, |
| 89 |
dart::dynamics::DynamicsAspect>(box); |
| 90 |
box_node->getVisualAspect()->setColor(dart::Color::Gray()); |
| 91 |
|
| 92 |
// Put the body into position |
| 93 |
Eigen::Isometry3d tf(Eigen::Isometry3d::Identity()); |
| 94 |
tf.translation()[2] -= floor_height / 2.0; |
| 95 |
body->getParentJoint()->setTransformFromParentBodyNode(tf); |
| 96 |
|
| 97 |
return floor_skel; |
| 98 |
} |
| 99 |
|
| 100 |
//============================================================================== |
| 101 |
TEST(Issue1243, State) |
| 102 |
{ |
| 103 |
auto world = std::make_shared<dart::simulation::World>(); |
| 104 |
|
| 105 |
auto box_skel = create_box({0.1, 0.1, 0.1}, 1., {1., 0., 0., 1.}, "box"); |
| 106 |
auto floor_skel = create_floor(); |
| 107 |
|
| 108 |
box_skel->setPosition(5, 0.2); |
| 109 |
|
| 110 |
world->addSkeleton(box_skel); |
| 111 |
world->addSkeleton(floor_skel); |
| 112 |
|
| 113 |
dart::dynamics::Skeleton::State bookmark_state; |
| 114 |
Eigen::Isometry3d bookmark_tf; |
| 115 |
for (size_t i = 0; i < 20; i++) |
| 116 |
{ |
| 117 |
if (i == 10) |
| 118 |
{ |
| 119 |
bookmark_state = box_skel->getState(); |
| 120 |
bookmark_tf = box_skel->getRootBodyNode()->getTransform(); |
| 121 |
} |
| 122 |
world->step(); |
| 123 |
} |
| 124 |
|
| 125 |
const Eigen::Isometry3d final_tf |
| 126 |
= box_skel->getRootBodyNode()->getTransform(); |
| 127 |
|
| 128 |
box_skel->setState(bookmark_state); |
| 129 |
const Eigen::Isometry3d rewind_tf |
| 130 |
= box_skel->getRootBodyNode()->getTransform(); |
| 131 |
|
| 132 |
EXPECT_FALSE(equals(bookmark_tf, final_tf)); |
| 133 |
EXPECT_TRUE(equals(bookmark_tf, rewind_tf)); |
| 134 |
} |