From ef36b7e2b8f7926a5223c173ca5d25ee23e477c2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bartek=20=C5=81ukawski?= Date: Sun, 29 Dec 2024 23:18:13 +0100 Subject: [PATCH] Add PG6 to IK problem builder --- .../ScrewTheoryIkProblemBuilder.cpp | 43 ++++++++++--------- 1 file changed, 22 insertions(+), 21 deletions(-) diff --git a/libraries/ScrewTheoryLib/ScrewTheoryIkProblemBuilder.cpp b/libraries/ScrewTheoryLib/ScrewTheoryIkProblemBuilder.cpp index 810da4eb0..ff63e5ff0 100644 --- a/libraries/ScrewTheoryLib/ScrewTheoryIkProblemBuilder.cpp +++ b/libraries/ScrewTheoryLib/ScrewTheoryIkProblemBuilder.cpp @@ -394,15 +394,14 @@ ScrewTheoryIkProblem::JointIdsToSubproblem ScrewTheoryIkProblemBuilder::trySolve // Find rightmost unknown and not simplified PoE term. auto lastUnknown = std::find_if(poeTerms.rbegin(), poeTerms.rend(), unknownNotSimplifiedTerm); int lastExpId = std::distance(poeTerms.begin(), lastUnknown.base()) - 1; - const MatrixExponential & lastExp = poe.exponentialAtJoint(lastExpId); + const auto & lastExp = poe.exponentialAtJoint(lastExpId); // Select the most adequate subproblem, if available. if (unknownsCount == 1) { if (depth == 0) { - if (lastExp.getMotionType() == MatrixExponential::ROTATION - && !liesOnAxis(lastExp, testPoints[0])) + if (lastExp.getMotionType() == MatrixExponential::ROTATION && !liesOnAxis(lastExp, testPoints[0])) { poeTerms[lastExpId].known = true; return {{lastExpId}, new PadenKahanOne(lastExp, testPoints[0])}; @@ -450,19 +449,30 @@ ScrewTheoryIkProblem::JointIdsToSubproblem ScrewTheoryIkProblemBuilder::trySolve } int nextToLastExpId = lastExpId - 1; - const MatrixExponential & nextToLastExp = poe.exponentialAtJoint(nextToLastExpId); + const auto & nextToLastExp = poe.exponentialAtJoint(nextToLastExpId); if (depth == 0) { - KDL::Vector r; - - if (lastExp.getMotionType() == MatrixExponential::ROTATION - && nextToLastExp.getMotionType() == MatrixExponential::ROTATION - && !parallelAxes(lastExp, nextToLastExp) - && intersectingAxes(lastExp, nextToLastExp, r)) + if (lastExp.getMotionType() == MatrixExponential::ROTATION && nextToLastExp.getMotionType() == MatrixExponential::ROTATION) { - poeTerms[lastExpId].known = poeTerms[nextToLastExpId].known = true; - return {{nextToLastExpId, lastExpId}, new PadenKahanTwo(nextToLastExp, lastExp, testPoints[0], r)}; + if (!parallelAxes(lastExp, nextToLastExp)) + { + poeTerms[lastExpId].known = poeTerms[nextToLastExpId].known = true; + + if (KDL::Vector r; intersectingAxes(lastExp, nextToLastExp, r)) + { + return {{nextToLastExpId, lastExpId}, new PadenKahanTwo(nextToLastExp, lastExp, testPoints[0], r)}; + } + else + { + return {{nextToLastExpId, lastExpId}, new PardosGotorSix(nextToLastExp, lastExp, testPoints[0])}; + } + } + else if (!colinearAxes(lastExp, nextToLastExp)) + { + poeTerms[lastExpId].known = poeTerms[nextToLastExpId].known = true; + return {{nextToLastExpId, lastExpId}, new PardosGotorFour(nextToLastExp, lastExp, testPoints[0])}; + } } if (lastExp.getMotionType() == MatrixExponential::TRANSLATION @@ -472,15 +482,6 @@ ScrewTheoryIkProblem::JointIdsToSubproblem ScrewTheoryIkProblemBuilder::trySolve poeTerms[lastExpId].known = poeTerms[nextToLastExpId].known = true; return {{nextToLastExpId, lastExpId}, new PardosGotorTwo(nextToLastExp, lastExp, testPoints[0])}; } - - if (lastExp.getMotionType() == MatrixExponential::ROTATION - && nextToLastExp.getMotionType() == MatrixExponential::ROTATION - && parallelAxes(lastExp, nextToLastExp) - && !colinearAxes(lastExp, nextToLastExp)) - { - poeTerms[lastExpId].known = poeTerms[nextToLastExpId].known = true; - return {{nextToLastExpId, lastExpId}, new PardosGotorFour(nextToLastExp, lastExp, testPoints[0])}; - } } }