| 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 |
} |