1 /*
2 * Copyright (c) 2011-2021, 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 <iostream>
34
35 #include <Eigen/Dense>
36 #include <gtest/gtest.h>
37
38 #include "TestHelpers.hpp"
39
40 #include "dart/collision/dart/DARTCollisionDetector.hpp"
41 #include "dart/common/Console.hpp"
42 #include "dart/dynamics/BodyNode.hpp"
43 #include "dart/dynamics/Skeleton.hpp"
44 #include "dart/math/Geometry.hpp"
45 #include "dart/math/Helpers.hpp"
46 #include "dart/math/Random.hpp"
47 #include "dart/simulation/World.hpp"
48 #include "dart/utils/SkelParser.hpp"
49
50 //==============================================================================
51 class ConstraintTest : public ::testing::Test
52 {
53 public:
54 // Get Skel file list to test.
55 const std::vector<std::string>& getList();
56
57 //
58 void SingleContactTest(const std::string& _fileName);
59
60 protected:
61 // Sets up the test fixture.
62 void SetUp() override;
63
64 // Skel file list.
65 std::vector<std::string> list;
66 };
67
68 //==============================================================================
SetUp()69 void ConstraintTest::SetUp()
70 {
71 list.push_back("dart://sample/skel/test/chainwhipa.skel");
72 list.push_back("dart://sample/skel/test/single_pendulum.skel");
73 list.push_back("dart://sample/skel/test/single_pendulum_euler_joint.skel");
74 list.push_back("dart://sample/skel/test/single_pendulum_ball_joint.skel");
75 list.push_back("dart://sample/skel/test/double_pendulum.skel");
76 list.push_back("dart://sample/skel/test/double_pendulum_euler_joint.skel");
77 list.push_back("dart://sample/skel/test/double_pendulum_ball_joint.skel");
78 list.push_back("dart://sample/skel/test/serial_chain_revolute_joint.skel");
79 list.push_back("dart://sample/skel/test/serial_chain_eulerxyz_joint.skel");
80 list.push_back("dart://sample/skel/test/serial_chain_ball_joint.skel");
81 list.push_back("dart://sample/skel/test/serial_chain_ball_joint_20.skel");
82 list.push_back("dart://sample/skel/test/serial_chain_ball_joint_40.skel");
83 list.push_back("dart://sample/skel/test/simple_tree_structure.skel");
84 list.push_back(
85 "dart://sample/skel/test/simple_tree_structure_euler_joint.skel");
86 list.push_back(
87 "dart://sample/skel/test/simple_tree_structure_ball_joint.skel");
88 list.push_back("dart://sample/skel/test/tree_structure.skel");
89 list.push_back("dart://sample/skel/test/tree_structure_euler_joint.skel");
90 list.push_back("dart://sample/skel/test/tree_structure_ball_joint.skel");
91 list.push_back("dart://sample/skel/fullbody1.skel");
92 }
93
94 //==============================================================================
getList()95 const std::vector<std::string>& ConstraintTest::getList()
96 {
97 return list;
98 }
99
100 //==============================================================================
SingleContactTest(const std::string &)101 void ConstraintTest::SingleContactTest(const std::string& /*_fileName*/)
102 {
103 using namespace std;
104 using namespace Eigen;
105 using namespace dart::math;
106 using namespace dart::collision;
107 using namespace dart::constraint;
108 using namespace dart::dynamics;
109 using namespace dart::simulation;
110 using namespace dart::utils;
111
112 //----------------------------------------------------------------------------
113 // Settings
114 //----------------------------------------------------------------------------
115 // Number of random state tests for each skeletons
116 #ifndef NDEBUG // Debug mode
117 // std::size_t testCount = 1;
118 #else
119 // std::size_t testCount = 1;
120 #endif
121
122 WorldPtr world = World::create();
123 EXPECT_TRUE(world != nullptr);
124 world->setGravity(Vector3d(0.0, -10.00, 0.0));
125 world->setTimeStep(0.001);
126 world->getConstraintSolver()->setCollisionDetector(
127 DARTCollisionDetector::create());
128
129 SkeletonPtr sphereSkel = createSphere(0.05, Vector3d(0.0, 1.0, 0.0));
130 BodyNode* sphere = sphereSkel->getBodyNode(0);
131 Joint* sphereJoint = sphere->getParentJoint();
132 sphereJoint->setVelocity(3, Random::uniform(-2.0, 2.0)); // x-axis
133 sphereJoint->setVelocity(5, Random::uniform(-2.0, 2.0)); // z-axis
134 world->addSkeleton(sphereSkel);
135 EXPECT_EQ(sphereSkel->getGravity(), world->getGravity());
136 assert(sphere);
137
138 SkeletonPtr boxSkel
139 = createBox(Vector3d(1.0, 1.0, 1.0), Vector3d(0.0, 1.0, 0.0));
140 BodyNode* box = boxSkel->getBodyNode(0);
141 Joint* boxJoint = box->getParentJoint();
142 boxJoint->setVelocity(3, Random::uniform(-2.0, 2.0)); // x-axis
143 boxJoint->setVelocity(5, Random::uniform(-2.0, 2.0)); // z-axis
144 // world->addSkeleton(boxSkel);
145 // EXPECT_EQ(boxSkel->getGravity(), world->getGravity());
146 // assert(box);
147
148 SkeletonPtr groundSkel = createGround(
149 Vector3d(10000.0, 0.1, 10000.0), Vector3d(0.0, -0.05, 0.0));
150 groundSkel->setMobile(false);
151 // BodyNode* ground = groundSkel->getBodyNode(0);
152 world->addSkeleton(groundSkel);
153 EXPECT_EQ(groundSkel->getGravity(), world->getGravity());
154 // assert(ground);
155
156 EXPECT_EQ((int)world->getNumSkeletons(), 2);
157
158 // Lower and upper bound of configuration for system
159 // double lb = -1.5 * constantsd::pi();
160 // double ub = 1.5 * constantsd::pi();
161
162 int maxSteps = 500;
163 for (int i = 0; i < maxSteps; ++i)
164 {
165 // Vector3d pos1 = sphere->getWorldTransform().translation();
166 // Vector3d vel1 = sphere->getWorldLinearVelocity(pos1);
167
168 // std::cout << "pos1:" << pos1.transpose() << std::endl;
169 // std::cout << "vel1:" << vel1.transpose() << std::endl;
170
171 if (!world->checkCollision())
172 {
173 world->step();
174 continue;
175 }
176
177 // for (std::size_t j = 0; j < cd->getNumContacts(); ++j)
178 // {
179 // Contact contact = cd->getContact(j);
180 // Vector3d pos1 = sphere->getTransform().inverse() * contact.point;
181 // Vector3d vel1 = sphere->getWorldLinearVelocity(pos1);
182
183 // std::cout << "pos1:" << pos1.transpose() << std::endl;
184 // std::cout << "vel1:" << vel1.transpose() << std::endl;
185 // }
186
187 world->step();
188
189 const auto& result = world->getConstraintSolver()->getLastCollisionResult();
190
191 for (const auto& contact : result.getContacts())
192 {
193 Vector3d pos1 = sphere->getTransform().inverse() * contact.point;
194 Vector3d vel1 = sphere->getLinearVelocity(pos1);
195
196 // std::cout << "pos1:" << pos1.transpose() << std::endl;
197
198 // std::cout << "pos1[1]: " << pos1[1] << std::endl;
199
200 // std::cout << "pos1:" << pos1.transpose() << std::endl;
201 std::cout << "vel1:" << vel1.transpose() << ", pos1[1]: " << pos1[1]
202 << std::endl;
203
204 // EXPECT_NEAR(pos1[0], 0.0, 1e-9);
205 // EXPECT_NEAR(pos1[1], -0.05, 1e-2);
206 // EXPECT_NEAR(pos1[2], 0.0, 1e-9);
207
208 // EXPECT_NEAR(vel1[0], 0.0, 1e-9);
209 // EXPECT_NEAR(vel1[1], 0.0, 1e-9);
210 // EXPECT_NEAR(vel1[2], 0.0, 1e-9);
211
212 // if (!equals(vel1, Vector3d(0.0, 0.0, 0.0)))
213 // std::cout << "vel1:" << vel1.transpose() << std::endl;
214
215 // EXPECT_EQ(vel1, Vector3d::Zero());
216 }
217
218 // std::cout << std::endl;
219
220 break;
221 }
222 }
223
224 //==============================================================================
TEST_F(ConstraintTest,SingleContactTest)225 TEST_F(ConstraintTest, SingleContactTest)
226 {
227 // for (int i = 0; i < getList().size(); ++i)
228 // {
229 //#ifndef NDEBUG
230 // dtdbg << getList()[i] << std::endl;
231 //#endif
232 // SingleContactTest(getList()[i]);
233 // }
234
235 SingleContactTest(getList()[0]);
236 }
237