Debian Codegraph @

unittests/.deprecated/testKinematics.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 #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 /* ********************************************************************************************* */