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 "dart/dynamics/IkFast.hpp"
34
35 #include "dart/dynamics/BodyNode.hpp"
36 #include "dart/dynamics/DegreeOfFreedom.hpp"
37 #include "dart/dynamics/RevoluteJoint.hpp"
38 #include "dart/external/ikfast/ikfast.h"
39
40 namespace dart {
41 namespace dynamics {
42
43 namespace {
44
45 //==============================================================================
convertTransform(const Eigen::Isometry3d & tf,std::array<IkReal,3> & eetrans,std::array<IkReal,9> & eerot)46 void convertTransform(
47 const Eigen::Isometry3d& tf,
48 std::array<IkReal, 3>& eetrans,
49 std::array<IkReal, 9>& eerot)
50 {
51 eerot[0 * 3 + 0] = tf.linear()(0, 0);
52 eerot[0 * 3 + 1] = tf.linear()(0, 1);
53 eerot[0 * 3 + 2] = tf.linear()(0, 2);
54 eerot[1 * 3 + 0] = tf.linear()(1, 0);
55 eerot[1 * 3 + 1] = tf.linear()(1, 1);
56 eerot[1 * 3 + 2] = tf.linear()(1, 2);
57 eerot[2 * 3 + 0] = tf.linear()(2, 0);
58 eerot[2 * 3 + 1] = tf.linear()(2, 1);
59 eerot[2 * 3 + 2] = tf.linear()(2, 2);
60
61 eetrans[0] = tf.translation()[0];
62 eetrans[1] = tf.translation()[1];
63 eetrans[2] = tf.translation()[2];
64 }
65
66 //==============================================================================
convertTransform(const std::array<IkReal,3> & eetrans,const std::array<IkReal,9> & eerot,Eigen::Isometry3d & tf)67 void convertTransform(
68 const std::array<IkReal, 3>& eetrans,
69 const std::array<IkReal, 9>& eerot,
70 Eigen::Isometry3d& tf)
71 {
72 tf.setIdentity();
73
74 tf(0, 0) = eerot[0 * 3 + 0];
75 tf(0, 1) = eerot[0 * 3 + 1];
76 tf(0, 2) = eerot[0 * 3 + 2];
77 tf(1, 0) = eerot[1 * 3 + 0];
78 tf(1, 1) = eerot[1 * 3 + 1];
79 tf(1, 2) = eerot[1 * 3 + 2];
80 tf(2, 0) = eerot[2 * 3 + 0];
81 tf(2, 1) = eerot[2 * 3 + 1];
82 tf(2, 2) = eerot[2 * 3 + 2];
83
84 tf(3, 0) = eetrans[0];
85 tf(3, 1) = eetrans[1];
86 tf(3, 2) = eetrans[2];
87 }
88
89 //==============================================================================
getFreeJointFlags(int numParams,int numFreeParams,const int * freeParams)90 std::vector<bool> getFreeJointFlags(
91 int numParams, int numFreeParams, const int* freeParams)
92 {
93 std::vector<bool> flags(numParams, false);
94
95 for (int i = 0; i < numFreeParams; ++i)
96 {
97 assert(freeParams[i] < numParams);
98 flags[freeParams[i]] = true;
99 }
100
101 return flags;
102 }
103
104 //==============================================================================
convertIkSolution(const IkFast * ikfast,int numJoints,int numFreeParameters,int * freeParameters,const ikfast::IkSolutionBase<IkReal> & ikfastSolution,InverseKinematics::Analytical::Solution & solution)105 void convertIkSolution(
106 const IkFast* ikfast,
107 int numJoints,
108 int numFreeParameters,
109 int* freeParameters,
110 const ikfast::IkSolutionBase<IkReal>& ikfastSolution,
111 InverseKinematics::Analytical::Solution& solution)
112 {
113 const auto ik = ikfast->getIK();
114 const auto dofIndices = ikfast->getDofs();
115 const auto skel = ik->getNode()->getSkeleton();
116
117 std::vector<IkReal> solutionValues(numJoints);
118 std::vector<IkReal> freeValues(numFreeParameters);
119
120 ikfastSolution.GetSolution(solutionValues, freeValues);
121
122 const auto freeJointFlags
123 = getFreeJointFlags(numJoints, numFreeParameters, freeParameters);
124
125 bool limitViolated = false;
126
127 auto index = 0u;
128 assert(solutionValues.size());
129 solution.mConfig.resize(dofIndices.size());
130 for (auto i = 0u; i < solutionValues.size(); ++i)
131 {
132 const auto isFreeJoint = freeJointFlags[i];
133 if (isFreeJoint)
134 continue;
135
136 const auto dofIndex = dofIndices[index];
137 const auto* dof = skel->getDof(dofIndex);
138 const auto* joint = dof->getJoint();
139
140 auto solutionValue = solutionValues[i];
141
142 const auto lb = dof->getPositionLowerLimit();
143 const auto ub = dof->getPositionUpperLimit();
144
145 if (joint->getType() == RevoluteJoint::getStaticType())
146 {
147 // TODO(JS): Apply this to any DegreeOfFreedom whose configuration space
148 // is SO(2).
149
150 const auto currentValue = dof->getPosition();
151 if (!wrapCyclicSolution(currentValue, lb, ub, solutionValue))
152 {
153 limitViolated = true;
154 break;
155 }
156 }
157 else
158 {
159 if (solutionValues[i] < lb)
160 {
161 limitViolated = true;
162 break;
163 }
164
165 if (solutionValues[i] > ub)
166 {
167 limitViolated = true;
168 break;
169 }
170 }
171
172 solution.mConfig[index] = solutionValue;
173
174 index++;
175 }
176
177 solution.mValidity = limitViolated
178 ? InverseKinematics::Analytical::LIMIT_VIOLATED
179 : InverseKinematics::Analytical::VALID;
180 }
181
182 //==============================================================================
convertIkSolutions(const IkFast * ikfast,int numJoints,int numFreeParameters,int * freeParameters,const ikfast::IkSolutionList<IkReal> & ikfastSolutions,std::vector<InverseKinematics::Analytical::Solution> & solutions)183 void convertIkSolutions(
184 const IkFast* ikfast,
185 int numJoints,
186 int numFreeParameters,
187 int* freeParameters,
188 const ikfast::IkSolutionList<IkReal>& ikfastSolutions,
189 std::vector<InverseKinematics::Analytical::Solution>& solutions)
190 {
191 const auto numSolutions = ikfastSolutions.GetNumSolutions();
192
193 solutions.resize(numSolutions);
194
195 for (auto i = 0u; i < numSolutions; ++i)
196 {
197 const auto& ikfastSolution = ikfastSolutions.GetSolution(i);
198 convertIkSolution(
199 ikfast,
200 numJoints,
201 numFreeParameters,
202 freeParameters,
203 ikfastSolution,
204 solutions[i]);
205 }
206 }
207
208 //==============================================================================
checkDofMapValidity(const InverseKinematics * ik,const std::vector<std::size_t> & dofMap,const std::string & dofMapName)209 bool checkDofMapValidity(
210 const InverseKinematics* ik,
211 const std::vector<std::size_t>& dofMap,
212 const std::string& dofMapName)
213 {
214 // dependentDofs are the dependent DOFs of the BodyNode that is associated
215 // with ik. This function returns true if all the indices in dofMap are found
216 // in dependentDofs. Returns false otherwise.
217 const auto& dependentDofs = ik->getNode()->getDependentDofs();
218
219 for (const auto& dof : dofMap)
220 {
221 bool found = false;
222 for (auto dependentDof : dependentDofs)
223 {
224 const auto dependentDofIndex = dependentDof->getIndexInSkeleton();
225 if (dof == dependentDofIndex)
226 {
227 found = true;
228 break;
229 }
230 }
231
232 if (!found)
233 {
234 dterr << "[IkFast::configure] Failed to configure. An element of the "
235 << "given " << dofMapName << " '" << dof
236 << "' is not a dependent dofs of Node '" << ik->getNode()->getName()
237 << "'.\n";
238 return false;
239 }
240 }
241
242 return true;
243 }
244
245 } // namespace
246
247 //==============================================================================
IkFast(InverseKinematics * ik,const std::vector<std::size_t> & dofMap,const std::vector<std::size_t> & freeDofMap,const std::string & methodName,const InverseKinematics::Analytical::Properties & properties)248 IkFast::IkFast(
249 InverseKinematics* ik,
250 const std::vector<std::size_t>& dofMap,
251 const std::vector<std::size_t>& freeDofMap,
252 const std::string& methodName,
253 const InverseKinematics::Analytical::Properties& properties)
254 : Analytical{ik, methodName, properties}, mConfigured{false}
255 {
256 setExtraDofUtilization(UNUSED);
257
258 mDofs = dofMap;
259 mFreeDofs = freeDofMap;
260 }
261
262 //==============================================================================
getDofs() const263 auto IkFast::getDofs() const -> const std::vector<std::size_t>&
264 {
265 if (!mConfigured)
266 {
267 configure();
268
269 if (!mConfigured)
270 {
271 dtwarn << "[IkFast::getDofs] This analytical IK was not able "
272 << "to configure properly, so it will not be able to compute "
273 << "solutions. Returning an empty list of dofs.\n";
274 assert(mDofs.empty());
275 }
276 }
277
278 return mDofs;
279 }
280
281 //==============================================================================
getFreeDofs() const282 const std::vector<std::size_t>& IkFast::getFreeDofs() const
283 {
284 return mFreeDofs;
285 }
286
287 //==============================================================================
isConfigured() const288 bool IkFast::isConfigured() const
289 {
290 return mConfigured;
291 }
292
293 //==============================================================================
getNumFreeParameters2() const294 std::size_t IkFast::getNumFreeParameters2() const
295 {
296 return static_cast<std::size_t>(getNumFreeParameters());
297 }
298
299 //==============================================================================
getNumJoints2() const300 std::size_t IkFast::getNumJoints2() const
301 {
302 return static_cast<std::size_t>(getNumJoints());
303 }
304
305 //==============================================================================
getIkType2() const306 IkFast::IkType IkFast::getIkType2() const
307 {
308 // Following conversion is referred from:
309 // https://github.com/rdiankov/openrave/blob/b1ebe135b4217823ebdf56d9af5fe89b29723603/include/openrave/openrave.h#L575-L623
310
311 const int type = getIkType();
312
313 if (type == 0)
314 return IkType::UNKNOWN;
315 else if (type == 0x67000001)
316 return IkType::TRANSFORM_6D;
317 else if (type == 0x34000002)
318 return IkType::ROTATION_3D;
319 else if (type == 0x34000003)
320 return IkType::TRANSLATION_3D;
321 else if (type == 0x34000004)
322 return IkType::DIRECTION_3D;
323 else if (type == 0x34000005)
324 return IkType::RAY_4D;
325 else if (type == 0x34000006)
326 return IkType::LOOKAT_3D;
327 else if (type == 0x34000007)
328 return IkType::TRANSLATION_DIRECTION_5D;
329 else if (type == 0x34000008)
330 return IkType::TRANSLATION_XY_2D;
331 else if (type == 0x34000009)
332 return IkType::TRANSLATION_XY_ORIENTATION_3D;
333 else if (type == 0x3400000a)
334 return IkType::TRANSLATION_LOCAL_GLOBAL_6D;
335 else if (type == 0x3400000b)
336 return IkType::TRANSLATION_X_AXIS_ANGLE_4D;
337 else if (type == 0x3400000c)
338 return IkType::TRANSLATION_Y_AXIS_ANGLE_4D;
339 else if (type == 0x3400000d)
340 return IkType::TRANSLATION_Z_AXIS_ANGLE_4D;
341 else if (type == 0x3400000e)
342 return IkType::TRANSLATION_X_AXIS_ANGLE_Z_NORM_4D;
343 else if (type == 0x3400000f)
344 return IkType::TRANSLATION_Y_AXIS_ANGLE_X_NORM_4D;
345 else if (type == 0x34000010)
346 return IkType::TRANSLATION_Z_AXIS_ANGLE_Y_NORM_4D;
347
348 return IkType::UNKNOWN;
349 }
350
351 //==============================================================================
getKinematicsHash2() const352 const std::string IkFast::getKinematicsHash2() const
353 {
354 return const_cast<IkFast*>(this)->getKinematicsHash();
355 }
356
357 //==============================================================================
getIkFastVersion2() const358 std::string IkFast::getIkFastVersion2() const
359 {
360 return const_cast<IkFast*>(this)->getIkFastVersion();
361 }
362
363 //==============================================================================
configure() const364 void IkFast::configure() const
365 {
366 const auto ikFastNumJoints = getNumJoints();
367 const auto ikFastNumFreeJoints = getNumFreeParameters();
368 const auto ikFastNumNonFreeJoints = ikFastNumJoints - ikFastNumFreeJoints;
369
370 if (static_cast<std::size_t>(ikFastNumNonFreeJoints) != mDofs.size())
371 {
372 dterr << "[IkFast::configure] Failed to configure. Received a joint map of "
373 << "size '" << mDofs.size() << "' but the actual dofs IkFast is '"
374 << ikFastNumNonFreeJoints << "'.\n";
375 return;
376 }
377
378 if (static_cast<std::size_t>(ikFastNumFreeJoints) != mFreeDofs.size())
379 {
380 dterr << "[IkFast::configure] Failed to configure. Received a free joint "
381 << "map of size '" << mDofs.size()
382 << "' but the actual dofs IkFast is '" << ikFastNumFreeJoints
383 << "'.\n";
384 return;
385 }
386
387 if (!checkDofMapValidity(mIK.get(), mDofs, "dof map"))
388 return;
389
390 if (!checkDofMapValidity(mIK.get(), mFreeDofs, "free dof map"))
391 return;
392
393 mFreeParams.resize(static_cast<std::size_t>(ikFastNumFreeJoints));
394
395 mConfigured = true;
396 }
397
398 //==============================================================================
computeSolutions(const Eigen::Isometry3d & desiredBodyTf)399 auto IkFast::computeSolutions(const Eigen::Isometry3d& desiredBodyTf)
400 -> const std::vector<InverseKinematics::Analytical::Solution>&
401 {
402 mSolutions.clear();
403
404 if (!mConfigured)
405 {
406 configure();
407
408 if (!mConfigured)
409 {
410 dtwarn << "[IkFast::computeSolutions] This analytical IK was not able "
411 << "to configure properly, so it will not be able to compute "
412 << "solutions. Returning an empty list of solutions.\n";
413 return mSolutions;
414 }
415 }
416
417 convertTransform(desiredBodyTf, mTargetTranspose, mTargetRotation);
418
419 const auto dofs = mIK->getNode()->getSkeleton()->getDofs();
420 const auto ikFastNumFreeParams = getNumFreeParameters();
421 const auto ikFastFreeParams = getFreeParameters();
422 for (auto i = 0; i < ikFastNumFreeParams; ++i)
423 mFreeParams[i] = dofs[ikFastFreeParams[i]]->getPosition();
424
425 ikfast::IkSolutionList<IkReal> solutions;
426 const auto success = computeIk(
427 mTargetTranspose.data(),
428 mTargetRotation.data(),
429 mFreeParams.data(),
430 solutions);
431
432 if (success)
433 {
434 convertIkSolutions(
435 this,
436 getNumJoints(),
437 getNumFreeParameters(),
438 getFreeParameters(),
439 solutions,
440 mSolutions);
441 }
442
443 return mSolutions;
444 }
445
446 //==============================================================================
computeFk(const Eigen::VectorXd & parameters)447 Eigen::Isometry3d IkFast::computeFk(const Eigen::VectorXd& parameters)
448 {
449 const std::size_t ikFastNumNonFreeJoints
450 = getNumJoints2() - getNumFreeParameters2();
451 if (static_cast<std::size_t>(parameters.size()) != ikFastNumNonFreeJoints)
452 {
453 dtwarn << "[IkFast::computeFk] The dimension of given joint positions "
454 << "doesn't agree with the number of joints of this IkFast solver. "
455 << "Returning identity.\n";
456 return Eigen::Isometry3d::Identity();
457 }
458
459 std::array<IkReal, 3> eetrans;
460 std::array<IkReal, 9> eerot;
461 computeFk(parameters.data(), eetrans.data(), eerot.data());
462
463 Eigen::Isometry3d tf;
464 convertTransform(eetrans, eerot, tf);
465 return tf;
466 }
467
468 //==============================================================================
wrapCyclicSolution(double currentValue,double lb,double ub,double & solutionValue)469 bool wrapCyclicSolution(
470 double currentValue, double lb, double ub, double& solutionValue)
471 {
472 if (lb > ub)
473 return false;
474
475 const auto pi2 = math::constantsd::two_pi();
476
477 if (currentValue < lb)
478 {
479 const auto diff_lb = lb - solutionValue;
480 const auto lb_ceil = solutionValue + std::ceil(diff_lb / pi2) * pi2;
481 assert(lb <= lb_ceil);
482 if (lb_ceil <= ub)
483 {
484 solutionValue = lb_ceil;
485 }
486 else
487 {
488 return false;
489 }
490 }
491 else if (ub < currentValue)
492 {
493 const auto diff_ub = ub - solutionValue;
494 const auto ub_floor = solutionValue + std::floor(diff_ub / pi2) * pi2;
495 assert(ub_floor <= ub);
496 if (lb <= ub_floor)
497 {
498 solutionValue = ub_floor;
499 }
500 else
501 {
502 return false;
503 }
504 }
505 else
506 {
507 const auto diff_curr = currentValue - solutionValue;
508 const auto curr_floor = solutionValue + std::floor(diff_curr / pi2) * pi2;
509 const auto curr_ceil = solutionValue + std::ceil(diff_curr / pi2) * pi2;
510
511 bool found = false;
512
513 if (lb <= curr_floor)
514 {
515 solutionValue = curr_floor;
516 found = true;
517 }
518
519 if (curr_ceil <= ub)
520 {
521 if (found)
522 {
523 if (std::abs(curr_floor - currentValue)
524 > std::abs(curr_ceil - currentValue))
525 {
526 solutionValue = curr_ceil;
527 found = true;
528 }
529 }
530 else
531 {
532 solutionValue = curr_ceil;
533 found = true;
534 }
535 }
536
537 if (!found)
538 {
539 return false;
540 }
541 }
542
543 return true;
544 }
545
546 } // namespace dynamics
547 } // namespace dart
548