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 <gtest/gtest.h>
34
35 #include "dart/collision/ode/OdeCollisionDetector.hpp"
36 #include "dart/constraint/ConstraintSolver.hpp"
37 #include "dart/dynamics/SimpleFrame.hpp"
38 #include "dart/math/Geometry.hpp"
39 #include "dart/math/Helpers.hpp"
40 #include "dart/math/Random.hpp"
41
42 #include "TestHelpers.hpp"
43
44 using namespace dart;
45 using namespace dynamics;
46
47 //==============================================================================
createWorld()48 std::shared_ptr<World> createWorld()
49 {
50 auto world = simulation::World::create();
51 world->getConstraintSolver()->setCollisionDetector(
52 collision::OdeCollisionDetector::create());
53 return world;
54 }
55
56 //==============================================================================
createFloor()57 dynamics::SkeletonPtr createFloor()
58 {
59 auto floor = dynamics::Skeleton::create("floor");
60
61 // Give the floor a body
62 auto body
63 = floor->createJointAndBodyNodePair<dynamics::WeldJoint>(nullptr).second;
64
65 // Give the body a shape
66 double floorWidth = 10000.0;
67 double floorHeight = 0.01;
68 auto box = std::make_shared<dynamics::BoxShape>(
69 Eigen::Vector3d(floorWidth, floorWidth, floorHeight));
70 auto* shapeNode = body->createShapeNodeWith<
71 dynamics::VisualAspect,
72 dynamics::CollisionAspect,
73 dynamics::DynamicsAspect>(box);
74 shapeNode->getVisualAspect()->setColor(dart::Color::LightGray());
75 shapeNode->getDynamicsAspect()->setPrimarySlipCompliance(0);
76 shapeNode->getDynamicsAspect()->setSecondarySlipCompliance(0);
77
78 // Put the body into position
79 Eigen::Isometry3d tf = Eigen::Isometry3d::Identity();
80 tf.translation() = Eigen::Vector3d(0.0, 0.0, -floorHeight / 2.0);
81 body->getParentJoint()->setTransformFromParentBodyNode(tf);
82
83 return floor;
84 }
85
86 //==============================================================================
createCylinder(double _radius,double _height,const Eigen::Vector3d & _position=Eigen::Vector3d::Zero (),const Eigen::Vector3d & _orientation=Eigen::Vector3d::Zero ())87 SkeletonPtr createCylinder(
88 double _radius,
89 double _height,
90 const Eigen::Vector3d& _position = Eigen::Vector3d::Zero(),
91 const Eigen::Vector3d& _orientation = Eigen::Vector3d::Zero())
92 {
93 SkeletonPtr cylinder = createObject(_position, _orientation);
94
95 BodyNode* bn = cylinder->getBodyNode(0);
96 std::shared_ptr<Shape> cylinderShape(new CylinderShape(_radius, _height));
97 bn->createShapeNodeWith<VisualAspect, CollisionAspect, DynamicsAspect>(
98 cylinderShape);
99
100 return cylinder;
101 }
102
103 //==============================================================================
TEST(ForceDependentSlip,BoxSlipVelocity)104 TEST(ForceDependentSlip, BoxSlipVelocity)
105 {
106 using Eigen::Vector3d;
107 const double mass = 5.0;
108 const double slip = 0.02;
109 auto skeleton1 = createBox({0.3, 0.3, 0.3}, {0, -0.5, 0.15});
110 skeleton1->setName("Skeleton1");
111 auto skeleton2 = createBox({0.3, 0.3, 0.3}, {0, +0.5, 0.15});
112 skeleton2->setName("Skeleton2");
113
114 auto body1 = skeleton1->getRootBodyNode();
115 body1->setMass(mass);
116 auto body1Dynamics = body1->getShapeNode(0)->getDynamicsAspect();
117
118 EXPECT_DOUBLE_EQ(body1Dynamics->getFrictionCoeff(), 1.0);
119
120 body1Dynamics->setPrimarySlipCompliance(slip);
121 body1Dynamics->setFirstFrictionDirection(Vector3d::UnitX());
122 EXPECT_DOUBLE_EQ(body1Dynamics->getPrimarySlipCompliance(), slip);
123 EXPECT_EQ(body1Dynamics->getFirstFrictionDirection(), Vector3d::UnitX());
124
125 auto body2 = skeleton2->getRootBodyNode();
126 body2->setMass(mass);
127 auto body2Dynamics = body2->getShapeNode(0)->getDynamicsAspect();
128 body2Dynamics->setFirstFrictionDirection(Vector3d::UnitX());
129 EXPECT_EQ(body2Dynamics->getFirstFrictionDirection(), Vector3d::UnitX());
130 EXPECT_DOUBLE_EQ(body2Dynamics->getFrictionCoeff(), 1.0);
131
132 // Create a world and add the rigid bodies
133 auto world = createWorld();
134 world->addSkeleton(createFloor());
135 world->addSkeleton(skeleton1);
136 world->addSkeleton(skeleton2);
137
138 const auto numSteps = 2000;
139 const double extForce = 10.0;
140 for (auto i = 0u; i < numSteps; ++i)
141 {
142 body1->addExtForce({extForce, 0, 0});
143 body2->addExtForce({extForce, 0, 0});
144 world->step();
145
146 if (i > 1000)
147 {
148 // The velocity of body1 should stabilize at F_ext * slip = 0.2 m/s
149 EXPECT_NEAR(extForce * slip, body1->getLinearVelocity().x(), 2e-5);
150 EXPECT_NEAR(0.0, body1->getLinearVelocity().y(), 2e-5);
151
152 // The second box should remain at rest because of friction
153 EXPECT_NEAR(0.0, body2->getLinearVelocity().x(), 2e-5);
154 EXPECT_NEAR(0.0, body2->getLinearVelocity().y(), 2e-5);
155 }
156 }
157
158 const double slip2 = 0.03;
159 // Test slip compliance in the secondary friction direction
160 body1Dynamics->setPrimarySlipCompliance(0);
161 body1Dynamics->setSecondarySlipCompliance(slip2);
162
163 EXPECT_DOUBLE_EQ(body1Dynamics->getPrimarySlipCompliance(), 0.0);
164 EXPECT_DOUBLE_EQ(body1Dynamics->getSecondarySlipCompliance(), slip2);
165
166 // Step without external force so the body stop moving
167 for (auto i = 0u; i < 500; ++i)
168 {
169 world->step();
170 }
171 EXPECT_NEAR(0.0, body1->getLinearVelocity().x(), 2e-5);
172 EXPECT_NEAR(0.0, body1->getLinearVelocity().y(), 2e-5);
173
174 // The second box should remain at rest because of friction
175 EXPECT_NEAR(0.0, body2->getLinearVelocity().x(), 2e-5);
176 EXPECT_NEAR(0.0, body2->getLinearVelocity().y(), 2e-5);
177
178 // Apply force in the +y direction
179 for (auto i = 0u; i < numSteps; ++i)
180 {
181 body1->addExtForce({0, extForce, 0});
182 body2->addExtForce({0, extForce, 0});
183 world->step();
184
185 if (i > 1500)
186 {
187 // The velocity of body1 should stabilize at F_ext * slip2 = 0.3 m/s
188 EXPECT_NEAR(0.0, body1->getLinearVelocity().x(), 2e-5);
189 EXPECT_NEAR(extForce * slip2, body1->getLinearVelocity().y(), 2e-5);
190
191 // The second box should remain at rest because of friction
192 EXPECT_NEAR(0.0, body2->getLinearVelocity().x(), 2e-5);
193 EXPECT_NEAR(0.0, body2->getLinearVelocity().y(), 2e-5);
194 }
195 }
196 }
197
198 //==============================================================================
199 // Test two cylinders, one with its z axis pointing in the z axis of the world
200 // so it's purely slipping, and the other with its z axis pointing in the y axis
201 // of the world so it's rolling and slipping.
TEST(ForceDependentSlip,CylinderSlipVelocity)202 TEST(ForceDependentSlip, CylinderSlipVelocity)
203 {
204 using Eigen::Vector3d;
205 const double mass = 2.0;
206 const double radius = 0.5;
207 const double slip = 0.02;
208 auto skeleton1 = createCylinder(radius, 0.3, {0, -5, radius});
209 skeleton1->setName("Skeleton1");
210
211 auto skeleton2 = createCylinder(
212 radius, 0.8, {0, 5, radius}, {math::constantsd::half_pi(), 0, 0});
213 skeleton2->setName("Skeleton2");
214
215 auto body1 = skeleton1->getRootBodyNode();
216 body1->setMass(5.0);
217 auto body1Dynamics = body1->getShapeNode(0)->getDynamicsAspect();
218
219 EXPECT_DOUBLE_EQ(body1Dynamics->getFrictionCoeff(), 1.0);
220
221 body1Dynamics->setPrimarySlipCompliance(slip);
222 body1Dynamics->setFirstFrictionDirection(Vector3d::UnitX());
223 EXPECT_DOUBLE_EQ(body1Dynamics->getPrimarySlipCompliance(), slip);
224 EXPECT_EQ(body1Dynamics->getFirstFrictionDirection(), Vector3d::UnitX());
225
226 auto body2 = skeleton2->getRootBodyNode();
227 body2->setMass(mass);
228 auto body2Dynamics = body2->getShapeNode(0)->getDynamicsAspect();
229 EXPECT_DOUBLE_EQ(body2Dynamics->getFrictionCoeff(), 1.0);
230
231 // Set the friction direction to +z in the shape frame because it will always
232 // be orthogonal to the floor's normal.
233 body2Dynamics->setFirstFrictionDirection(Vector3d::UnitZ());
234 // Since we want to test rolling with slipping for body2, we want to set a
235 // non-zero slip parameter in the direction orthogonal to the axis of rotation
236 // of the cylinder. The axis of rotation is in the body's +z direction, so we
237 // make that the first friction direction and set the non-zero slip parameter
238 // in the secondary direction.
239 body2Dynamics->setSecondarySlipCompliance(slip);
240 EXPECT_DOUBLE_EQ(body2Dynamics->getSecondarySlipCompliance(), slip);
241 EXPECT_EQ(body2Dynamics->getFirstFrictionDirection(), Vector3d::UnitZ());
242
243 // Create a world and add the rigid bodies
244 auto world = createWorld();
245
246 world->addSkeleton(createFloor());
247 world->addSkeleton(skeleton1);
248 world->addSkeleton(skeleton2);
249
250 const double dt = 0.001;
251 const auto numSteps = 2000;
252 const double extForceX = 1.0;
253 const double extTorqueY = 2.0;
254
255 auto lastVel = body2->getLinearVelocity();
256 for (auto i = 0u; i < numSteps; ++i)
257 {
258 body1->addExtForce({extForceX, 0, 0});
259 body2->addExtTorque({0, extTorqueY, 0.0}, false);
260 world->step();
261
262 if (i > 1000)
263 {
264 // The velocity of body1 should stabilize at F_ext * slip
265 EXPECT_NEAR(extForceX * slip, body1->getLinearVelocity().x(), 1e-4);
266 EXPECT_NEAR(0.0, body1->getLinearVelocity().y(), 1e-4);
267
268 // body2 rolls with sliding. The difference between the linear velocity
269 // and the expected non-sliding velocity (angular velocity * radius) is
270 // equal to F_fr * slip, where F_fr is the friction force. We compute the
271 // friction force from the linear acceleration since it's the only linear
272 // force on the body.
273 auto linVel = body2->getLinearVelocity().x();
274 auto spinVel = body2->getAngularVelocity().y() * radius;
275 // There appears to be a bug in DART in obtaining the linear acceleration
276 // of the body using (BodyNode::getLinearAcceleration), so we compute it
277 // here via finite difference.
278 auto accel = (body2->getLinearVelocity() - lastVel) / dt;
279 EXPECT_NEAR(mass * accel.x() * slip, spinVel - linVel, 2e-4);
280 EXPECT_NEAR(0.0, body2->getLinearVelocity().y(), 1e-4);
281 }
282
283 lastVel = body2->getLinearVelocity();
284 }
285 }
286