Debian Codegraph @

unittests/integration/test_ForwardKinematics.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 <iostream>
34
35 #include <gtest/gtest.h>
36
37 #include "dart/utils/urdf/DartLoader.hpp"
38
39 #include "TestHelpers.hpp"
40
41 std::vector<std::size_t> twoLinkIndices;
42
43 //==============================================================================
44 TEST(FORWARD_KINEMATICS, YAW_ROLL)
45 {
46 // Checks forward kinematics for two DoF arm manipulators.
47 // NOTE: The following is the reference frame description of the world
48 // frame. The x-axis is into the page, z-axis is to the top of the
49 // page and the y-axis is to the left. At the zero angle, the links
50 // are parallel to the z-axis and face the +x-axis.
51
52 // Create the world
53 const double l1 = 1.5, l2 = 1.0;
54 SkeletonPtr robot = createTwoLinkRobot(
55 Vector3d(0.3, 0.3, l1), DOF_YAW, Vector3d(0.3, 0.3, l2), DOF_ROLL);
56
57 // Set the test cases with the joint values and the expected end-effector
58 // positions
59 const std::size_t numTests = 2;
60 double temp = sqrt(0.5 * l2 * l2);
61 Vector2d joints[numTests]
62 = {Vector2d(constantsd::pi() / 4.0, constantsd::pi() / 2.0),
63 Vector2d(-constantsd::pi() / 4.0, -constantsd::pi() / 4.0)};
64 Vector3d expectedPos[numTests]
65 = {Vector3d(temp, -temp, l1),
66 Vector3d(temp / sqrt(2.0), temp / sqrt(2.0), l1 + temp)};
67
68 // Check each case by setting the joint values and obtaining the end-effector
69 // position
70 for (std::size_t i = 0; i < numTests; i++)
71 {
72 robot->setPositions(Eigen::VectorXd(joints[i]));
73 BodyNode* bn = robot->getBodyNode("ee");
74 Vector3d actual = bn->getTransform().translation();
75 bool equality = equals(actual, expectedPos[i], 1e-3);
76 EXPECT_TRUE(equality);
77 if (!equality)
78 {
79 std::cout << "Joint values: " << joints[i].transpose() << std::endl;
80 std::cout << "Actual pos: " << actual.transpose() << std::endl;
81 std::cout << "Expected pos: " << expectedPos[i].transpose() << std::endl;
82 }
83 }
84 }
85
86 //==============================================================================
87 // TODO: Use link lengths in expectations explicitly
88 TEST(FORWARD_KINEMATICS, TWO_ROLLS)
89 {
90 // Checks forward kinematics for two DoF arm manipulators.
91 // NOTE: The following is the reference frame description of the world
92 // frame. The x-axis is into the page, z-axis is to the top of the
93 // page and the y-axis is to the left. At the zero angle, the links
94 // are parallel to the z-axis and face the +x-axis.
95
96 // Create the world
97 const double link1 = 1.5, link2 = 1.0;
98 SkeletonPtr robot = createTwoLinkRobot(
99 Vector3d(0.3, 0.3, link1), DOF_ROLL, Vector3d(0.3, 0.3, link2), DOF_ROLL);
100
101 // Set the test cases with the joint values and the expected end-effector
102 // positions
103 const std::size_t numTests = 2;
104 Vector2d joints[numTests]
105 = {Vector2d(0.0, constantsd::pi() / 2.0),
106 Vector2d(3 * constantsd::pi() / 4.0, -constantsd::pi() / 4.0)};
107 Vector3d expectedPos[numTests]
108 = {Vector3d(0.0, -1.0, 1.5), Vector3d(0.0, -2.06, -1.06)};
109
110 // Check each case by setting the joint values and obtaining the end-effector
111 // position
112 for (std::size_t i = 0; i < numTests; i++)
113 {
114 robot->setPositions(joints[i]);
115 Vector3d actual = robot->getBodyNode("ee")->getTransform().translation();
116 bool equality = equals(actual, expectedPos[i], 1e-3);
117 EXPECT_TRUE(equality);
118 if (!equality)
119 {
120 std::cout << "Joint values: " << joints[i].transpose() << std::endl;
121 std::cout << "Actual pos: " << actual.transpose() << std::endl;
122 std::cout << "Expected pos: " << expectedPos[i].transpose() << std::endl;
123 }
124 }
125 }
126
127 //==============================================================================
128 Eigen::MatrixXd finiteDifferenceJacobian(
129 const SkeletonPtr& skeleton,
130 const Eigen::VectorXd& q,
131 const std::vector<std::size_t>& active_indices,
132 JacobianNode* node)
133 {
134 Eigen::MatrixXd J(3, q.size());
135 for (int i = 0; i < q.size(); ++i)
136 {
137 const double dq = 1e-4;
138
139 Eigen::VectorXd q_up = q;
140 Eigen::VectorXd q_down = q;
141
142 q_up[i] += 0.5 * dq;
143 q_down[i] -= 0.5 * dq;
144
145 skeleton->setPositions(active_indices, q_up);
146 Eigen::Vector3d x_up = node->getTransform().translation();
147
148 skeleton->setPositions(active_indices, q_down);
149 Eigen::Vector3d x_down = node->getTransform().translation();
150
151 skeleton->setPositions(active_indices, q);
152 J.col(i)
153 = node->getWorldTransform().linear().transpose() * (x_up - x_down) / dq;
154 }
155
156 return J;
157 }
158
159 //==============================================================================
160 Eigen::MatrixXd standardJacobian(
161 const SkeletonPtr& skeleton,
162 const Eigen::VectorXd& q,
163 const std::vector<std::size_t>& active_indices,
164 JacobianNode* node)
165 {
166 skeleton->setPositions(active_indices, q);
167
168 Eigen::MatrixXd J = skeleton->getJacobian(node).bottomRows<3>();
169
170 Eigen::MatrixXd reduced_J(3, q.size());
171 for (int i = 0; i < q.size(); ++i)
172 reduced_J.col(i) = J.col(active_indices[i]);
173
174 return reduced_J;
175 }
176
177 //==============================================================================
178 TEST(FORWARD_KINEMATICS, JACOBIAN_PARTIAL_CHANGE)
179 {
180 // This is a regression test for issue #499
181 const double tolerance = 1e-8;
182
183 dart::utils::DartLoader loader;
184 SkeletonPtr skeleton1
185 = loader.parseSkeleton("dart://sample/urdf/KR5/KR5 sixx R650.urdf");
186
187 SkeletonPtr skeleton2 = skeleton1->cloneSkeleton();
188
189 std::vector<std::size_t> active_indices;
190 for (std::size_t i = 0; i < 3; ++i)
191 active_indices.push_back(i);
192
193 Eigen::VectorXd q = Eigen::VectorXd::Random(active_indices.size());
194
195 Eigen::MatrixXd fd_J = finiteDifferenceJacobian(
196 skeleton1,
197 q,
198 active_indices,
199 skeleton1->getBodyNode(skeleton1->getNumBodyNodes() - 1));
200
201 Eigen::MatrixXd J = standardJacobian(
202 skeleton2,
203 q,
204 active_indices,
205 skeleton2->getBodyNode(skeleton2->getNumBodyNodes() - 1));
206
207 EXPECT_TRUE((fd_J - J).norm() < tolerance);
208
209 q = Eigen::VectorXd::Random(active_indices.size());
210
211 fd_J = finiteDifferenceJacobian(
212 skeleton1,
213 q,
214 active_indices,
215 skeleton1->getBodyNode(skeleton1->getNumBodyNodes() - 1));
216
217 J = standardJacobian(
218 skeleton2,
219 q,
220 active_indices,
221 skeleton2->getBodyNode(skeleton2->getNumBodyNodes() - 1));
222
223 EXPECT_TRUE((fd_J - J).norm() < tolerance);
224 }
225
226 //==============================================================================
227 TEST(FORWARD_KINEMATICS, JACOBIAN_END_EFFECTOR_CHANGE)
228 {
229 // This is a regression test for pull request #683
230 const double tolerance = 1e-8;
231
232 dart::utils::DartLoader loader;
233 SkeletonPtr skeleton1
234 = loader.parseSkeleton("dart://sample/urdf/KR5/KR5 sixx R650.urdf");
235
236 BodyNode* last_bn1 = skeleton1->getBodyNode(skeleton1->getNumBodyNodes() - 1);
237 EndEffector* ee1 = last_bn1->createEndEffector();
238
239 SkeletonPtr skeleton2 = skeleton1->cloneSkeleton();
240 BodyNode* last_bn2 = skeleton2->getBodyNode(skeleton2->getNumBodyNodes() - 1);
241 EndEffector* ee2 = last_bn2->createEndEffector();
242
243 std::vector<std::size_t> active_indices;
244 for (std::size_t i = 0; i < 3; ++i)
245 active_indices.push_back(i);
246
247 Eigen::VectorXd q = Eigen::VectorXd::Random(active_indices.size());
248
249 Eigen::MatrixXd fd_J
250 = finiteDifferenceJacobian(skeleton1, q, active_indices, ee1);
251
252 Eigen::MatrixXd J = standardJacobian(skeleton2, q, active_indices, ee2);
253
254 EXPECT_TRUE((fd_J - J).norm() < tolerance);
255
256 q = Eigen::VectorXd::Random(active_indices.size());
257 fd_J = finiteDifferenceJacobian(skeleton1, q, active_indices, ee1);
258 J = standardJacobian(skeleton2, q, active_indices, ee2);
259
260 EXPECT_TRUE((fd_J - J).norm() < tolerance);
261 }