1 /*
2 * Copyright (c) 2011-2021, 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 <sstream>
34
35 #include <dart/dart.hpp>
36 #include <dart/utils/urdf/urdf.hpp>
37 #include <gtest/gtest.h>
38 #include "TestHelpers.hpp"
39
40 using namespace dart;
41
42 //==============================================================================
TEST(IkFast,WrapCyclicSolution)43 TEST(IkFast, WrapCyclicSolution)
44 {
45 const auto pi = math::constantsd::pi();
46
47 double sol;
48
49 // Invalid bounds (lb > ub)
50 EXPECT_FALSE(dynamics::wrapCyclicSolution(0, 10, -10, sol));
51
52 // Current value is in the lmits, but solution is lesser than lower limit.
53 // Expect valid solution that is the cloest to the current value.
54 sol = -3 * pi;
55 EXPECT_TRUE(dynamics::wrapCyclicSolution(-pi / 2, -pi, +pi, sol));
56 EXPECT_DOUBLE_EQ(sol, -pi);
57 sol = -3 * pi;
58 EXPECT_TRUE(
59 dynamics::wrapCyclicSolution(-pi / 2, -(3.0 / 4.0) * pi, +pi, sol));
60 EXPECT_DOUBLE_EQ(sol, +pi);
61 sol = -3 * pi;
62 EXPECT_FALSE(
63 dynamics::wrapCyclicSolution(-pi / 2, -0.9 * pi, +0.9 * pi, sol));
64
65 // Current value is in the lmits, but solution is greater than upper limit.
66 // Expect valid solution that is the cloest to the current value.
67 sol = -3 * pi;
68 EXPECT_TRUE(dynamics::wrapCyclicSolution(+pi / 2, -pi, +pi, sol));
69 EXPECT_DOUBLE_EQ(sol, +pi);
70 sol = -3 * pi;
71 EXPECT_TRUE(
72 dynamics::wrapCyclicSolution(+pi / 2, -pi, +(3.0 / 4.0) * pi, sol));
73 EXPECT_DOUBLE_EQ(sol, -pi);
74 sol = -3 * pi;
75 EXPECT_FALSE(
76 dynamics::wrapCyclicSolution(+pi / 2, -0.9 * pi, +0.9 * pi, sol));
77
78 // Both current value and solution are lesser than lower limit.
79 // Expect least valid solution.
80 sol = -9 * pi;
81 EXPECT_TRUE(dynamics::wrapCyclicSolution(-5 * pi, -4 * pi, +4 * pi, sol));
82 EXPECT_DOUBLE_EQ(sol, -3 * pi);
83 sol = -9 * pi;
84 EXPECT_FALSE(dynamics::wrapCyclicSolution(-5 * pi, -4 * pi, -3.1 * pi, sol));
85
86 // Both current value and solution are greater than upper limit.
87 // Expect greatest valid solution.
88 sol = +9 * pi;
89 EXPECT_TRUE(dynamics::wrapCyclicSolution(+5 * pi, -4 * pi, +4 * pi, sol));
90 EXPECT_DOUBLE_EQ(sol, +3 * pi);
91 sol = +9 * pi;
92 EXPECT_FALSE(dynamics::wrapCyclicSolution(+5 * pi, +3.1 * pi, +4 * pi, sol));
93 }
94
95 //==============================================================================
TEST(IkFast,FailedToLoadSharedLibrary)96 TEST(IkFast, FailedToLoadSharedLibrary)
97 {
98 auto skel = dynamics::Skeleton::create();
99 ASSERT_NE(skel, nullptr);
100
101 auto bodyNode
102 = skel->createJointAndBodyNodePair<dynamics::FreeJoint>().second;
103
104 auto ee = bodyNode->createEndEffector("ee");
105 ASSERT_NE(ee, nullptr);
106 auto ik = ee->createIK();
107 ASSERT_NE(ik, nullptr);
108 auto ikfast = ik->setGradientMethod<dynamics::SharedLibraryIkFast>(
109 "doesn't exist", std::vector<std::size_t>(), std::vector<std::size_t>());
110 EXPECT_EQ(ikfast.isConfigured(), false);
111 }
112
113 //==============================================================================
TEST(IkFast,LoadWamArmIk)114 TEST(IkFast, LoadWamArmIk)
115 {
116 utils::DartLoader urdfParser;
117 urdfParser.addPackageDirectory(
118 "herb_description", DART_DATA_PATH "/urdf/wam");
119 auto wam = urdfParser.parseSkeleton(DART_DATA_PATH "/urdf/wam/wam.urdf");
120 ASSERT_NE(wam, nullptr);
121
122 auto wam7 = wam->getBodyNode("/wam7");
123 ASSERT_NE(wam7, nullptr);
124 auto ee = wam7->createEndEffector("ee");
125 auto ik = ee->createIK();
126 auto targetFrame
127 = dynamics::SimpleFrame::createShared(dynamics::Frame::World());
128 targetFrame->setRotation(Eigen::Matrix3d::Identity());
129
130 ik->setTarget(targetFrame);
131 ik->setHierarchyLevel(1);
132 std::stringstream ss;
133 ss << DART_SHARED_LIB_PREFIX << "GeneratedWamIkFast";
134 #if (DART_OS_LINUX || DART_OS_FREEBSD || DART_OS_MACOS) && !NDEBUG
135 ss << "d";
136 #endif
137 ss << "." << DART_SHARED_LIB_EXTENSION;
138 std::string libName = ss.str();
139 std::vector<std::size_t> ikFastDofs{0, 1, 3, 4, 5, 6};
140 std::vector<std::size_t> ikFastFreeDofs{2};
141 ik->setGradientMethod<dynamics::SharedLibraryIkFast>(
142 libName, ikFastDofs, ikFastFreeDofs);
143 auto analytical = ik->getAnalytical();
144 ASSERT_NE(analytical, nullptr);
145 EXPECT_EQ(analytical->getDofs().size(), 6);
146
147 auto ikfast = dynamic_cast<dynamics::SharedLibraryIkFast*>(analytical);
148 ASSERT_NE(ikfast, nullptr);
149 EXPECT_EQ(ikfast->getNumJoints2(), 7);
150 EXPECT_EQ(ikfast->getNumFreeParameters2(), 1);
151 EXPECT_EQ(ikfast->getIkType2(), dynamics::IkFast::IkType::TRANSFORM_6D);
152 EXPECT_EQ(ikfast->getIkFastVersion2(), "71");
153
154 targetFrame->setTranslation(Eigen::Vector3d(0, 0, 0.5));
155 auto solutions = ikfast->getSolutions(targetFrame->getTransform());
156 EXPECT_TRUE(!solutions.empty());
157
158 const auto dofs = ikfast->getDofs();
159
160 for (const auto& solution : solutions)
161 {
162 ASSERT_EQ(solution.mConfig.size(), 6);
163
164 if (solution.mValidity != InverseKinematics::Analytical::VALID)
165 continue;
166
167 wam->setPositions(dofs, solution.mConfig);
168 Eigen::Isometry3d newTf = ee->getTransform();
169 EXPECT_TRUE(equals(targetFrame->getTransform(), newTf, 1e-2));
170 }
171 }
172