Debian Codegraph @

unittests/integration/test_ConstraintSolver.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 <gtest/gtest.h>
34
35 #include "dart/constraint/ConstraintSolver.hpp"
36 #include "dart/constraint/ContactSurface.hpp"
37 #include "dart/simulation/World.hpp"
38
39 #include "TestHelpers.hpp"
40
41 using namespace dart;
42
43 //==============================================================================
44 std::shared_ptr<World> createWorld()
45 {
46 return simulation::World::create();
47 }
48
49 //==============================================================================
50 TEST(ConstraintSolver, DefaultConstactSurfaceHandler)
51 {
52 auto world = createWorld();
53 auto solver = world->getConstraintSolver();
54 ASSERT_NE(nullptr, solver->getLastContactSurfaceHandler());
55 }
56
57 //==============================================================================
58 TEST(ConstraintSolver, CustomConstactSurfaceHandler)
59 {
60 class CustomHandler : public constraint::ContactSurfaceHandler
61 {
62 public:
63 constraint::ContactSurfaceParams createParams(
64 const collision::Contact& contact,
65 const size_t numContactsOnCollisionObject) const override
66 {
67 auto params = ContactSurfaceHandler::createParams(
68 contact, numContactsOnCollisionObject);
69 params.mFirstFrictionalDirection = Eigen::Vector3d::UnitY();
70 params.mContactSurfaceMotionVelocity = Eigen::Vector3d::UnitY();
71 return params;
72 }
73 };
74
75 auto world = createWorld();
76
77 auto solver = world->getConstraintSolver();
78 auto defaultHandler = solver->getLastContactSurfaceHandler();
79 EXPECT_EQ(nullptr, defaultHandler->getParent());
80
81 auto customHandler = std::make_shared<CustomHandler>();
82 solver->addContactSurfaceHandler(customHandler);
83
84 ASSERT_NE(nullptr, solver->getLastContactSurfaceHandler());
85 EXPECT_EQ(nullptr, defaultHandler->getParent());
86 EXPECT_EQ(defaultHandler, customHandler->getParent());
87
88 // try to remove nonexisting handler
89 EXPECT_FALSE(
90 solver->removeContactSurfaceHandler(std::make_shared<CustomHandler>()));
91
92 EXPECT_TRUE(solver->removeContactSurfaceHandler(defaultHandler));
93 EXPECT_EQ(nullptr, customHandler->getParent());
94 EXPECT_EQ(customHandler, solver->getLastContactSurfaceHandler());
95
96 // removing last handler should not be done, but we test it anyways
97 // a printed error message is expected
98 EXPECT_TRUE(solver->removeContactSurfaceHandler(customHandler));
99 EXPECT_EQ(nullptr, customHandler->getParent());
100 EXPECT_EQ(nullptr, solver->getLastContactSurfaceHandler());
101
102 solver->addContactSurfaceHandler(defaultHandler);
103 ASSERT_NE(nullptr, solver->getLastContactSurfaceHandler());
104 EXPECT_EQ(defaultHandler, solver->getLastContactSurfaceHandler());
105
106 solver->addContactSurfaceHandler(customHandler);
107 ASSERT_NE(nullptr, solver->getLastContactSurfaceHandler());
108 EXPECT_EQ(customHandler, solver->getLastContactSurfaceHandler());
109 EXPECT_EQ(nullptr, defaultHandler->getParent());
110 EXPECT_EQ(defaultHandler, customHandler->getParent());
111
112 auto customHandler2 = std::make_shared<CustomHandler>();
113 auto customHandler3 = std::make_shared<CustomHandler>();
114 solver->addContactSurfaceHandler(customHandler2);
115 solver->addContactSurfaceHandler(customHandler3);
116 ASSERT_NE(nullptr, solver->getLastContactSurfaceHandler());
117 EXPECT_EQ(customHandler3, solver->getLastContactSurfaceHandler());
118 EXPECT_EQ(nullptr, defaultHandler->getParent());
119 EXPECT_EQ(defaultHandler, customHandler->getParent());
120 EXPECT_EQ(customHandler, customHandler2->getParent());
121 EXPECT_EQ(customHandler2, customHandler3->getParent());
122
123 EXPECT_TRUE(solver->removeContactSurfaceHandler(customHandler));
124 ASSERT_NE(nullptr, solver->getLastContactSurfaceHandler());
125 EXPECT_EQ(customHandler3, solver->getLastContactSurfaceHandler());
126 EXPECT_EQ(nullptr, defaultHandler->getParent());
127 EXPECT_EQ(defaultHandler, customHandler->getParent());
128 EXPECT_EQ(defaultHandler, customHandler2->getParent());
129 EXPECT_EQ(customHandler2, customHandler3->getParent());
130
131 EXPECT_TRUE(solver->removeContactSurfaceHandler(customHandler3));
132 ASSERT_NE(nullptr, solver->getLastContactSurfaceHandler());
133 EXPECT_EQ(customHandler2, solver->getLastContactSurfaceHandler());
134 EXPECT_EQ(nullptr, defaultHandler->getParent());
135 EXPECT_EQ(defaultHandler, customHandler->getParent());
136 EXPECT_EQ(defaultHandler, customHandler2->getParent());
137 EXPECT_EQ(customHandler2, customHandler3->getParent());
138
139 // after we break the chain at handler 2, default handler is no longer
140 // reachable
141 customHandler2->setParent(nullptr);
142 EXPECT_FALSE(solver->removeContactSurfaceHandler(defaultHandler));
143 }
144
145 //==============================================================================
146 TEST(ConstraintSolver, ConstactSurfaceHandlerIsCalled)
147 {
148 class ValueHandler : public constraint::ContactSurfaceHandler
149 {
150 public:
151 ValueHandler(int value) : mValue(value)
152 {
153 // Do nothing
154 }
155
156 constraint::ContactSurfaceParams createParams(
157 const collision::Contact& contact,
158 const size_t numContactsOnCollisionObject) const override
159 {
160 auto params = ContactSurfaceHandler::createParams(
161 contact, numContactsOnCollisionObject);
162 mCalled = true;
163 params.mPrimaryFrictionCoeff = mValue;
164
165 return params;
166 }
167
168 mutable bool mCalled{false};
169 int mValue{0};
170 };
171
172 auto world = createWorld();
173
174 auto solver = world->getConstraintSolver();
175 auto defaultHandler = solver->getLastContactSurfaceHandler();
176 EXPECT_EQ(nullptr, defaultHandler->getParent());
177
178 auto customHandler = std::make_shared<ValueHandler>(1);
179 solver->addContactSurfaceHandler(customHandler);
180 solver->removeContactSurfaceHandler(defaultHandler);
181
182 customHandler->mCalled = false;
183 auto params = solver->getLastContactSurfaceHandler()->createParams({}, 0);
184 EXPECT_TRUE(customHandler->mCalled);
185 EXPECT_EQ(1, params.mPrimaryFrictionCoeff);
186
187 auto customHandler2 = std::make_shared<ValueHandler>(2);
188 solver->addContactSurfaceHandler(customHandler2);
189
190 customHandler->mCalled = customHandler2->mCalled = false;
191 params = solver->getLastContactSurfaceHandler()->createParams({}, 0);
192 EXPECT_TRUE(customHandler->mCalled);
193 EXPECT_TRUE(customHandler2->mCalled);
194 EXPECT_EQ(2, params.mPrimaryFrictionCoeff);
195
196 // Try once more adding the same handler instance; this should be ignored.
197 // If it were added, the createParams() call could get into an infinite loop
198 // calling the last handler as its parent, so rather check for it.
199 solver->addContactSurfaceHandler(customHandler2);
200
201 customHandler->mCalled = customHandler2->mCalled = false;
202 params = solver->getLastContactSurfaceHandler()->createParams({}, 0);
203 EXPECT_TRUE(customHandler->mCalled);
204 EXPECT_TRUE(customHandler2->mCalled);
205 EXPECT_EQ(2, params.mPrimaryFrictionCoeff);
206 }
207
208 //==============================================================================
209 TEST(ConstraintSolver, ConstactSurfaceHandlerIgnoreParent)
210 {
211 class IgnoreParentHandler : public constraint::ContactSurfaceHandler
212 {
213 public:
214 IgnoreParentHandler(int value) : mValue(value)
215 {
216 // Do nothing
217 }
218
219 constraint::ContactSurfaceParams createParams(
220 const collision::Contact& /*contact*/,
221 const size_t /*numContactsOnCollisionObject*/) const override
222 {
223 auto params = constraint::ContactSurfaceParams{};
224 mCalled = true;
225 params.mPrimaryFrictionCoeff = mValue;
226
227 return params;
228 }
229
230 mutable bool mCalled{false};
231 int mValue{0};
232 };
233
234 auto world = createWorld();
235
236 auto solver = world->getConstraintSolver();
237 auto defaultHandler = solver->getLastContactSurfaceHandler();
238 EXPECT_EQ(nullptr, defaultHandler->getParent());
239
240 auto customHandler = std::make_shared<IgnoreParentHandler>(1);
241 solver->addContactSurfaceHandler(customHandler);
242 solver->removeContactSurfaceHandler(defaultHandler);
243
244 customHandler->mCalled = false;
245 auto params = solver->getLastContactSurfaceHandler()->createParams({}, 0);
246 EXPECT_TRUE(customHandler->mCalled);
247 EXPECT_EQ(1, params.mPrimaryFrictionCoeff);
248
249 auto customHandler2 = std::make_shared<IgnoreParentHandler>(2);
250 solver->addContactSurfaceHandler(customHandler2);
251
252 customHandler->mCalled = customHandler2->mCalled = false;
253 params = solver->getLastContactSurfaceHandler()->createParams({}, 0);
254 EXPECT_FALSE(customHandler->mCalled);
255 EXPECT_TRUE(customHandler2->mCalled);
256 EXPECT_EQ(2, params.mPrimaryFrictionCoeff);
257 }