Debian Codegraph @

unittests/regression/test_Issue1193.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/dart.hpp"
36 #include "dart/utils/sdf/SdfParser.hpp"
37
38 #include "TestHelpers.hpp"
39
40 using namespace dart::math;
41 using namespace dart::collision;
42 using namespace dart::dynamics;
43 using namespace dart::simulation;
44 using namespace dart::utils;
45
46 //==============================================================================
47 TEST(Issue1193, AngularVelAdd)
48 {
49 WorldPtr world = World::create();
50 EXPECT_TRUE(world != nullptr);
51 world->setGravity(Eigen::Vector3d(0.0, -10.0, 0.0));
52 const double dt = 0.001;
53 world->setTimeStep(dt);
54
55 SkeletonPtr sphereSkel = createSphere(0.05, Vector3d(0.0, 1.0, 0.0));
56 BodyNode* sphere = sphereSkel->getBodyNode(0);
57 Joint* sphereJoint = sphere->getParentJoint();
58 sphereJoint->setVelocity(0, 10.0); // ang_x -> Affect lz and ly
59 sphereJoint->setVelocity(1, 10.0); // ang_y -> No effect
60 sphereJoint->setVelocity(2, 10.0); // ang_z -> Affect lx and ly
61 world->addSkeleton(sphereSkel);
62
63 Eigen::Vector3d linearVelBefore = sphere->getLinearVelocity();
64 EXPECT_EQ(Vector3d::Zero(), linearVelBefore);
65
66 int maxSteps = 500;
67 for (int i = 0; i < maxSteps; i++)
68 {
69 world->step();
70 }
71
72 Vector3d linearVelAfter = sphere->getLinearVelocity();
73 double lx = linearVelAfter[0];
74 double ly = linearVelAfter[1];
75 double lz = linearVelAfter[2];
76
77 EXPECT_NEAR(0.0, lx, 1e-8);
78 EXPECT_NEAR(maxSteps * world->getGravity().y() * dt, ly, 1e-8);
79 EXPECT_NEAR(0.0, lz, 1e-8);
80 }
81
82 const double tol = 1e-5;
83 const int g_iters = 100000;
84
85 Eigen::Vector3d computeWorldAngularMomentum(const SkeletonPtr skel)
86 {
87 Eigen::Vector3d angMomentum = Eigen::Vector3d::Zero();
88 for (auto* bn : skel->getBodyNodes())
89 {
90 angMomentum += dart::math::dAdInvT(
91 bn->getWorldTransform(),
92 bn->getSpatialInertia() * bn->getSpatialVelocity())
93 .head<3>();
94 }
95 return angMomentum;
96 }
97
98 //==============================================================================
99 TEST(Issue1193, SingleBody)
100 {
101 WorldPtr world = World::create();
102 const double dt = 0.001;
103 world->setTimeStep(dt);
104 world->setGravity(Vector3d::Zero());
105 SkeletonPtr skel = createBox({1, 1, 1});
106 world->addSkeleton(skel);
107 auto rootBn = skel->getRootBodyNode();
108
109 Eigen::Vector3d initPosition = rootBn->getWorldTransform().translation();
110 Vector6d vels = compose({0, 25, 0}, {0, 0, -100});
111
112 rootBn->getParentJoint()->setVelocities(vels);
113
114 rootBn->addExtTorque({0, 50, 0});
115 for (int i = 0; i < g_iters; ++i)
116 {
117 world->step();
118 }
119 Eigen::Vector3d positionDiff
120 = rootBn->getWorldTransform().translation() - initPosition;
121 EXPECT_NEAR(0.0, positionDiff.x(), tol);
122 EXPECT_NEAR(0.0, positionDiff.y(), tol);
123 EXPECT_NEAR(g_iters * dt * vels[5], positionDiff.z(), tol);
124 }
125
126 //==============================================================================
127 TEST(Issue1193, SingleBodyWithOffDiagonalMoi)
128 {
129 WorldPtr world = World::create();
130 const double dt = 0.001;
131 world->setTimeStep(dt);
132 world->setGravity(Vector3d::Zero());
133 SkeletonPtr skel = createBox({1, 4, 9});
134 world->addSkeleton(skel);
135 auto rootBn = skel->getRootBodyNode();
136 rootBn->setMomentOfInertia(1.1, 1.1, 0.7, 0.1, 0, 0);
137 Eigen::Vector3d initPosition = rootBn->getWorldTransform().translation();
138
139 // Setting this to compose({0, 25, 0}, {0, 0, -100}) causes the test to fail.
140 Vector6d vels = compose({0, 2.5, 0}, {0, 0, -10.0});
141
142 rootBn->getParentJoint()->setVelocities(vels);
143
144 rootBn->addExtTorque({0, 50, 0});
145 //
146 for (int i = 0; i < g_iters; ++i)
147 {
148 world->step();
149 }
150 Eigen::Vector3d positionDiff
151 = rootBn->getWorldTransform().translation() - initPosition;
152 EXPECT_NEAR(0.0, positionDiff.x(), tol);
153 EXPECT_NEAR(0.0, positionDiff.y(), tol);
154 EXPECT_NEAR(g_iters * dt * vels[5], positionDiff.z(), tol);
155 }
156
157 //==============================================================================
158 TEST(Issue1193, SingleBodyWithJointOffset)
159 {
160 WorldPtr world = World::create();
161 const double dt = 0.001;
162 world->setTimeStep(dt);
163 world->setGravity(Vector3d::Zero());
164 SkeletonPtr skel = createBox({1, 1, 1});
165 world->addSkeleton(skel);
166 auto rootBn = skel->getRootBodyNode();
167 rootBn->setMomentOfInertia(1.1, 1.1, 0.7, 0.1, 0, 0);
168 Eigen::Vector3d initPosition = rootBn->getWorldTransform().translation();
169
170 Vector6d vels = compose({0, 2.5, 0}, {0, 0, -10});
171
172 auto* freeJoint = dynamic_cast<FreeJoint*>(rootBn->getParentJoint());
173 freeJoint->setVelocities(vels);
174
175 Eigen::Isometry3d jointPoseInParent = Eigen::Isometry3d::Identity();
176 jointPoseInParent.translate(Eigen::Vector3d(0.0, 4.0, 0));
177 freeJoint->setTransformFromParentBodyNode(jointPoseInParent);
178
179 rootBn->addExtTorque({0, 50, 0});
180 for (int i = 0; i < g_iters; ++i)
181 {
182 world->step();
183 }
184
185 Eigen::Vector3d positionDiff
186 = rootBn->getWorldTransform().translation() - initPosition;
187 EXPECT_NEAR(0.0, positionDiff.x(), tol);
188 EXPECT_NEAR(4.0, positionDiff.y(), tol);
189 EXPECT_NEAR(g_iters * dt * vels[5], positionDiff.z(), tol);
190 }
191
192 //==============================================================================
193 TEST(Issue1193, SingleBodyWithCOMOffset)
194 {
195 WorldPtr world = World::create();
196 const double dt = 0.001;
197 world->setTimeStep(dt);
198 world->setGravity(Vector3d::Zero());
199 SkeletonPtr skel = createBox({1, 1, 1});
200 world->addSkeleton(skel);
201 auto rootBn = skel->getRootBodyNode();
202 rootBn->setMomentOfInertia(1.0 / 6, 1.0 / 6.0, 1.0 / 6.0, 0, 0, 0);
203 rootBn->setLocalCOM({1, 5, 8});
204 const Eigen::Vector3d extForce{10, 0, 0};
205 rootBn->addExtForce(extForce);
206 world->step();
207 auto comLinearVel = rootBn->getCOMLinearVelocity();
208 // TODO (azeey) Improve FreeJoint integration so that a higher tolerance is
209 // not necessary here.
210 EXPECT_NEAR(extForce.x() * dt, comLinearVel.x(), tol * 1e2);
211 }
212
213 //==============================================================================
214 TEST(Issue1193, WithFixedJoint)
215 {
216 WorldPtr world = World::create();
217 const double dt = 0.001;
218 world->setTimeStep(dt);
219 world->setGravity(Vector3d::Zero());
220 SkeletonPtr skel = createBox({1, 1, 1}, {0, 0, 2});
221 world->addSkeleton(skel);
222 auto rootBn = skel->getRootBodyNode();
223
224 Eigen::Isometry3d comRelPose;
225 comRelPose = Eigen::Translation3d(0, 0, -2);
226 auto comFrame = SimpleFrame::createShared(rootBn, "CombinedCOM", comRelPose);
227 Eigen::Vector3d initComPosition = comFrame->getWorldTransform().translation();
228
229 GenericJoint<R1Space>::Properties joint2Prop(std::string("joint2"));
230 BodyNode::Properties link2Prop(
231 BodyNode::AspectProperties(std::string("link2")));
232 link2Prop.mInertia.setMass(1.0);
233
234 auto pair = rootBn->createChildJointAndBodyNodePair<WeldJoint>(
235 WeldJoint::Properties(joint2Prop), link2Prop);
236 auto* joint = pair.first;
237
238 Eigen::Isometry3d jointPoseInParent = Eigen::Isometry3d::Identity();
239 jointPoseInParent.translate(Eigen::Vector3d(0.0, 0.0, -4));
240 joint->setTransformFromParentBodyNode(jointPoseInParent);
241
242 // TODO (azeey) Improve FreeJoint integration so we can test with larger
243 // forces. Currently, increasing these forces much more causes the test to
244 // fail.
245 rootBn->setExtTorque({0, 2500, 0});
246 rootBn->setExtForce({0, 0, -1000});
247
248 for (int i = 0; i < g_iters; ++i)
249 {
250 world->step();
251 }
252 Eigen::Vector3d positionDiff
253 = comFrame->getWorldTransform().translation() - initComPosition;
254 EXPECT_NEAR(0.0, positionDiff.x(), tol);
255 EXPECT_NEAR(0.0, positionDiff.y(), tol);
256 }
257
258 //==============================================================================
259 TEST(Issue1193, WithRevoluteJoint)
260 {
261 auto world = SdfParser::readWorld(
262 "dart://sample/sdf/test/issue1193_revolute_test.sdf");
263 ASSERT_TRUE(world != nullptr);
264 const double dt = 0.001;
265 world->setTimeStep(dt);
266
267 SkeletonPtr skel = world->getSkeleton(0);
268 auto rootBn = skel->getRootBodyNode();
269
270 Eigen::Isometry3d comRelPose;
271 comRelPose = Eigen::Translation3d(0, 0, -2);
272 auto comFrame = SimpleFrame::createShared(rootBn, "CombinedCOM", comRelPose);
273 Eigen::Vector3d initComPosition = comFrame->getWorldTransform().translation();
274
275 auto* joint = skel->getJoint("revJoint");
276 ASSERT_NE(nullptr, joint);
277
278 for (int i = 0; i < g_iters; ++i)
279 {
280 joint->setCommand(0, 0.1);
281 world->step();
282 }
283
284 Eigen::Vector3d positionDiff
285 = comFrame->getWorldTransform().translation() - initComPosition;
286 EXPECT_NEAR(0.0, positionDiff.x(), tol * 5e2);
287 EXPECT_NEAR(0.0, positionDiff.y(), tol * 5e2);
288 }
289
290 //==============================================================================
291 TEST(Issue1193, ConservationOfMomentumWithRevoluteJointWithOffset)
292 {
293 auto world = SdfParser::readWorld(
294 "dart://sample/sdf/test/issue1193_revolute_with_offset_test.sdf");
295 ASSERT_TRUE(world != nullptr);
296 const double dt = 0.0001;
297 world->setTimeStep(dt);
298 world->setGravity(Vector3d::Zero());
299
300 SkeletonPtr skel = world->getSkeleton(0);
301 auto link1 = skel->getBodyNode("link1");
302
303 auto* joint = skel->getJoint("revJoint");
304 ASSERT_NE(nullptr, joint);
305
306 link1->getParentJoint()->setVelocities(compose({0, 0.25, 0}, {0, 0, -0.1}));
307 world->step();
308 Eigen::Vector3d maxAngMomentumChange = Eigen::Vector3d::Zero();
309 Eigen::Vector3d h0 = computeWorldAngularMomentum(skel);
310 for (int i = 1; i < g_iters; ++i)
311 {
312 joint->setCommand(0, 0.1);
313 world->step();
314
315 Eigen::Vector3d hNext = computeWorldAngularMomentum(skel);
316 maxAngMomentumChange
317 = (hNext - h0).cwiseAbs().cwiseMax(maxAngMomentumChange);
318 }
319
320 EXPECT_NEAR(0.0, maxAngMomentumChange.norm(), tol * 10);
321 }