Skip to content

Commit

Permalink
Use a few more modern C++ goodies
Browse files Browse the repository at this point in the history
  • Loading branch information
@PeterBowman
PeterBowman committed Dec 28, 2024
1 parent 775d582 commit c99583a
Show file tree
Hide file tree
Showing 4 changed files with 27 additions and 41 deletions.
17 changes: 7 additions & 10 deletions libraries/ScrewTheoryLib/ProductOfExponentials.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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());
Expand All @@ -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);
}

Expand All @@ -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);

Expand Down Expand Up @@ -179,7 +176,7 @@ PoeExpression PoeExpression::fromChain(const KDL::Chain & chain)

if (motionTypeId != UNKNOWN_OR_STATIC_JOINT)
{
MatrixExponential::motion motionType = static_cast<MatrixExponential::motion>(motionTypeId);
auto motionType = static_cast<MatrixExponential::motion>(motionTypeId);
MatrixExponential exp(motionType, H_S_prev.M * joint.JointAxis(), H_S_prev * joint.JointOrigin());
poe.append(exp);
}
Expand Down
2 changes: 1 addition & 1 deletion libraries/ScrewTheoryLib/ProductOfExponentials.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
33 changes: 11 additions & 22 deletions libraries/ScrewTheoryLib/ScrewTheoryIkProblem.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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();
}
Expand All @@ -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;
}
}

Expand All @@ -51,20 +44,17 @@ 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;
}
}

// -----------------------------------------------------------------------------

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);
Expand All @@ -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)
Expand All @@ -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);
Expand All @@ -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;
Expand Down Expand Up @@ -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));
}

Expand Down Expand Up @@ -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;
}
Expand Down
16 changes: 8 additions & 8 deletions libraries/ScrewTheoryLib/ScrewTheoryIkProblemBuilder.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down Expand Up @@ -231,9 +231,9 @@ std::vector<KDL::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.
Expand All @@ -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();
Expand Down Expand Up @@ -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).
Expand Down

0 comments on commit c99583a

Please sign in to comment.