From c99583a1a6513f54621fb93142a6c6edfefecf71 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bartek=20=C5=81ukawski?= Date: Sat, 28 Dec 2024 18:20:53 +0100 Subject: [PATCH] Use a few more modern C++ goodies --- .../ScrewTheoryLib/ProductOfExponentials.cpp | 17 ++++------ .../ScrewTheoryLib/ProductOfExponentials.hpp | 2 +- .../ScrewTheoryLib/ScrewTheoryIkProblem.cpp | 33 +++++++------------ .../ScrewTheoryIkProblemBuilder.cpp | 16 ++++----- 4 files changed, 27 insertions(+), 41 deletions(-) diff --git a/libraries/ScrewTheoryLib/ProductOfExponentials.cpp b/libraries/ScrewTheoryLib/ProductOfExponentials.cpp index 8436b146c..0a1637c48 100644 --- a/libraries/ScrewTheoryLib/ProductOfExponentials.cpp +++ b/libraries/ScrewTheoryLib/ProductOfExponentials.cpp @@ -69,9 +69,9 @@ void PoeExpression::append(const PoeExpression & poe, const KDL::Frame & H_new_o void PoeExpression::changeBaseFrame(const KDL::Frame & H_new_old) { - for (int i = 0; i < exps.size(); i++) + for (auto & exp : exps) { - exps[i].changeBase(H_new_old); + exp.changeBase(H_new_old); } H_S_T = H_new_old * H_S_T; @@ -105,9 +105,9 @@ void PoeExpression::reverseSelf() { H_S_T = H_S_T.Inverse(); - for (int i = 0; i < exps.size(); i++) + for (auto & exp : exps) { - exps[i].changeBase(H_S_T); + exp.changeBase(H_S_T); } std::reverse(exps.begin(), exps.end()); @@ -122,9 +122,8 @@ PoeExpression PoeExpression::makeReverse() const poeReversed.exps.reserve(exps.size()); poeReversed.H_S_T = H_S_T.Inverse(); - for (int i = 0; i < exps.size(); i++) + for (const auto & exp : exps) { - const MatrixExponential & exp = exps[i]; poeReversed.append(exp, poeReversed.H_S_T); } @@ -140,10 +139,8 @@ KDL::Chain PoeExpression::toChain() const KDL::Chain chain; KDL::Frame H_S_prev; - for (int i = 0; i < exps.size(); i++) + for (const auto & exp : exps) { - const MatrixExponential & exp = exps[i]; - KDL::Joint::JointType jointType = motionToJointType(exp.getMotionType()); KDL::Joint joint(H_S_prev.Inverse() * exp.getOrigin(), exp.getAxis(), jointType); @@ -179,7 +176,7 @@ PoeExpression PoeExpression::fromChain(const KDL::Chain & chain) if (motionTypeId != UNKNOWN_OR_STATIC_JOINT) { - MatrixExponential::motion motionType = static_cast(motionTypeId); + auto motionType = static_cast(motionTypeId); MatrixExponential exp(motionType, H_S_prev.M * joint.JointAxis(), H_S_prev * joint.JointOrigin()); poe.append(exp); } diff --git a/libraries/ScrewTheoryLib/ProductOfExponentials.hpp b/libraries/ScrewTheoryLib/ProductOfExponentials.hpp index f436e913f..3e2164c0e 100644 --- a/libraries/ScrewTheoryLib/ProductOfExponentials.hpp +++ b/libraries/ScrewTheoryLib/ProductOfExponentials.hpp @@ -45,7 +45,7 @@ class PoeExpression * base frame of the input POE term. */ void append(const MatrixExponential & exp, const KDL::Frame & H_new_old = KDL::Frame::Identity()) - { exps.push_back(exp.cloneWithBase(H_new_old)); } + { exps.emplace_back(exp.cloneWithBase(H_new_old)); } /** * @brief Appends a POE to this formula diff --git a/libraries/ScrewTheoryLib/ScrewTheoryIkProblem.cpp b/libraries/ScrewTheoryLib/ScrewTheoryIkProblem.cpp index e033bacdd..f966dcb41 100644 --- a/libraries/ScrewTheoryLib/ScrewTheoryIkProblem.cpp +++ b/libraries/ScrewTheoryLib/ScrewTheoryIkProblem.cpp @@ -18,7 +18,7 @@ namespace struct solution_accumulator { - int operator()(int count, const ScrewTheoryIkSubproblem * subproblem) + inline int operator()(int count, const ScrewTheoryIkSubproblem * subproblem) { return count * subproblem->solutions(); } @@ -27,14 +27,7 @@ namespace inline int computeSolutions(const ScrewTheoryIkProblem::Steps & steps) { - if (!steps.empty()) - { - return std::accumulate(steps.begin(), steps.end(), 1, solutionAccumulator); - } - else - { - return 0; - } + return !steps.empty() ? std::accumulate(steps.begin(), steps.end(), 1, solutionAccumulator) : 0; } } @@ -51,9 +44,9 @@ ScrewTheoryIkProblem::ScrewTheoryIkProblem(const PoeExpression & _poe, const Ste ScrewTheoryIkProblem::~ScrewTheoryIkProblem() { - for (int i = 0; i < steps.size(); i++) + for (auto & step : steps) { - delete steps[i]; + delete step; } } @@ -61,10 +54,7 @@ ScrewTheoryIkProblem::~ScrewTheoryIkProblem() bool ScrewTheoryIkProblem::solve(const KDL::Frame & H_S_T, Solutions & solutions) { - if (!solutions.empty()) - { - solutions.clear(); - } + solutions.clear(); // Reserve space in memory to avoid additional allocations on runtime. solutions.reserve(soln); @@ -80,6 +70,8 @@ bool ScrewTheoryIkProblem::solve(const KDL::Frame & H_S_T, Solutions & solutions bool firstIteration = true; bool reachable = true; + ScrewTheoryIkSubproblem::Solutions partialSolutions; + for (int i = 0; i < steps.size(); i++) { if (!firstIteration) @@ -97,8 +89,6 @@ bool ScrewTheoryIkProblem::solve(const KDL::Frame & H_S_T, Solutions & solutions // Apply known frames to the first characteristic point for each subproblem. const KDL::Frame & H = transformPoint(solutions[j], poeTerms); - ScrewTheoryIkSubproblem::Solutions partialSolutions; - // Actually solve each subproblem, use current right-hand side of PoE to obtain // the right-hand side of said subproblem. reachable = reachable & steps[i]->solve(rhsFrames[j], H, partialSolutions); @@ -123,13 +113,12 @@ bool ScrewTheoryIkProblem::solve(const KDL::Frame & H_S_T, Solutions & solutions // For each local solution of this subproblem... for (int k = 0; k < partialSolutions.size(); k++) { - const ScrewTheoryIkSubproblem::JointIdsToSolutions & jointIdsToSolutions = partialSolutions[k]; + const auto & jointIdsToSolutions = partialSolutions[k]; // For each joint-id-to-value pair of this local solution... for (int l = 0; l < jointIdsToSolutions.size(); l++) { - const ScrewTheoryIkSubproblem::JointIdToSolution & jointIdToSolution = jointIdsToSolutions[l]; - auto [id, theta] = jointIdToSolution; + auto [id, theta] = jointIdsToSolutions[l]; // Preserve mapping of ids (associated to `poe`). poeTerms[id] = EXP_KNOWN; @@ -196,7 +185,7 @@ bool ScrewTheoryIkProblem::recalculateFrames(const Solutions & solutions, Frames { for (int j = 0; j < solutions.size(); j++) { - const MatrixExponential & exp = poe.exponentialAtJoint(i); + const auto & exp = poe.exponentialAtJoint(i); frames[j] = frames[j] * exp.asFrame(getTheta(solutions[j], i, reversed)); } @@ -232,7 +221,7 @@ KDL::Frame ScrewTheoryIkProblem::transformPoint(const KDL::JntArray & jointValue { if (poeTerms[i] == EXP_KNOWN) { - const MatrixExponential & exp = poe.exponentialAtJoint(i); + const auto & exp = poe.exponentialAtJoint(i); H = exp.asFrame(getTheta(jointValues, i, reversed)) * H; foundKnown = true; } diff --git a/libraries/ScrewTheoryLib/ScrewTheoryIkProblemBuilder.cpp b/libraries/ScrewTheoryLib/ScrewTheoryIkProblemBuilder.cpp index 4efefc3a8..abe47a8f3 100644 --- a/libraries/ScrewTheoryLib/ScrewTheoryIkProblemBuilder.cpp +++ b/libraries/ScrewTheoryLib/ScrewTheoryIkProblemBuilder.cpp @@ -130,9 +130,9 @@ namespace void clearSteps(ScrewTheoryIkProblem::Steps & steps) { - for (auto it = steps.begin(); it != steps.end(); ++it) + for (auto & step : steps) { - delete (*it); + delete step; } steps.clear(); @@ -231,9 +231,9 @@ std::vector ScrewTheoryIkProblemBuilder::searchPoints(const PoeExpr ScrewTheoryIkProblem * ScrewTheoryIkProblemBuilder::build() { // Reset state, mark all PoE terms as unknown. - for (auto it = poeTerms.begin(); it != poeTerms.end(); ++it) + for (auto & poeTerm : poeTerms) { - it->known = false; + poeTerm.known = false; } // Find solutions, if available. @@ -253,9 +253,9 @@ ScrewTheoryIkProblem * ScrewTheoryIkProblemBuilder::build() // No solution found, try with reversed PoE. poe.reverseSelf(); - for (auto it = poeTerms.begin(); it != poeTerms.end(); ++it) + for (auto & poeTerm : poeTerms) { - it->known = false; + poeTerm.known = false; } steps = searchSolutions(); @@ -347,9 +347,9 @@ ScrewTheoryIkProblem::Steps ScrewTheoryIkProblemBuilder::searchSolutions() void ScrewTheoryIkProblemBuilder::refreshSimplificationState() { // Reset simplification mark on all terms. - for (auto it = poeTerms.begin(); it != poeTerms.end(); ++it) + for (auto & poeTerm : poeTerms) { - it->simplified = false; + poeTerm.simplified = false; } // Leading known terms can be simplified (pre-multiply).