Debian Codegraph @

unittests/integration/test_JointForceTorque.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 // The tests in this file are inspired by the tests in:
34 // https://github.com/osrf/gazebo/blob/01b395a5fa92eb054c72f9a2027cdcfd35f287f4/test/integration/joint_force_torque.cc
35
36 #include <gtest/gtest.h>
37
38 #include "dart/dart.hpp"
39 #include "dart/utils/sdf/sdf.hpp"
40
41 #include "TestHelpers.hpp"
42
43 using namespace dart;
44 using namespace dart::math;
45 using namespace dart::dynamics;
46 using namespace dart::simulation;
47 using namespace dart::utils;
48
49 //==============================================================================
50 WorldPtr readWorld(const common::Uri& uri)
51 {
52 SdfParser::Options options;
53 options.mDefaultRootJointType = SdfParser::RootJointType::FIXED;
54 WorldPtr world = SdfParser::readWorld(uri, options);
55 world->eachSkeleton([](Skeleton* skel) {
56 skel->eachJoint([](Joint* joint) { joint->setLimitEnforcement(true); });
57 });
58 return world;
59 }
60
61 //==============================================================================
62 TEST(JointForceTorqueTest, Static)
63 {
64 // Load world
65 WorldPtr world = readWorld("dart://sample/sdf/test/force_torque_test.world");
66 ASSERT_NE(world, nullptr);
67 ASSERT_EQ(world->getNumSkeletons(), 1);
68
69 // Check if the world is correctly loaded
70 SkeletonPtr model_1 = world->getSkeleton(0);
71 ASSERT_NE(model_1, nullptr);
72 EXPECT_EQ(model_1->getName(), "model_1");
73 EXPECT_EQ(model_1->getNumBodyNodes(), 2);
74 EXPECT_EQ(model_1->getNumJoints(), 2);
75
76 world->setGravity(Eigen::Vector3d(0, 0, -50));
77
78 // Simulate 1 step
79 world->step();
80 double t = world->getTime();
81
82 // Get time step size
83 double dt = world->getTimeStep();
84 EXPECT_GT(dt, 0);
85
86 // Check that time moves forward
87 EXPECT_DOUBLE_EQ(t, dt);
88
89 // Get joint and get force torque
90 auto link_1 = model_1->getBodyNode("link_1");
91 ASSERT_NE(link_1, nullptr);
92 auto link_2 = model_1->getBodyNode("link_2");
93 ASSERT_NE(link_2, nullptr);
94 auto joint_01 = model_1->getJoint("joint_01");
95 ASSERT_NE(joint_01, nullptr);
96 auto joint_12 = model_1->getJoint("joint_12");
97 ASSERT_NE(joint_12, nullptr);
98
99 Eigen::Isometry3d tf = Eigen::Isometry3d::Identity();
100
101 // Run 10 steps
102 for (auto i = 0u; i < 10; ++i)
103 {
104 world->step();
105
106 //----------------------
107 // Test joint_01 wrench
108 //----------------------
109
110 // Reference adjustment for the difference of the joint frame conventions
111 // between Gazebo and DART
112 tf.setIdentity();
113 tf.translation() = joint_01->getTransformFromParentBodyNode().translation();
114 SimpleFramePtr parentFrame01
115 = std::make_shared<SimpleFrame>(Frame::World(), "parentFrame01", tf);
116 tf.setIdentity();
117 tf.translation() = joint_01->getTransformFromChildBodyNode().translation();
118 SimpleFramePtr childFrame01
119 = std::make_shared<SimpleFrame>(link_1, "childFrame01", tf);
120
121 const Eigen::Vector6d parentF01
122 = joint_01->getWrenchToParentBodyNode(parentFrame01.get());
123 EXPECT_DOUBLE_EQ(parentF01[0], 0); // torque
124 EXPECT_DOUBLE_EQ(parentF01[1], 0);
125 EXPECT_DOUBLE_EQ(parentF01[2], 0);
126 EXPECT_DOUBLE_EQ(parentF01[3], 0); // force
127 EXPECT_DOUBLE_EQ(parentF01[4], 0);
128 EXPECT_DOUBLE_EQ(parentF01[5], 1000);
129
130 const Eigen::Vector6d childF01
131 = joint_01->getWrenchToChildBodyNode(childFrame01.get());
132 EXPECT_EQ(childF01, -parentF01);
133
134 //----------------------
135 // Test joint_12 wrench
136 //----------------------
137
138 // Reference adjustment for the difference of the joint frame conventions
139 // between Gazebo and DART
140 tf.setIdentity();
141 tf.translation() = joint_12->getTransformFromParentBodyNode().translation();
142 SimpleFramePtr parentFrame12
143 = std::make_shared<SimpleFrame>(link_1, "parentFrame12", tf);
144 tf.setIdentity();
145 tf.translation() = joint_12->getTransformFromChildBodyNode().translation();
146 SimpleFramePtr childFrame12
147 = std::make_shared<SimpleFrame>(link_2, "childFrame12", tf);
148
149 const Eigen::Vector6d parentF12
150 = joint_12->getWrenchToParentBodyNode(parentFrame12.get());
151 EXPECT_DOUBLE_EQ(parentF12[0], 0); // torque
152 EXPECT_DOUBLE_EQ(parentF12[1], 0);
153 EXPECT_DOUBLE_EQ(parentF12[2], 0);
154 EXPECT_DOUBLE_EQ(parentF12[3], 0); // force
155 EXPECT_DOUBLE_EQ(parentF12[4], 0);
156 EXPECT_DOUBLE_EQ(parentF12[5], 500);
157
158 const Eigen::Vector6d childF12
159 = joint_12->getWrenchToChildBodyNode(childFrame01.get());
160 EXPECT_EQ(childF12, -parentF12);
161 }
162 }
163
164 //==============================================================================
165 TEST(JointForceTorqueTest, ForceTorqueAtJointLimits)
166 {
167 // Load world
168 WorldPtr world = readWorld("dart://sample/sdf/test/force_torque_test.world");
169 ASSERT_NE(world, nullptr);
170 ASSERT_EQ(world->getNumSkeletons(), 1);
171
172 // Check if the world is correctly loaded
173 SkeletonPtr model_1 = world->getSkeleton(0);
174 ASSERT_NE(model_1, nullptr);
175 EXPECT_EQ(model_1->getName(), "model_1");
176 EXPECT_EQ(model_1->getNumBodyNodes(), 2);
177 EXPECT_EQ(model_1->getNumJoints(), 2);
178
179 world->setGravity(Eigen::Vector3d(0, 0, -50));
180
181 // Simulate 1 step
182 world->step();
183 double t = world->getTime();
184
185 // Get time step size
186 double dt = world->getTimeStep();
187 EXPECT_GT(dt, 0);
188
189 // Check that time moves forward
190 EXPECT_DOUBLE_EQ(t, dt);
191
192 // Get joint and get force torque
193 auto link_1 = model_1->getBodyNode("link_1");
194 ASSERT_NE(link_1, nullptr);
195 auto link_2 = model_1->getBodyNode("link_2");
196 ASSERT_NE(link_2, nullptr);
197 auto joint_01 = model_1->getJoint("joint_01");
198 ASSERT_NE(joint_01, nullptr);
199 auto joint_12 = model_1->getJoint("joint_12");
200 ASSERT_NE(joint_12, nullptr);
201
202 // Change gravity so that the top link topples over, then remeasure
203 world->setGravity(Eigen::Vector3d(-30, 10, -50));
204
205 // Wait for dynamics to be stabilized
206 for (auto i = 0; i < 2000; ++i)
207 {
208 world->step();
209 }
210
211 Eigen::Isometry3d tf = Eigen::Isometry3d::Identity();
212
213 // Run 5 steps
214 for (auto i = 0u; i < 5; ++i)
215 {
216 world->step();
217
218 //----------------------
219 // Test joint_01 wrench
220 //----------------------
221
222 // Reference adjustment for the difference of the joint frame conventions
223 // between Gazebo and DART
224 tf.setIdentity();
225 tf.translation() = joint_01->getTransformFromParentBodyNode().translation();
226 SimpleFramePtr parentFrame01
227 = std::make_shared<SimpleFrame>(Frame::World(), "parentFrame01", tf);
228 tf.setIdentity();
229 tf.translation() = joint_01->getTransformFromChildBodyNode().translation();
230 SimpleFramePtr childFrame01
231 = std::make_shared<SimpleFrame>(link_1, "childFrame01", tf);
232
233 const Eigen::Vector6d parentF01
234 = joint_01->getWrenchToParentBodyNode(parentFrame01.get());
235 EXPECT_NEAR(parentF01[0], 750, 7.5); // torque
236 EXPECT_NEAR(parentF01[1], 0, 4.5);
237 EXPECT_NEAR(parentF01[2], -450, 0.1);
238 EXPECT_NEAR(parentF01[3], 600, 6); // force
239 EXPECT_NEAR(parentF01[4], -200, 10);
240 EXPECT_NEAR(parentF01[5], 1000, 2);
241
242 const Eigen::Vector6d childF01
243 = joint_01->getWrenchToChildBodyNode(childFrame01.get());
244 EXPECT_NEAR(childF01[0], -750, 7.5); // torque
245 EXPECT_NEAR(childF01[1], -450, 4.5);
246 EXPECT_NEAR(childF01[2], 0, 0.1);
247 EXPECT_NEAR(childF01[3], -600, 6); // force
248 EXPECT_NEAR(childF01[4], 1000, 10);
249 EXPECT_NEAR(childF01[5], 200, 2);
250
251 //----------------------
252 // Test joint_12 wrench
253 //----------------------
254
255 // Reference adjustment for the difference of the joint frame conventions
256 // between Gazebo and DART
257 tf.setIdentity();
258 tf.translation() = joint_12->getTransformFromParentBodyNode().translation();
259 SimpleFramePtr parentFrame12
260 = std::make_shared<SimpleFrame>(link_1, "parentFrame12", tf);
261 tf.setIdentity();
262 tf.translation() = joint_12->getTransformFromChildBodyNode().translation();
263 SimpleFramePtr childFrame12
264 = std::make_shared<SimpleFrame>(link_2, "childFrame12", tf);
265
266 const Eigen::Vector6d parentF12
267 = joint_12->getWrenchToParentBodyNode(parentFrame12.get());
268 EXPECT_NEAR(parentF12[0], 250, 2.5); // torque
269 EXPECT_NEAR(parentF12[1], 150, 1.5);
270 EXPECT_NEAR(parentF12[2], 0, 0.1);
271 EXPECT_NEAR(parentF12[3], 300, 3); // force
272 EXPECT_NEAR(parentF12[4], -500, 5);
273 EXPECT_NEAR(parentF12[5], -100, 1);
274
275 const Eigen::Vector6d childF12
276 = joint_12->getWrenchToChildBodyNode(childFrame12.get());
277 EXPECT_TRUE(childF12.isApprox(-parentF12));
278 }
279 }
280
281 //==============================================================================
282 TEST(JointForceTorqueTest, ForceTorqeAtJointLimitsWithExternalForces)
283 {
284 // Load world
285 WorldPtr world = readWorld("dart://sample/sdf/test/force_torque_test2.world");
286 ASSERT_NE(world, nullptr);
287 ASSERT_EQ(world->getNumSkeletons(), 1);
288
289 // Check if the world is correctly loaded
290 SkeletonPtr model_1 = world->getSkeleton(0);
291 ASSERT_NE(model_1, nullptr);
292 ASSERT_EQ(model_1->getName(), "boxes");
293 ASSERT_EQ(model_1->getNumBodyNodes(), 3);
294 ASSERT_EQ(model_1->getNumJoints(), 3);
295 ASSERT_EQ(model_1->getNumDofs(), 2);
296 // The first joint is fixed joint
297 EXPECT_EQ(
298 model_1->getRootJoint()->getType(),
299 dart::dynamics::WeldJoint::getStaticType());
300
301 world->setGravity(Eigen::Vector3d(0, 0, -50));
302
303 // Simulate 1 step
304 world->step();
305 double t = world->getTime();
306
307 // Get time step size
308 double dt = world->getTimeStep();
309 EXPECT_GT(dt, 0);
310
311 // Check that time moves forward
312 EXPECT_DOUBLE_EQ(t, dt);
313
314 // Get joint and get force torque
315 auto link_1 = model_1->getBodyNode("link1");
316 ASSERT_NE(link_1, nullptr);
317 auto link_2 = model_1->getBodyNode("link2");
318 ASSERT_NE(link_2, nullptr);
319 auto link_3 = model_1->getBodyNode("link3");
320 ASSERT_NE(link_3, nullptr);
321 auto joint_12 = model_1->getJoint("joint1");
322 ASSERT_NE(joint_12, nullptr);
323 auto joint_23 = model_1->getJoint("joint2");
324 ASSERT_NE(joint_23, nullptr);
325
326 Eigen::Isometry3d tf = Eigen::Isometry3d::Identity();
327
328 // Run 4500 steps
329 const double kp1 = 5e+4;
330 const double kp2 = 1e+4;
331 const double target1 = 0;
332 const double target2 = -0.25 * math::constantsd::pi();
333 const auto steps = 4500u;
334 for (auto i = 0u; i < steps; ++i)
335 {
336 // PD control
337 const double j1State = joint_12->getPosition(0);
338 const double j2State = joint_23->getPosition(0);
339 const double p1Error = target1 - j1State;
340 const double p2Error = target2 - j2State;
341 const double effort1 = kp1 * p1Error;
342 const double effort2 = kp2 * p2Error;
343 joint_12->setForce(0, effort1);
344 joint_23->setForce(0, effort2);
345
346 world->step();
347 }
348
349 EXPECT_NEAR(joint_12->getPosition(0), target1, 0.1);
350 EXPECT_NEAR(joint_23->getPosition(0), target2, 0.1);
351
352 const double tol = 2;
353
354 //----------------------
355 // Test joint_12 wrench
356 //----------------------
357
358 // Reference adjustment for different joint frame conventions between Gazebo
359 // and DART
360 tf.setIdentity();
361 tf.translation() = joint_12->getTransformFromParentBodyNode().translation();
362 SimpleFramePtr parentFrame01
363 = std::make_shared<SimpleFrame>(link_1, "parentFrame01", tf);
364 tf.setIdentity();
365 tf.translation() = joint_12->getTransformFromChildBodyNode().translation();
366 SimpleFramePtr childFrame01
367 = std::make_shared<SimpleFrame>(link_2, "childFrame01", tf);
368
369 const Eigen::Vector6d parentF01
370 = joint_12->getWrenchToParentBodyNode(parentFrame01.get());
371 EXPECT_NEAR(parentF01[0], 25, tol); // torque
372 EXPECT_NEAR(parentF01[1], -175, tol);
373 EXPECT_NEAR(parentF01[2], 0, tol);
374 EXPECT_NEAR(parentF01[3], 0, tol); // force
375 EXPECT_NEAR(parentF01[4], 0, tol);
376 EXPECT_NEAR(parentF01[5], 300, tol);
377
378 const Eigen::Vector6d childF01
379 = joint_12->getWrenchToChildBodyNode(childFrame01.get());
380 EXPECT_TRUE(childF01.isApprox(-parentF01, tol));
381
382 //----------------------
383 // Test joint_23 wrench
384 //----------------------
385
386 // Reference adjustment for different joint frame conventions between Gazebo
387 // and DART
388 tf.setIdentity();
389 tf.translation() = joint_23->getTransformFromParentBodyNode().translation();
390 SimpleFramePtr parentFrame12
391 = std::make_shared<SimpleFrame>(link_2, "parentFrame12", tf);
392 tf.setIdentity();
393 tf.translation() = joint_23->getTransformFromChildBodyNode().translation();
394 SimpleFramePtr childFrame12
395 = std::make_shared<SimpleFrame>(link_3, "childFrame12", tf);
396
397 const Eigen::Vector6d parentF12
398 = joint_23->getWrenchToParentBodyNode(parentFrame12.get());
399 EXPECT_NEAR(parentF12[0], 25, tol); // torque
400 EXPECT_NEAR(parentF12[1], 0, tol);
401 EXPECT_NEAR(parentF12[2], 0, tol);
402 EXPECT_NEAR(parentF12[3], 0, tol); // force
403 EXPECT_NEAR(parentF12[4], 0, tol);
404 EXPECT_NEAR(parentF12[5], 50, tol);
405
406 const Eigen::Vector6d childF12
407 = joint_23->getWrenchToChildBodyNode(childFrame12.get());
408 EXPECT_NEAR(childF12[0], -17.678, tol); // torque
409 EXPECT_NEAR(childF12[1], 0, tol);
410 EXPECT_NEAR(childF12[2], 17.678, tol);
411 EXPECT_NEAR(childF12[3], -35.355, tol); // force
412 EXPECT_NEAR(childF12[4], 0, tol);
413 EXPECT_NEAR(childF12[5], -35.355, tol);
414 }