Debian Codegraph @

unittests/regression/test_Issue1243.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/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 }