| 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 |
#include <gtest/gtest.h> |
| 35 |
#include "kinematics/FileInfoSkel.hpp" |
| 36 |
#include "kinematics/Skeleton.h" |
| 37 |
#include "kinematics/BodyNode.h" |
| 38 |
#include "kinematics/FileInfoDof.h" |
| 39 |
#include "kinematics/FileInfoC3D.h" |
| 40 |
#include "kinematics/TrfmRotateExpmap.h" |
| 41 |
#include "utils/Paths.h" |
| 42 |
#include "math/UtilsMath.h" |
| 43 |
|
| 44 |
using namespace std; |
| 45 |
|
| 46 |
/* ********************************************************************************************* */ |
| 47 |
TEST(KINEMATICS, VSK_LOADER) { |
| 48 |
using namespace Eigen; |
| 49 |
using namespace kinematics; |
| 50 |
|
| 51 |
FileInfoSkel<Skeleton> modelFile; |
| 52 |
modelFile.loadFile(DART_DATA_PATH"skel/Yuting.vsk"); |
| 53 |
Skeleton* skel = modelFile.getSkel(); |
| 54 |
|
| 55 |
EXPECT_TRUE(skel != NULL); |
| 56 |
|
| 57 |
EXPECT_EQ(skel->getNumDofs(), 66); |
| 58 |
EXPECT_EQ(skel->getNumNodes(), 30); |
| 59 |
EXPECT_EQ(skel->getNumMarkers(), 53); |
| 60 |
} |
| 61 |
|
| 62 |
/* ********************************************************************************************* */ |
| 63 |
TEST(KINEMATICS, C3D_LOADER) { |
| 64 |
using namespace Eigen; |
| 65 |
using namespace kinematics; |
| 66 |
|
| 67 |
FileInfoC3D c3dFile; |
| 68 |
bool result = c3dFile.loadFile(DART_DATA_PATH"c3d/squat.c3d"); |
| 69 |
ASSERT_TRUE(result); |
| 70 |
|
| 71 |
EXPECT_EQ(c3dFile.getNumMarkers(), 53); |
| 72 |
EXPECT_EQ(c3dFile.getNumFrames(), 2539); |
| 73 |
} |
| 74 |
|
| 75 |
/* ********************************************************************************************* */ |
| 76 |
TEST(KINEMATICS, TRANS_AND_DERIV) { |
| 77 |
using namespace Eigen; |
| 78 |
using namespace kinematics; |
| 79 |
|
| 80 |
FileInfoSkel<Skeleton> modelFile; |
| 81 |
bool loadModelResult = modelFile.loadFile(DART_DATA_PATH"skel/SehoonVSK3.vsk"); |
| 82 |
ASSERT_TRUE(loadModelResult); |
| 83 |
|
| 84 |
Skeleton* skel = modelFile.getSkel(); |
| 85 |
EXPECT_TRUE(skel != NULL); |
| 86 |
/* LOG(INFO) << "# Dofs = " << skel->getNumDofs(); */ |
| 87 |
/* LOG(INFO) << "# Nodes = " << skel->getNumNodes(); */ |
| 88 |
|
| 89 |
|
| 90 |
FileInfoDof dofFile(skel); |
| 91 |
bool loadDofResult = dofFile.loadFile(DART_DATA_PATH"dof/init_Tpose.dof"); |
| 92 |
ASSERT_TRUE(loadDofResult); |
| 93 |
/* LOG(INFO) << "# frames = " << dofFile.getNumFrames(); */ |
| 94 |
|
| 95 |
VectorXd pose = dofFile.getPoseAtFrame(0); |
| 96 |
skel->setPose(pose, true, true); |
| 97 |
|
| 98 |
const int nodeIndex = 1; |
| 99 |
BodyNode* node = skel->getNode(nodeIndex); |
| 100 |
EXPECT_TRUE(node != NULL); |
| 101 |
|
| 102 |
const double TOLERANCE = 0.000001; |
| 103 |
|
| 104 |
// Check the one global transform matrix |
| 105 |
const Matrix4d& W = skel->getNode(20)->getWorldTransform(); |
| 106 |
Matrix4d W_truth; |
| 107 |
W_truth << -0.110662, 0.991044, 0.074734, 0.642841 |
| 108 |
, -0.987564, -0.118099, 0.103775, 0.327496 |
| 109 |
, 0.111672, -0.0623207, 0.991789, -0.12855 |
| 110 |
, 0, 0, 0, 1; |
| 111 |
for (int i = 0; i < 4; i++) { |
| 112 |
for (int j = 0; j < 4; j++) { |
| 113 |
EXPECT_NEAR(W(i, j), W_truth(i, j), TOLERANCE); |
| 114 |
} |
| 115 |
} |
| 116 |
|
| 117 |
// Check the derivative of one global transform matrix |
| 118 |
// const Matrix4d& Wq = skel->getNode(10)->mWq.at(4); |
| 119 |
const Matrix4d& Wq = skel->getNode(10)->getDerivWorldTransform(4); |
| 120 |
Matrix4d Wq_truth; |
| 121 |
Wq_truth << 0.121382, 0.413015, 0.902378, 0.161838 |
| 122 |
,-0.0175714, 0.00698451, 0.0149899, 0.00571836 |
| 123 |
,-0.992336, 0.0556535, 0.107702, 0.175059 |
| 124 |
,0, 0, 0, 0; |
| 125 |
for (int i = 0; i < 4; i++) { |
| 126 |
for (int j = 0; j < 4; j++) { |
| 127 |
EXPECT_NEAR(Wq(i, j), Wq_truth(i, j), TOLERANCE); |
| 128 |
} |
| 129 |
} |
| 130 |
|
| 131 |
|
| 132 |
// Check Jacobians |
| 133 |
BodyNode *nodecheck = skel->getNode(23); |
| 134 |
|
| 135 |
// Linear Jacobian |
| 136 |
const MatrixXd Jv = nodecheck->getJacobianLinear(); |
| 137 |
MatrixXd Jv_truth = MatrixXd::Zero(Jv.rows(), Jv.cols()); |
| 138 |
Jv_truth<<1, 0, 0, 0.013694, -0.0194371, -0.422612, 0.0157281, -0.00961473, -0.421925, 0.0026645, -0.0048767, -0.179914, -0.0013539, -0.00132969, -0.00885535, |
| 139 |
0, 1, 0, 0.0321939, 0.00226492, -0.135736, 0.0330525, 0.00462613, -0.135448, 0.0181491, 0.00758086, -0.122187, 0.00532694, 0.0049221, -0.128917, |
| 140 |
0, 0, 1, 0.423948, 0.130694, 0.0166546, 0.42642, 0.118176, 0.0221309, 0.180296, 0.120584, 0.0105059, 0.0838259, 0.081304, 0.00719314; |
| 141 |
for (int i = 0; i < Jv.rows(); i++) { |
| 142 |
for (int j = 0; j < Jv.cols(); j++) { |
| 143 |
EXPECT_NEAR(Jv(i, j), Jv_truth(i, j), TOLERANCE); |
| 144 |
} |
| 145 |
} |
| 146 |
|
| 147 |
// Angular Jacobian |
| 148 |
const MatrixXd Jwbody = nodecheck->getWorldTransform().topLeftCorner<3,3>().transpose() |
| 149 |
* nodecheck->getJacobianAngular(); |
| 150 |
MatrixXd Jwbody_truth = MatrixXd::Zero(Jwbody.rows(), Jwbody.cols()); |
| 151 |
Jwbody_truth << 0, 0, 0, 0.0818662, 0.996527, 0.000504051, 0.110263, 0.993552, 0.0249018, 0.0772274, 0.995683, 0.0423836, 0.648386, 0.628838, 0.0338389, |
| 152 |
0, 0, 0, -0.995079, 0.082001, -0.0518665, -0.992054, 0.111479, -0.0554717, -0.996033, 0.078601, -0.0313861, -0.629201, 0.649145, -0.00994729, |
| 153 |
0, 0, 0, -0.0518265, 0.00391001, 0.998406, -0.0579139, -0.0187244, 0.998021, -0.0343359, -0.0398718, 0.998564, -0.0262454, -0.0235626, 0.999159; |
| 154 |
|
| 155 |
for (int i = 0; i < Jwbody.rows(); i++) { |
| 156 |
for (int j = 0; j < Jwbody.cols(); j++) { |
| 157 |
EXPECT_NEAR(Jwbody(i, j), Jwbody_truth(i, j), TOLERANCE); |
| 158 |
} |
| 159 |
} |
| 160 |
} |
| 161 |
|
| 162 |
/* ********************************************************************************************* */ |
| 163 |
int main(int argc, char* argv[]) { |
| 164 |
::testing::InitGoogleTest(&argc, argv); |
| 165 |
return RUN_ALL_TESTS(); |
| 166 |
} |
| 167 |
/* ********************************************************************************************* */ |