diff --git a/libraries/ScrewTheoryLib/PadenKahanSubproblems.cpp b/libraries/ScrewTheoryLib/PadenKahanSubproblems.cpp
index a3d076d74..d4a381563 100644
--- a/libraries/ScrewTheoryLib/PadenKahanSubproblems.cpp
+++ b/libraries/ScrewTheoryLib/PadenKahanSubproblems.cpp
@@ -10,9 +10,8 @@ using namespace roboticslab;
 
 // -----------------------------------------------------------------------------
 
-PadenKahanOne::PadenKahanOne(int _id, const MatrixExponential & _exp, const KDL::Vector & _p)
-    : id(_id),
-      exp(_exp),
+PadenKahanOne::PadenKahanOne(const MatrixExponential & _exp, const KDL::Vector & _p)
+    : exp(_exp),
       p(_p),
       axisPow(vectorPow2(exp.getAxis()))
 {}
@@ -21,9 +20,6 @@ PadenKahanOne::PadenKahanOne(int _id, const MatrixExponential & _exp, const KDL:
 
 bool PadenKahanOne::solve(const KDL::Frame & rhs, const KDL::Frame & pointTransform, Solutions & solutions) const
 {
-    solutions.resize(PadenKahanOne::solutions());
-    JointIdsToSolutions jointIdsToSolutions(1);
-
     KDL::Vector f = pointTransform * p;
     KDL::Vector k = rhs * p;
 
@@ -37,19 +33,15 @@ bool PadenKahanOne::solve(const KDL::Frame & rhs, const KDL::Frame & pointTransf
     KDL::Vector v_p = v - v_w;
 
     double theta = std::atan2(KDL::dot(exp.getAxis(), u_p * v_p), KDL::dot(u_p, v_p));
-
-    jointIdsToSolutions[0] = {id, normalizeAngle(theta)};
-    solutions[0] = jointIdsToSolutions;
+    solutions = {{normalizeAngle(theta)}};
 
     return KDL::Equal(u_w, v_w) && KDL::Equal(u_p.Norm(), v_p.Norm());
 }
 
 // -----------------------------------------------------------------------------
 
-PadenKahanTwo::PadenKahanTwo(int _id1, int _id2, const MatrixExponential & _exp1, const MatrixExponential & _exp2, const KDL::Vector & _p, const KDL::Vector & _r)
-  : id1(_id1),
-    id2(_id2),
-    exp1(_exp1),
+PadenKahanTwo::PadenKahanTwo(const MatrixExponential & _exp1, const MatrixExponential & _exp2, const KDL::Vector & _p, const KDL::Vector & _r)
+  : exp1(_exp1),
     exp2(_exp2),
     p(_p),
     r(_r),
@@ -63,9 +55,6 @@ PadenKahanTwo::PadenKahanTwo(int _id1, int _id2, const MatrixExponential & _exp1
 
 bool PadenKahanTwo::solve(const KDL::Frame & rhs, const KDL::Frame & pointTransform, Solutions & solutions) const
 {
-    solutions.resize(PadenKahanTwo::solutions());
-    JointIdsToSolutions jointIdsToSolution1(2), jointIdsToSolution2(2);
-
     KDL::Vector f = pointTransform * p;
     KDL::Vector k = rhs * p;
 
@@ -84,14 +73,10 @@ bool PadenKahanTwo::solve(const KDL::Frame & rhs, const KDL::Frame & pointTransf
 
     KDL::Vector term1 = r + alpha * exp1.getAxis() + beta * exp2.getAxis();
 
-    double gamma2 = (std::pow(u.Norm(), 2) - std::pow(alpha, 2) - std::pow(beta, 2) - 2 * alpha * beta * axesDot) /
-            std::pow(axesCross.Norm(), 2);
+    double gamma2 = (std::pow(u.Norm(), 2) - std::pow(alpha, 2) - std::pow(beta, 2) - 2 * alpha * beta * axesDot) / std::pow(axesCross.Norm(), 2);
 
     bool gamma2_zero = KDL::Equal(gamma2, 0.0);
 
-    jointIdsToSolution1[0].first = jointIdsToSolution2[0].first = id1;
-    jointIdsToSolution1[1].first = jointIdsToSolution2[1].first = id2;
-
     bool ret;
 
     if (!gamma2_zero && gamma2 > 0.0)
@@ -117,11 +102,10 @@ bool PadenKahanTwo::solve(const KDL::Frame & rhs, const KDL::Frame & pointTransf
         double theta1_2 = std::atan2(KDL::dot(exp1.getAxis(), n1_p * v_p), KDL::dot(n1_p, v_p));
         double theta2_2 = std::atan2(KDL::dot(exp2.getAxis(), u_p * n2_p), KDL::dot(u_p, n2_p));
 
-        jointIdsToSolution1[0].second = normalizeAngle(theta1_1);
-        jointIdsToSolution1[1].second = normalizeAngle(theta2_1);
-
-        jointIdsToSolution2[0].second = normalizeAngle(theta1_2);
-        jointIdsToSolution2[1].second = normalizeAngle(theta2_2);
+        solutions = {
+            {normalizeAngle(theta1_1), normalizeAngle(theta2_1)},
+            {normalizeAngle(theta1_2), normalizeAngle(theta2_2)}
+        };
 
         ret = KDL::Equal(m1_p.Norm(), v_p.Norm());
     }
@@ -134,23 +118,24 @@ bool PadenKahanTwo::solve(const KDL::Frame & rhs, const KDL::Frame & pointTransf
         double theta1 = std::atan2(KDL::dot(exp1.getAxis(), n1_p * v_p), KDL::dot(n1_p, v_p));
         double theta2 = std::atan2(KDL::dot(exp2.getAxis(), u_p * n2_p), KDL::dot(u_p, n2_p));
 
-        jointIdsToSolution1[0].second = jointIdsToSolution2[0].second = normalizeAngle(theta1);
-        jointIdsToSolution1[1].second = jointIdsToSolution2[1].second = normalizeAngle(theta2);
+        double normalized1 = normalizeAngle(theta1);
+        double normalized2 = normalizeAngle(theta2);
+
+        solutions = {
+            {normalized1, normalized2},
+            {normalized1, normalized2}
+        };
 
         ret = gamma2_zero && KDL::Equal(n1_p.Norm(), v_p.Norm());
     }
 
-    solutions[0] = jointIdsToSolution1;
-    solutions[1] = jointIdsToSolution2;
-
     return ret;
 }
 
 // -----------------------------------------------------------------------------
 
-PadenKahanThree::PadenKahanThree(int _id, const MatrixExponential & _exp, const KDL::Vector & _p, const KDL::Vector & _k)
-    : id(_id),
-      exp(_exp),
+PadenKahanThree::PadenKahanThree(const MatrixExponential & _exp, const KDL::Vector & _p, const KDL::Vector & _k)
+    : exp(_exp),
       p(_p),
       k(_k),
       axisPow(vectorPow2(exp.getAxis()))
@@ -160,9 +145,6 @@ PadenKahanThree::PadenKahanThree(int _id, const MatrixExponential & _exp, const
 
 bool PadenKahanThree::solve(const KDL::Frame & rhs, const KDL::Frame & pointTransform, Solutions & solutions) const
 {
-    solutions.resize(PadenKahanThree::solutions());
-    JointIdsToSolutions jointIdsToSolution1(1), jointIdsToSolution2(1);
-
     KDL::Vector f = pointTransform * p;
     KDL::Vector rhsAsVector = rhs * p - k;
     double delta = rhsAsVector.Norm();
@@ -183,8 +165,6 @@ bool PadenKahanThree::solve(const KDL::Frame & rhs, const KDL::Frame & pointTran
     double betaCosAbs = std::abs(betaCos);
     bool beta_zero = KDL::Equal(betaCosAbs, 1.0);
 
-    jointIdsToSolution1[0].first = jointIdsToSolution2[0].first = id;
-
     bool ret;
 
     if (!beta_zero && betaCosAbs < 1.0)
@@ -195,20 +175,16 @@ bool PadenKahanThree::solve(const KDL::Frame & rhs, const KDL::Frame & pointTran
         double theta1 = alpha + beta;
         double theta2 = alpha - beta;
 
-        jointIdsToSolution1[0].second = normalizeAngle(theta1);
-        jointIdsToSolution2[0].second = normalizeAngle(theta2);
-
+        solutions = {{normalizeAngle(theta1)}, {normalizeAngle(theta2)}};
         ret = true;
     }
     else
     {
-        jointIdsToSolution1[0].second = jointIdsToSolution2[0].second = normalizeAngle(alpha);
+        double normalized = normalizeAngle(alpha);
+        solutions = {{normalized}, {normalized}};
         ret = beta_zero;
     }
 
-    solutions[0] = jointIdsToSolution1;
-    solutions[1] = jointIdsToSolution2;
-
     return ret;
 }
 
diff --git a/libraries/ScrewTheoryLib/PardosGotorSubproblems.cpp b/libraries/ScrewTheoryLib/PardosGotorSubproblems.cpp
index 94819a4cb..b1bd087e2 100644
--- a/libraries/ScrewTheoryLib/PardosGotorSubproblems.cpp
+++ b/libraries/ScrewTheoryLib/PardosGotorSubproblems.cpp
@@ -21,9 +21,8 @@ namespace
 
 // -----------------------------------------------------------------------------
 
-PardosGotorOne::PardosGotorOne(int _id, const MatrixExponential & _exp, const KDL::Vector & _p)
-    : id(_id),
-      exp(_exp),
+PardosGotorOne::PardosGotorOne(const MatrixExponential & _exp, const KDL::Vector & _p)
+    : exp(_exp),
       p(_p)
 {}
 
@@ -31,27 +30,20 @@ PardosGotorOne::PardosGotorOne(int _id, const MatrixExponential & _exp, const KD
 
 bool PardosGotorOne::solve(const KDL::Frame & rhs, const KDL::Frame & pointTransform, Solutions & solutions) const
 {
-    solutions.resize(PardosGotorOne::solutions());
-    JointIdsToSolutions jointIdsToSolutions(1);
-
     KDL::Vector f = pointTransform * p;
     KDL::Vector k = rhs * p;
 
     KDL::Vector diff = k - f;
     double theta = KDL::dot(exp.getAxis(), diff);
 
-    jointIdsToSolutions[0] = {id, theta};
-    solutions[0] = jointIdsToSolutions;
-
+    solutions = {{theta}};
     return true;
 }
 
 // -----------------------------------------------------------------------------
 
-PardosGotorTwo::PardosGotorTwo(int _id1, int _id2, const MatrixExponential & _exp1, const MatrixExponential & _exp2, const KDL::Vector & _p)
-    : id1(_id1),
-      id2(_id2),
-      exp1(_exp1),
+PardosGotorTwo::PardosGotorTwo(const MatrixExponential & _exp1, const MatrixExponential & _exp2, const KDL::Vector & _p)
+    : exp1(_exp1),
       exp2(_exp2),
       p(_p),
       crossPr2(exp2.getAxis() * exp1.getAxis()),
@@ -62,9 +54,6 @@ PardosGotorTwo::PardosGotorTwo(int _id1, int _id2, const MatrixExponential & _ex
 
 bool PardosGotorTwo::solve(const KDL::Frame & rhs, const KDL::Frame & pointTransform, Solutions & solutions) const
 {
-    solutions.resize(PardosGotorTwo::solutions());
-    JointIdsToSolutions jointIdsToSolutions(2);
-
     KDL::Vector f = pointTransform * p;
     KDL::Vector k = rhs * p;
 
@@ -85,19 +74,15 @@ bool PardosGotorTwo::solve(const KDL::Frame & rhs, const KDL::Frame & pointTrans
     double theta1 = KDL::dot(exp1.getAxis(), k - c);
     double theta2 = KDL::dot(exp2.getAxis(), c - f);
 
-    jointIdsToSolutions[0] = {id1, theta1};
-    jointIdsToSolutions[1] = {id2, theta2};
-
-    solutions[0] = jointIdsToSolutions;
+    solutions = {{theta1, theta2}};
 
     return true;
 }
 
 // -----------------------------------------------------------------------------
 
-PardosGotorThree::PardosGotorThree(int _id, const MatrixExponential & _exp, const KDL::Vector & _p, const KDL::Vector & _k)
-    : id(_id),
-      exp(_exp),
+PardosGotorThree::PardosGotorThree(const MatrixExponential & _exp, const KDL::Vector & _p, const KDL::Vector & _k)
+    : exp(_exp),
       p(_p),
       k(_k)
 {}
@@ -106,9 +91,6 @@ PardosGotorThree::PardosGotorThree(int _id, const MatrixExponential & _exp, cons
 
 bool PardosGotorThree::solve(const KDL::Frame & rhs, const KDL::Frame & pointTransform, Solutions & solutions) const
 {
-    solutions.resize(PardosGotorThree::solutions());
-    JointIdsToSolutions jointIdsToSolution1(1), jointIdsToSolution2(1);
-
     KDL::Vector f = pointTransform * p;
     KDL::Vector rhsAsVector = rhs * p - k;
     double delta = rhsAsVector.Norm();
@@ -119,36 +101,29 @@ bool PardosGotorThree::solve(const KDL::Frame & rhs, const KDL::Frame & pointTra
     double sq2 = std::pow(dotPr, 2) - std::pow(diff.Norm(), 2) + std::pow(delta, 2);
     bool sq2_zero = KDL::Equal(sq2, 0.0);
 
-    jointIdsToSolution1[0].first = jointIdsToSolution2[0].first = id;
-
     bool ret;
 
     if (!sq2_zero && sq2 > 0)
     {
         double sq = std::sqrt(std::abs(sq2));
-        jointIdsToSolution1[0].second = dotPr + sq;
-        jointIdsToSolution2[0].second = dotPr - sq;
+        solutions = {{dotPr + sq}, {dotPr - sq}};
         ret = true;
     }
     else
     {
         KDL::Vector proy = vectorPow2(exp.getAxis()) * diff;
-        jointIdsToSolution1[0].second = jointIdsToSolution2[0].second = proy.Norm();
+        double norm = proy.Norm();
+        solutions = {{norm}, {norm}};
         ret = sq2_zero;
     }
 
-    solutions[0] = jointIdsToSolution1;
-    solutions[1] = jointIdsToSolution2;
-
     return ret;
 }
 
 // -----------------------------------------------------------------------------
 
-PardosGotorFour::PardosGotorFour(int _id1, int _id2, const MatrixExponential & _exp1, const MatrixExponential & _exp2, const KDL::Vector & _p)
-    : id1(_id1),
-      id2(_id2),
-      exp1(_exp1),
+PardosGotorFour::PardosGotorFour(const MatrixExponential & _exp1, const MatrixExponential & _exp2, const KDL::Vector & _p)
+    : exp1(_exp1),
       exp2(_exp2),
       p(_p),
       n(computeNormal(exp1, exp2)),
@@ -159,9 +134,6 @@ PardosGotorFour::PardosGotorFour(int _id1, int _id2, const MatrixExponential & _
 
 bool PardosGotorFour::solve(const KDL::Frame & rhs, const KDL::Frame & pointTransform, Solutions & solutions) const
 {
-    solutions.resize(PardosGotorFour::solutions());
-    JointIdsToSolutions jointIdsToSolution1(2), jointIdsToSolution2(2);
-
     KDL::Vector f = pointTransform * p;
     KDL::Vector k = rhs * p;
 
@@ -190,9 +162,6 @@ bool PardosGotorFour::solve(const KDL::Frame & rhs, const KDL::Frame & pointTran
     double c_test = u_p_norm + v_p_norm - c_norm;
     bool c_zero = KDL::Equal(c_test, 0.0);
 
-    jointIdsToSolution1[0].first = jointIdsToSolution2[0].first = id1;
-    jointIdsToSolution1[1].first = jointIdsToSolution2[1].first = id2;
-
     bool ret;
 
     if (!c_zero && c_test > 0.0)
@@ -227,11 +196,10 @@ bool PardosGotorFour::solve(const KDL::Frame & rhs, const KDL::Frame & pointTran
         double theta1_2 = std::atan2(KDL::dot(exp1.getAxis(), n1_p * v_p), KDL::dot(n1_p, v_p));
         double theta2_2 = std::atan2(KDL::dot(exp2.getAxis(), u_p * n2_p), KDL::dot(u_p, n2_p));
 
-        jointIdsToSolution1[0].second = normalizeAngle(theta1_1);
-        jointIdsToSolution1[1].second = normalizeAngle(theta2_1);
-
-        jointIdsToSolution2[0].second = normalizeAngle(theta1_2);
-        jointIdsToSolution2[1].second = normalizeAngle(theta2_2);
+        solutions = {
+            {normalizeAngle(theta1_1), normalizeAngle(theta2_1)},
+            {normalizeAngle(theta1_2), normalizeAngle(theta2_2)}
+        };
 
         ret = samePlane && KDL::Equal(m1_p.Norm(), v_p_norm);
     }
@@ -240,14 +208,125 @@ bool PardosGotorFour::solve(const KDL::Frame & rhs, const KDL::Frame & pointTran
         double theta1 = std::atan2(KDL::dot(exp1.getAxis(), c_diff * v_p), KDL::dot(c_diff, v_p));
         double theta2 = std::atan2(KDL::dot(exp2.getAxis(), u_p * c_diff), KDL::dot(-c_diff, u_p));
 
-        jointIdsToSolution1[0].second = jointIdsToSolution2[0].second = normalizeAngle(theta1);
-        jointIdsToSolution1[1].second = jointIdsToSolution2[1].second = normalizeAngle(theta2);
+        double normalized1 = normalizeAngle(theta1);
+        double normalized2 = normalizeAngle(theta2);
+
+        solutions = {
+            {normalized1, normalized2},
+            {normalized1, normalized2}
+        };
 
         ret = c_zero;
     }
 
-    solutions[0] = jointIdsToSolution1;
-    solutions[1] = jointIdsToSolution2;
+    return ret;
+}
+
+// -----------------------------------------------------------------------------
+
+PardosGotorSix::PardosGotorSix(const MatrixExponential & _exp1, const MatrixExponential & _exp2, const KDL::Vector & _p)
+    : exp1(_exp1),
+      exp2(_exp2),
+      p(_p),
+      axesCross(exp1.getAxis() * exp2.getAxis()),
+      axisPow1(vectorPow2(exp1.getAxis())),
+      axisPow2(vectorPow2(exp2.getAxis())),
+      axesDot(KDL::dot(exp1.getAxis(), exp2.getAxis()))
+{}
+
+// -----------------------------------------------------------------------------
+
+bool PardosGotorSix::solve(const KDL::Frame & rhs, const KDL::Frame & pointTransform, Solutions & solutions) const
+{
+    KDL::Vector f = pointTransform * p;
+    KDL::Vector k = rhs * p;
+
+    KDL::Vector u = f - exp2.getOrigin();
+    KDL::Vector v = k - exp1.getOrigin();
+
+    KDL::Vector u_p = u - axisPow2 * u;
+    KDL::Vector v_p = v - axisPow1 * v;
+
+    KDL::Vector o2 = exp2.getOrigin() + axisPow2 * u;
+    KDL::Vector o1 = exp1.getOrigin() + axisPow1 * v;
+
+    double o2_dot = KDL::dot(exp2.getAxis(), o2);
+    double o1_dot = KDL::dot(exp1.getAxis(), o1);
+
+    KDL::Vector r3 = (exp1.getAxis() * (o1_dot - o2_dot * axesDot) + exp2.getAxis() * (o2_dot - o1_dot * axesDot)) / (1 - axesDot);
+
+    MatrixExponential exp3(MatrixExponential::TRANSLATION, axesCross);
+    PardosGotorThree pg3_2(exp3, r3, o2);
+    PardosGotorThree pg3_1(exp3, r3, o1);
+
+    Solutions pg3_1_sols, pg3_2_sols;
+
+    bool pg3_1_ret = pg3_1.solve(KDL::Frame(v_p - (r3 - o1)), KDL::Frame::Identity(), pg3_1_sols);
+    bool pg3_2_ret = pg3_2.solve(KDL::Frame(u_p - (r3 - o2)), KDL::Frame::Identity(), pg3_2_sols);
+
+    KDL::Vector c2 = r3 + pg3_2_sols[0][0] * exp3.getAxis();
+    KDL::Vector d2 = r3 + pg3_2_sols[1][0] * exp3.getAxis();
+
+    KDL::Vector c1 = r3 + pg3_1_sols[0][0] * exp3.getAxis();
+    KDL::Vector d1 = r3 + pg3_1_sols[1][0] * exp3.getAxis();
+
+    bool ret = pg3_1_ret && pg3_2_ret;
+
+    double theta1, theta2;
+
+    if (c1 == c2)
+    {
+        KDL::Vector m2 = c2 - exp2.getOrigin();
+        KDL::Vector m1 = c1 - exp1.getOrigin();
+
+        KDL::Vector m2_p = m2 - axisPow2 * m2;
+        KDL::Vector m1_p = m1 - axisPow1 * m1;
+
+        theta1 = std::atan2(KDL::dot(exp1.getAxis(), m1_p * v_p), KDL::dot(m1_p, v_p));
+        theta2 = std::atan2(KDL::dot(exp2.getAxis(), u_p * m2_p), KDL::dot(u_p, m2_p));
+    }
+    else if (d1 == d2)
+    {
+        KDL::Vector n2 = d2 - exp2.getOrigin();
+        KDL::Vector n1 = d1 - exp1.getOrigin();
+
+        KDL::Vector n2_p = n2 - axisPow2 * n2;
+        KDL::Vector n1_p = n1 - axisPow1 * n1;
+
+        theta1 = std::atan2(KDL::dot(exp1.getAxis(), n1_p * v_p), KDL::dot(n1_p, v_p));
+        theta2 = std::atan2(KDL::dot(exp2.getAxis(), u_p * n2_p), KDL::dot(u_p, n2_p));
+    }
+    else
+    {
+        // these might be equal if `pg3_2_ret` is false
+        KDL::Vector m1 = c2 - exp1.getOrigin();
+        KDL::Vector n1 = d2 - exp1.getOrigin();
+
+        if (m1.Norm() <= n1.Norm())
+        {
+            KDL::Vector m2 = c2 - exp2.getOrigin();
+
+            KDL::Vector m2_p = m2 - axisPow2 * m2;
+            KDL::Vector m1_p = m1 - axisPow1 * m1;
+
+            theta1 = std::atan2(KDL::dot(exp1.getAxis(), m1_p * v_p), KDL::dot(m1_p, v_p));
+            theta2 = std::atan2(KDL::dot(exp2.getAxis(), u_p * m2_p), KDL::dot(u_p, m2_p));
+        }
+        else
+        {
+            KDL::Vector n2 = d2 - exp2.getOrigin();
+
+            KDL::Vector n2_p = n2 - axisPow2 * n2;
+            KDL::Vector n1_p = n1 - axisPow1 * n1;
+
+            theta1 = std::atan2(KDL::dot(exp1.getAxis(), n1_p * v_p), KDL::dot(n1_p, v_p));
+            theta2 = std::atan2(KDL::dot(exp2.getAxis(), u_p * n2_p), KDL::dot(u_p, n2_p));
+        }
+
+        ret = false;
+    }
+
+    solutions = {{normalizeAngle(theta1), normalizeAngle(theta2)}};
 
     return ret;
 }
diff --git a/libraries/ScrewTheoryLib/ScrewTheoryIkProblem.cpp b/libraries/ScrewTheoryLib/ScrewTheoryIkProblem.cpp
index f966dcb41..ea4abd95b 100644
--- a/libraries/ScrewTheoryLib/ScrewTheoryIkProblem.cpp
+++ b/libraries/ScrewTheoryLib/ScrewTheoryIkProblem.cpp
@@ -18,9 +18,9 @@ namespace
 
     struct solution_accumulator
     {
-        inline int operator()(int count, const ScrewTheoryIkSubproblem * subproblem)
+        inline int operator()(int count, const ScrewTheoryIkProblem::JointIdsToSubproblem & idToSubproblem)
         {
-            return count * subproblem->solutions();
+            return count * idToSubproblem.second->solutions();
         }
     }
     solutionAccumulator;
@@ -46,7 +46,7 @@ ScrewTheoryIkProblem::~ScrewTheoryIkProblem()
 {
     for (auto & step : steps)
     {
-        delete step;
+        delete step.second;
     }
 }
 
@@ -72,7 +72,7 @@ bool ScrewTheoryIkProblem::solve(const KDL::Frame & H_S_T, Solutions & solutions
 
     ScrewTheoryIkSubproblem::Solutions partialSolutions;
 
-    for (int i = 0; i < steps.size(); i++)
+    for (const auto [ids, subproblem] : steps)
     {
         if (!firstIteration)
         {
@@ -84,14 +84,14 @@ bool ScrewTheoryIkProblem::solve(const KDL::Frame & H_S_T, Solutions & solutions
         // on demand for improved efficiency (instead of allocating everything from the start).
         int previousSize = solutions.size();
 
-        for (int j = 0; j < previousSize; j++)
+        for (int i = 0; i < previousSize; i++)
         {
             // Apply known frames to the first characteristic point for each subproblem.
-            const KDL::Frame & H = transformPoint(solutions[j], poeTerms);
+            const KDL::Frame & H = transformPoint(solutions[i], poeTerms);
 
             // 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);
+            reachable = reachable & subproblem->solve(rhsFrames[i], H, partialSolutions);
 
             // The global number of solutions is increased by this step.
             if (partialSolutions.size() > 1)
@@ -100,25 +100,24 @@ bool ScrewTheoryIkProblem::solve(const KDL::Frame & H_S_T, Solutions & solutions
                 solutions.resize(previousSize * partialSolutions.size());
                 rhsFrames.resize(previousSize * partialSolutions.size());
 
-                for (int k = 1; k < partialSolutions.size(); k++)
+                for (int j = 1; j < partialSolutions.size(); j++)
                 {
                     // Replicate known solutions, these won't change further on.
-                    solutions[j + previousSize * k] = solutions[j];
+                    solutions[i + previousSize * j] = solutions[i];
 
                     // Replicate right-hand side frames for the next iteration, these might change.
-                    rhsFrames[j + previousSize * k] = rhsFrames[j];
+                    rhsFrames[i + previousSize * j] = rhsFrames[i];
                 }
             }
 
             // For each local solution of this subproblem...
-            for (int k = 0; k < partialSolutions.size(); k++)
+            for (int j = 0; j < partialSolutions.size(); j++)
             {
-                const auto & jointIdsToSolutions = partialSolutions[k];
-
                 // For each joint-id-to-value pair of this local solution...
-                for (int l = 0; l < jointIdsToSolutions.size(); l++)
+                for (int k = 0; k < ids.size(); k++)
                 {
-                    auto [id, theta] = jointIdsToSolutions[l];
+                    auto id = ids[k];
+                    auto theta = partialSolutions[j][k];
 
                     // Preserve mapping of ids (associated to `poe`).
                     poeTerms[id] = EXP_KNOWN;
@@ -130,7 +129,7 @@ bool ScrewTheoryIkProblem::solve(const KDL::Frame & H_S_T, Solutions & solutions
                     }
 
                     // Store the final value in the desired index, don't shuffle it after this point.
-                    solutions[j + previousSize * k](id) = theta;
+                    solutions[i + previousSize * j](id) = theta;
                 }
             }
         }
diff --git a/libraries/ScrewTheoryLib/ScrewTheoryIkProblem.hpp b/libraries/ScrewTheoryLib/ScrewTheoryIkProblem.hpp
index dc0fa85b7..ba8dee650 100644
--- a/libraries/ScrewTheoryLib/ScrewTheoryIkProblem.hpp
+++ b/libraries/ScrewTheoryLib/ScrewTheoryIkProblem.hpp
@@ -25,14 +25,8 @@ namespace roboticslab
 class ScrewTheoryIkSubproblem
 {
 public:
-    //! Maps a joint id to a screw magnitude
-    using JointIdToSolution = std::pair<int, double>;
-
-    //! At least one joint-id+value pair per solution
-    using JointIdsToSolutions = std::vector<JointIdToSolution>;
-
     //! Collection of local IK solutions
-    using Solutions = std::vector<JointIdsToSolutions>;
+    using Solutions = std::vector<std::vector<double>>;
 
     //! Destructor
     virtual ~ScrewTheoryIkSubproblem() = default;
@@ -91,11 +85,14 @@ class ScrewTheoryIkSubproblem
  *
  * @see ScrewTheoryIkProblemBuilder
  */
-class ScrewTheoryIkProblem
+class ScrewTheoryIkProblem final
 {
 public:
-    //! Ordered sequence of IK subproblems needed to solve a IK problem
-    using Steps = std::vector<const ScrewTheoryIkSubproblem *>;
+    //! Pair of joint ids and an their associated local IK subproblem
+    using JointIdsToSubproblem = std::pair<std::vector<int>, const ScrewTheoryIkSubproblem *>;
+
+    //! Ordered sequence of IK subproblems that solve a global IK problem
+    using Steps = std::vector<JointIdsToSubproblem>;
 
     //! Collection of global IK solutions
     using Solutions = std::vector<KDL::JntArray>;
@@ -215,7 +212,7 @@ class ScrewTheoryIkProblemBuilder
     void simplifyWithPadenKahanThree(const KDL::Vector & point);
     void simplifyWithPardosOne();
 
-    ScrewTheoryIkSubproblem * trySolve(int depth);
+    ScrewTheoryIkProblem::JointIdsToSubproblem trySolve(int depth);
 
     PoeExpression poe;
 
diff --git a/libraries/ScrewTheoryLib/ScrewTheoryIkProblemBuilder.cpp b/libraries/ScrewTheoryLib/ScrewTheoryIkProblemBuilder.cpp
index abe47a8f3..810da4eb0 100644
--- a/libraries/ScrewTheoryLib/ScrewTheoryIkProblemBuilder.cpp
+++ b/libraries/ScrewTheoryLib/ScrewTheoryIkProblemBuilder.cpp
@@ -132,7 +132,7 @@ namespace
     {
         for (auto & step : steps)
         {
-            delete step;
+            delete step.second;
         }
 
         steps.clear();
@@ -299,11 +299,11 @@ ScrewTheoryIkProblem::Steps ScrewTheoryIkProblemBuilder::searchSolutions()
         simplify(depth);
 
         // Find a solution if available.
-        if (auto * subproblem = trySolve(depth))
+        if (auto [ids, subproblem] = trySolve(depth); subproblem != nullptr)
         {
             // Solution found, reset and start again. We'll iterate over the same points, taking
             // into account that some terms are already known.
-            steps.push_back(subproblem);
+            steps.emplace_back(ids, subproblem);
             iterators.assign(iterators.size(), points.begin());
             testPoints[0] = points[0];
             depth = 0;
@@ -381,14 +381,14 @@ void ScrewTheoryIkProblemBuilder::refreshSimplificationState()
 
 // -----------------------------------------------------------------------------
 
-ScrewTheoryIkSubproblem * ScrewTheoryIkProblemBuilder::trySolve(int depth)
+ScrewTheoryIkProblem::JointIdsToSubproblem ScrewTheoryIkProblemBuilder::trySolve(int depth)
 {
     int unknownsCount = std::count_if(poeTerms.begin(), poeTerms.end(), unknownNotSimplifiedTerm);
 
     if (unknownsCount == 0 || unknownsCount > 2) // TODO: hardcoded
     {
         // Can't solve yet, too many unknowns or oversimplified.
-        return nullptr;
+        return {{}, nullptr};
     }
 
     // Find rightmost unknown and not simplified PoE term.
@@ -405,13 +405,13 @@ ScrewTheoryIkSubproblem * ScrewTheoryIkProblemBuilder::trySolve(int depth)
                     && !liesOnAxis(lastExp, testPoints[0]))
             {
                 poeTerms[lastExpId].known = true;
-                return new PadenKahanOne(lastExpId, lastExp, testPoints[0]);
+                return {{lastExpId}, new PadenKahanOne(lastExp, testPoints[0])};
             }
 
             if (lastExp.getMotionType() == MatrixExponential::TRANSLATION)
             {
                 poeTerms[lastExpId].known = true;
-                return new PardosGotorOne(lastExpId, lastExp, testPoints[0]);
+                return {{lastExpId}, new PardosGotorOne(lastExp, testPoints[0])};
             }
         }
 
@@ -420,7 +420,7 @@ ScrewTheoryIkSubproblem * ScrewTheoryIkProblemBuilder::trySolve(int depth)
             // There can be no other non-simplified terms to the left of our unknown.
             if (std::find_if(poeTerms.begin(), poeTerms.end(), knownNotSimplifiedTerm) != poeTerms.end())
             {
-                return nullptr;
+                return {{}, nullptr};
             }
 
             if (lastExp.getMotionType() == MatrixExponential::ROTATION
@@ -428,13 +428,13 @@ ScrewTheoryIkSubproblem * ScrewTheoryIkProblemBuilder::trySolve(int depth)
                     && !liesOnAxis(lastExp, testPoints[1]))
             {
                 poeTerms[lastExpId].known = true;
-                return new PadenKahanThree(lastExpId, lastExp, testPoints[0], testPoints[1]);
+                return {{lastExpId}, new PadenKahanThree(lastExp, testPoints[0], testPoints[1])};
             }
 
             if (lastExp.getMotionType() == MatrixExponential::TRANSLATION)
             {
                 poeTerms[lastExpId].known = true;
-                return new PardosGotorThree(lastExpId, lastExp, testPoints[0], testPoints[1]);
+                return {{lastExpId}, new PardosGotorThree(lastExp, testPoints[0], testPoints[1])};
             }
         }
     }
@@ -446,7 +446,7 @@ ScrewTheoryIkSubproblem * ScrewTheoryIkProblemBuilder::trySolve(int depth)
 
         if (!unknownNotSimplifiedTerm(*nextToLastUnknown))
         {
-            return nullptr;
+            return {{}, nullptr};
         }
 
         int nextToLastExpId = lastExpId - 1;
@@ -462,7 +462,7 @@ ScrewTheoryIkSubproblem * ScrewTheoryIkProblemBuilder::trySolve(int depth)
                     && intersectingAxes(lastExp, nextToLastExp, r))
             {
                 poeTerms[lastExpId].known = poeTerms[nextToLastExpId].known = true;
-                return new PadenKahanTwo(nextToLastExpId, lastExpId, nextToLastExp, lastExp, testPoints[0], r);
+                return {{nextToLastExpId, lastExpId}, new PadenKahanTwo(nextToLastExp, lastExp, testPoints[0], r)};
             }
 
             if (lastExp.getMotionType() == MatrixExponential::TRANSLATION
@@ -470,7 +470,7 @@ ScrewTheoryIkSubproblem * ScrewTheoryIkProblemBuilder::trySolve(int depth)
                     && !parallelAxes(lastExp, nextToLastExp))
             {
                 poeTerms[lastExpId].known = poeTerms[nextToLastExpId].known = true;
-                return new PardosGotorTwo(nextToLastExpId, lastExpId, nextToLastExp, lastExp, testPoints[0]);
+                return {{nextToLastExpId, lastExpId}, new PardosGotorTwo(nextToLastExp, lastExp, testPoints[0])};
             }
 
             if (lastExp.getMotionType() == MatrixExponential::ROTATION
@@ -479,12 +479,12 @@ ScrewTheoryIkSubproblem * ScrewTheoryIkProblemBuilder::trySolve(int depth)
                     && !colinearAxes(lastExp, nextToLastExp))
             {
                 poeTerms[lastExpId].known = poeTerms[nextToLastExpId].known = true;
-                return new PardosGotorFour(nextToLastExpId, lastExpId, nextToLastExp, lastExp, testPoints[0]);
+                return {{nextToLastExpId, lastExpId}, new PardosGotorFour(nextToLastExp, lastExp, testPoints[0])};
             }
         }
     }
 
-    return nullptr;
+    return {{}, nullptr};
 }
 
 // -----------------------------------------------------------------------------
diff --git a/libraries/ScrewTheoryLib/ScrewTheoryIkSubproblems.hpp b/libraries/ScrewTheoryLib/ScrewTheoryIkSubproblems.hpp
index 31294c412..9adb7647b 100644
--- a/libraries/ScrewTheoryLib/ScrewTheoryIkSubproblems.hpp
+++ b/libraries/ScrewTheoryLib/ScrewTheoryIkSubproblems.hpp
@@ -26,11 +26,10 @@ class PadenKahanOne : public ScrewTheoryIkSubproblem
     /**
      * @brief Constructor
      *
-     * @param id Zero-based joint id of the product of exponentials (POE) term.
      * @param exp POE term.
      * @param p Characteristic point.
      */
-    PadenKahanOne(int id, const MatrixExponential & exp, const KDL::Vector & p);
+    PadenKahanOne(const MatrixExponential & exp, const KDL::Vector & p);
 
     bool solve(const KDL::Frame & rhs, const KDL::Frame & pointTransform, Solutions & solutions) const override;
 
@@ -41,7 +40,6 @@ class PadenKahanOne : public ScrewTheoryIkSubproblem
     { return "PK1"; }
 
 private:
-    const int id;
     const MatrixExponential exp;
     const KDL::Vector p;
     const KDL::Rotation axisPow;
@@ -62,14 +60,12 @@ class PadenKahanTwo : public ScrewTheoryIkSubproblem
     /**
      * @brief Constructor
      *
-     * @param id1 Zero-based joint id of the first product of exponentials (POE) term.
-     * @param id2 Zero-based joint id of the second POE term.
      * @param exp1 First POE term.
      * @param exp2 Second POE term.
      * @param p Characteristic point.
      * @param r Point of intersection between both screw axes.
      */
-    PadenKahanTwo(int id1, int id2, const MatrixExponential & exp1, const MatrixExponential & exp2, const KDL::Vector & p, const KDL::Vector & r);
+    PadenKahanTwo(const MatrixExponential & exp1, const MatrixExponential & exp2, const KDL::Vector & p, const KDL::Vector & r);
 
     bool solve(const KDL::Frame & rhs, const KDL::Frame & pointTransform, Solutions & solutions) const override;
 
@@ -80,7 +76,6 @@ class PadenKahanTwo : public ScrewTheoryIkSubproblem
     { return "PK2"; }
 
 private:
-    const int id1, id2;
     const MatrixExponential exp1, exp2;
     const KDL::Vector p, r, axesCross;
     const KDL::Rotation axisPow1, axisPow2;
@@ -102,12 +97,11 @@ class PadenKahanThree : public ScrewTheoryIkSubproblem
     /**
      * @brief Constructor
      *
-     * @param id Zero-based joint id of the product of exponentials (POE) term.
      * @param exp POE term.
      * @param p First characteristic point.
      * @param k Second characteristic point.
      */
-    PadenKahanThree(int id, const MatrixExponential & exp, const KDL::Vector & p, const KDL::Vector & k);
+    PadenKahanThree(const MatrixExponential & exp, const KDL::Vector & p, const KDL::Vector & k);
 
     bool solve(const KDL::Frame & rhs, const KDL::Frame & pointTransform, Solutions & solutions) const override;
 
@@ -118,7 +112,6 @@ class PadenKahanThree : public ScrewTheoryIkSubproblem
     { return "PK3"; }
 
 private:
-    const int id;
     const MatrixExponential exp;
     const KDL::Vector p, k;
     const KDL::Rotation axisPow;
@@ -140,11 +133,10 @@ class PardosGotorOne : public ScrewTheoryIkSubproblem
     /**
      * @brief Constructor
      *
-     * @param id Zero-based joint id of the product of exponentials (POE) term.
      * @param exp POE term.
      * @param p Characteristic point.
      */
-    PardosGotorOne(int id, const MatrixExponential & exp, const KDL::Vector & p);
+    PardosGotorOne(const MatrixExponential & exp, const KDL::Vector & p);
 
     bool solve(const KDL::Frame & rhs, const KDL::Frame & pointTransform, Solutions & solutions) const override;
 
@@ -155,7 +147,6 @@ class PardosGotorOne : public ScrewTheoryIkSubproblem
     { return "PG1"; }
 
 private:
-    const int id;
     const MatrixExponential exp;
     const KDL::Vector p;
 };
@@ -176,13 +167,11 @@ class PardosGotorTwo : public ScrewTheoryIkSubproblem
     /**
      * @brief Constructor
      *
-     * @param id1 Zero-based joint id of the first product of exponentials (POE) term.
-     * @param id2 Zero-based joint id of the second POE term.
      * @param exp1 First POE term.
      * @param exp2 Second POE term.
      * @param p Characteristic point.
      */
-    PardosGotorTwo(int id1, int id2, const MatrixExponential & exp1, const MatrixExponential & exp2, const KDL::Vector & p);
+    PardosGotorTwo(const MatrixExponential & exp1, const MatrixExponential & exp2, const KDL::Vector & p);
 
     bool solve(const KDL::Frame & rhs, const KDL::Frame & pointTransform, Solutions & solutions) const override;
 
@@ -193,7 +182,6 @@ class PardosGotorTwo : public ScrewTheoryIkSubproblem
     { return "PG2"; }
 
 private:
-    const int id1, id2;
     const MatrixExponential exp1, exp2;
     const KDL::Vector p, crossPr2;
     const double crossPr2Norm;
@@ -215,12 +203,11 @@ class PardosGotorThree : public ScrewTheoryIkSubproblem
     /**
      * @brief Constructor
      *
-     * @param id Zero-based joint id of the product of exponentials (POE) term.
      * @param exp POE term.
      * @param p First characteristic point.
      * @param k Second characteristic point.
      */
-    PardosGotorThree(int id, const MatrixExponential & exp, const KDL::Vector & p, const KDL::Vector & k);
+    PardosGotorThree(const MatrixExponential & exp, const KDL::Vector & p, const KDL::Vector & k);
 
     bool solve(const KDL::Frame & rhs, const KDL::Frame & pointTransform, Solutions & solutions) const override;
 
@@ -231,7 +218,6 @@ class PardosGotorThree : public ScrewTheoryIkSubproblem
     { return "PG3"; }
 
 private:
-    const int id;
     const MatrixExponential exp;
     const KDL::Vector p, k;
 };
@@ -243,7 +229,7 @@ class PardosGotorThree : public ScrewTheoryIkSubproblem
  *
  * Dual solution, double revolute joint geometric IK subproblem given by
  * @f$ e\,^{\hat{\xi_1}\,{\theta_1}} \cdot e\,^{\hat{\xi_2}\,{\theta_2}} \cdot p = k @f$
- * (two consecutive parallel rotation screws applied to a point,
+ * (consecutive parallel rotation screws applied to a point,
  * see @cite pardosgotor2018str @cite pardosgotor2022str).
  */
 class PardosGotorFour : public ScrewTheoryIkSubproblem
@@ -252,13 +238,11 @@ class PardosGotorFour : public ScrewTheoryIkSubproblem
     /**
      * @brief Constructor
      *
-     * @param id1 Zero-based joint id of the first product of exponentials (POE) term.
-     * @param id2 Zero-based joint id of the second POE term.
      * @param exp1 First POE term.
      * @param exp2 Second POE term.
      * @param p Characteristic point.
      */
-    PardosGotorFour(int id1, int id2, const MatrixExponential & exp1, const MatrixExponential & exp2, const KDL::Vector & p);
+    PardosGotorFour(const MatrixExponential & exp1, const MatrixExponential & exp2, const KDL::Vector & p);
 
     bool solve(const KDL::Frame & rhs, const KDL::Frame & pointTransform, Solutions & solutions) const override;
 
@@ -269,12 +253,48 @@ class PardosGotorFour : public ScrewTheoryIkSubproblem
     { return "PG4"; }
 
 private:
-    const int id1, id2;
     const MatrixExponential exp1, exp2;
     const KDL::Vector p, n;
     const KDL::Rotation axisPow;
 };
 
+/**
+ * @ingroup ScrewTheoryLib
+ *
+ * @brief Sixth Pardos-Gotor subproblem
+ *
+ * Dual solution, double revolute joint geometric IK subproblem given by
+ * @f$ e\,^{\hat{\xi_1}\,{\theta_1}} \cdot e\,^{\hat{\xi_2}\,{\theta_2}} \cdot p = k @f$
+ * (consecutive skew rotation screws applied to a point,
+ * see @cite pardosgotor2022str).
+ */
+class PardosGotorSix : public ScrewTheoryIkSubproblem
+{
+public:
+    /**
+     * @brief Constructor
+     *
+     * @param exp1 First POE term.
+     * @param exp2 Second POE term.
+     * @param p Characteristic point.
+     */
+    PardosGotorSix(const MatrixExponential & exp1, const MatrixExponential & exp2, const KDL::Vector & p);
+
+    bool solve(const KDL::Frame & rhs, const KDL::Frame & pointTransform, Solutions & solutions) const override;
+
+    int solutions() const override
+    { return 1; }
+
+    const char * describe() const override
+    { return "PG6"; }
+
+private:
+    const MatrixExponential exp1, exp2;
+    const KDL::Vector p, axesCross;
+    const KDL::Rotation axisPow1, axisPow2;
+    const double axesDot;
+};
+
 } // namespace roboticslab
 
 #endif // __SCREW_THEORY_IK_SUBPROBLEMS_HPP__
diff --git a/libraries/YarpPlugins/KdlSolver/ChainIkSolverPos_ST.cpp b/libraries/YarpPlugins/KdlSolver/ChainIkSolverPos_ST.cpp
index 52771a78b..105093376 100644
--- a/libraries/YarpPlugins/KdlSolver/ChainIkSolverPos_ST.cpp
+++ b/libraries/YarpPlugins/KdlSolver/ChainIkSolverPos_ST.cpp
@@ -93,7 +93,7 @@ KDL::ChainIkSolverPos * ChainIkSolverPos_ST::create(const KDL::Chain & chain, co
     const auto & steps = problem->getSteps();
 
     std::vector<const char *> descriptions;
-    std::transform(steps.cbegin(), steps.cend(), std::back_inserter(descriptions), [](const auto * step) { return step->describe(); });
+    std::transform(steps.cbegin(), steps.cend(), std::back_inserter(descriptions), [](const auto & step) { return step.second->describe(); });
     yCInfo(KDLS) << "Found" << problem->solutions() << "solutions:" << descriptions << (problem->isReversed() ? "(reversed)" : "");
 
     ConfigurationSelector * config = configFactory.create();
diff --git a/tests/testScrewTheory.cpp b/tests/testScrewTheory.cpp
index 996cee5d6..7e42261a9 100644
--- a/tests/testScrewTheory.cpp
+++ b/tests/testScrewTheory.cpp
@@ -1,8 +1,5 @@
 #include "gtest/gtest.h"
 
-#include <algorithm> // std::sort
-#include <utility> // std::pair
-
 #include <kdl/chain.hpp>
 #include <kdl/chainfksolverpos_recursive.hpp>
 #include <kdl/frames.hpp>
@@ -276,34 +273,17 @@ class ScrewTheoryTest : public testing::Test
 
     static void checkSolutions(const ScrewTheoryIkSubproblem::Solutions & actual, const ScrewTheoryIkSubproblem::Solutions & expected)
     {
-        ScrewTheoryIkSubproblem::JointIdsToSolutions actualSorted, expectedSorted;
+        ASSERT_EQ(actual.size(), expected.size());
 
         for (int i = 0; i < actual.size(); i++)
         {
-            for (int j = 0; j < actual[i].size(); j++)
-            {
-                actualSorted.push_back(actual[i][j]);
-            }
-        }
+            ASSERT_EQ(actual[i].size(), expected[i].size());
 
-        for (int i = 0; i < expected.size(); i++)
-        {
-            for (int j = 0; j < expected[i].size(); j++)
+            for (int j = 0; j < actual[i].size(); j++)
             {
-                expectedSorted.push_back(expected[i][j]);
+                ASSERT_NEAR(actual[i][j], expected[i][j], KDL::epsilon);
             }
         }
-
-        ASSERT_EQ(actualSorted.size(), expectedSorted.size());
-
-        std::sort(actualSorted.begin(), actualSorted.end(), solutionComparator);
-        std::sort(expectedSorted.begin(), expectedSorted.end(), solutionComparator);
-
-        for (int i = 0; i < actualSorted.size(); i++)
-        {
-            ASSERT_EQ(actualSorted[i].first, expectedSorted[i].first);
-            ASSERT_NEAR(actualSorted[i].second, expectedSorted[i].second, KDL::epsilon);
-        }
     }
 
     static void checkRobotKinematics(const KDL::Chain & chain, const PoeExpression & poe, int soln)
@@ -334,10 +314,10 @@ class ScrewTheoryTest : public testing::Test
         ASSERT_TRUE(ikProblem->solve(H_S_T_q_ST, solutions));
         delete ikProblem;
 
-        for (int i = 0; i < solutions.size(); i++)
+        for (const auto & solution : solutions)
         {
             KDL::Frame H_S_T_q_ST_validate;
-            ASSERT_TRUE(poe.evaluate(solutions[i], H_S_T_q_ST_validate));
+            ASSERT_TRUE(poe.evaluate(solution, H_S_T_q_ST_validate));
             ASSERT_EQ(H_S_T_q_ST_validate, H_S_T_q_ST);
         }
     }
@@ -365,17 +345,6 @@ class ScrewTheoryTest : public testing::Test
 
         return -1;
     }
-
-private:
-
-    static struct compare_solutions
-    {
-        bool operator()(const std::pair<int, double> & lhs, const std::pair<int, double> & rhs)
-        {
-            return !(lhs.first > rhs.first) && (lhs.first < rhs.first || lhs.second < rhs.second);
-        }
-    }
-    solutionComparator;
 };
 
 TEST_F(ScrewTheoryTest, MatrixExponentialInit)
@@ -560,7 +529,7 @@ TEST_F(ScrewTheoryTest, PadenKahanOne)
     KDL::Vector k(1, 1, 1);
 
     MatrixExponential exp(MatrixExponential::ROTATION, KDL::Vector(0, 1, 0), KDL::Vector(1, 0, 0));
-    PadenKahanOne pk1(0, exp, p);
+    PadenKahanOne pk1(exp, p);
 
     ASSERT_EQ(pk1.solutions(), 1);
 
@@ -571,11 +540,7 @@ TEST_F(ScrewTheoryTest, PadenKahanOne)
     ASSERT_EQ(actual.size(), 1);
     ASSERT_EQ(actual[0].size(), 1);
 
-    ScrewTheoryIkSubproblem::Solutions expected(1);
-    ScrewTheoryIkSubproblem::JointIdsToSolutions sol(1);
-
-    sol[0] = {0, KDL::PI / 2};
-    expected[0] = sol;
+    ScrewTheoryIkSubproblem::Solutions expected = {{KDL::PI / 2}};
 
     checkSolutions(actual, expected);
 
@@ -600,7 +565,7 @@ TEST_F(ScrewTheoryTest, PadenKahanTwo)
 
     MatrixExponential exp1(MatrixExponential::ROTATION, KDL::Vector(1, 0, 0), r);
     MatrixExponential exp2(MatrixExponential::ROTATION, KDL::Vector(0, 1, 0), r);
-    PadenKahanTwo pk2(0, 1, exp1, exp2, p, r);
+    PadenKahanTwo pk2(exp1, exp2, p, r);
 
     ASSERT_EQ(pk2.solutions(), 2);
 
@@ -612,17 +577,10 @@ TEST_F(ScrewTheoryTest, PadenKahanTwo)
     ASSERT_EQ(actual[0].size(), 2);
     ASSERT_EQ(actual[1].size(), 2);
 
-    ScrewTheoryIkSubproblem::Solutions expected(2);
-    ScrewTheoryIkSubproblem::JointIdsToSolutions sols1(2), sols2(2);
-
-    sols1[0] = {0, KDL::PI / 2};
-    sols1[1] = {1, KDL::PI / 2};
-
-    sols2[0] = {0, KDL::PI};
-    sols2[1] = {1, -KDL::PI / 2};
-
-    expected[0] = sols1;
-    expected[1] = sols2;
+    ScrewTheoryIkSubproblem::Solutions expected = {
+        {KDL::PI, -KDL::PI / 2},
+        {KDL::PI / 2, KDL::PI / 2}
+    };
 
     checkSolutions(actual, expected);
 
@@ -630,16 +588,15 @@ TEST_F(ScrewTheoryTest, PadenKahanTwo)
     KDL::Frame rhs2(k2 - p);
     ASSERT_FALSE(pk2.solve(rhs2, KDL::Frame::Identity(), actual));
 
-    sols1[0].second = sols2[0].second = 3 * KDL::PI / 4;
-    sols1[1].second = sols2[1].second = KDL::PI;
-
-    expected[0] = sols1;
-    expected[1] = sols2;
+    expected = {
+        {3 * KDL::PI / 4, KDL::PI},
+        {3 * KDL::PI / 4, KDL::PI}
+    };
 
     checkSolutions(actual, expected);
 
     KDL::Vector p3 = p + KDL::Vector(0.5, 0, 0);
-    PadenKahanTwo pk2c(0, 1, exp1, exp2, p3, r);
+    PadenKahanTwo pk2c(exp1, exp2, p3, r);
     KDL::Frame rhs3(k2 - p3);
     ASSERT_FALSE(pk2c.solve(rhs3, KDL::Frame::Identity(), actual));
 
@@ -653,7 +610,7 @@ TEST_F(ScrewTheoryTest, PadenKahanThree)
     KDL::Vector delta(-1, 0, 0);
 
     MatrixExponential exp(MatrixExponential::ROTATION, KDL::Vector(0, 1, 0), KDL::Vector(1, 0, 0));
-    PadenKahanThree pk3(0, exp, p, k);
+    PadenKahanThree pk3(exp, p, k);
 
     ASSERT_EQ(pk3.solutions(), 2);
 
@@ -665,38 +622,34 @@ TEST_F(ScrewTheoryTest, PadenKahanThree)
     ASSERT_EQ(actual[0].size(), 1);
     ASSERT_EQ(actual[1].size(), 1);
 
-    ScrewTheoryIkSubproblem::Solutions expected(2);
-    ScrewTheoryIkSubproblem::JointIdsToSolutions sol1(1), sol2(1);
-
-    sol1[0] = {0, KDL::PI / 2};
-    sol2[0] = {0, KDL::PI};
-
-    expected[0] = sol1;
-    expected[1] = sol2;
+    ScrewTheoryIkSubproblem::Solutions expected = {
+        {KDL::PI},
+        {KDL::PI / 2}
+    };
 
     checkSolutions(actual, expected);
 
     KDL::Vector k2 = k + KDL::Vector(1, 0, 1);
-    PadenKahanThree pk3b(0, exp, p, k2);
+    PadenKahanThree pk3b(exp, p, k2);
     KDL::Frame rhs2(delta - (p - k2));
 
     ASSERT_FALSE(pk3b.solve(rhs2, KDL::Frame::Identity(), actual));
 
-    sol1[0] = sol2[0] = {0, 3 * KDL::PI / 4};
-
-    expected[0] = sol1;
-    expected[1] = sol2;
+    expected = {
+        {3 * KDL::PI / 4},
+        {3 * KDL::PI / 4}
+    };
 
     checkSolutions(actual, expected);
 }
 
-TEST_F(ScrewTheoryTest, PardosOne)
+TEST_F(ScrewTheoryTest, PardosGotorOne)
 {
     KDL::Vector p(1, 0, 0);
     KDL::Vector k(1, 1, 0);
 
     MatrixExponential exp(MatrixExponential::TRANSLATION, KDL::Vector(0, 1, 0));
-    PardosGotorOne pg1(0, exp, p);
+    PardosGotorOne pg1(exp, p);
 
     ASSERT_EQ(pg1.solutions(), 1);
 
@@ -707,23 +660,19 @@ TEST_F(ScrewTheoryTest, PardosOne)
     ASSERT_EQ(actual.size(), 1);
     ASSERT_EQ(actual[0].size(), 1);
 
-    ScrewTheoryIkSubproblem::Solutions expected(1);
-    ScrewTheoryIkSubproblem::JointIdsToSolutions sol(1);
-
-    sol[0] = {0, 1.0};
-    expected[0] = sol;
+    ScrewTheoryIkSubproblem::Solutions expected = {{1.0}};
 
     checkSolutions(actual, expected);
 }
 
-TEST_F(ScrewTheoryTest, PardosTwo)
+TEST_F(ScrewTheoryTest, PardosGotorTwo)
 {
     KDL::Vector p(1, 1, 0);
     KDL::Vector k(2, 3, 0);
 
     MatrixExponential exp1(MatrixExponential::TRANSLATION, KDL::Vector(0, 1, 0));
     MatrixExponential exp2(MatrixExponential::TRANSLATION, KDL::Vector(1, 0, 0));
-    PardosGotorTwo pg2(0, 1, exp1, exp2, p);
+    PardosGotorTwo pg2(exp1, exp2, p);
 
     ASSERT_EQ(pg2.solutions(), 1);
 
@@ -734,25 +683,19 @@ TEST_F(ScrewTheoryTest, PardosTwo)
     ASSERT_EQ(actual.size(), 1);
     ASSERT_EQ(actual[0].size(), 2);
 
-    ScrewTheoryIkSubproblem::Solutions expected(1);
-    ScrewTheoryIkSubproblem::JointIdsToSolutions sols(2);
-
-    sols[0] = {0, k.y() - p.y()};
-    sols[1] = {1, k.x() - p.x()};
-
-    expected[0] = sols;
+    ScrewTheoryIkSubproblem::Solutions expected = {{k.y() - p.y(), k.x() - p.x()}};
 
     checkSolutions(actual, expected);
 }
 
-TEST_F(ScrewTheoryTest, PardosThree)
+TEST_F(ScrewTheoryTest, PardosGotorThree)
 {
     KDL::Vector p(1, 0, 0);
     KDL::Vector k(1, 2, 0);
     KDL::Vector delta(0, 1, 0);
 
     MatrixExponential exp(MatrixExponential::TRANSLATION, KDL::Vector(0, 1, 0));
-    PardosGotorThree pg3(0, exp, p, k);
+    PardosGotorThree pg3(exp, p, k);
 
     ASSERT_EQ(pg3.solutions(), 2);
 
@@ -764,39 +707,32 @@ TEST_F(ScrewTheoryTest, PardosThree)
     ASSERT_EQ(actual[0].size(), 1);
     ASSERT_EQ(actual[1].size(), 1);
 
-    ScrewTheoryIkSubproblem::Solutions expected(2);
-    ScrewTheoryIkSubproblem::JointIdsToSolutions sol1(1), sol2(1);
-
-    sol1[0] = {0, k.y() - p.y() - delta.y()};
-    sol2[0] = {0, k.y() - p.y() + delta.y()};
-
-    expected[0] = sol1;
-    expected[1] = sol2;
+    ScrewTheoryIkSubproblem::Solutions expected = {
+        {k.y() - p.y() + delta.y()},
+        {k.y() - p.y() - delta.y()}
+    };
 
     checkSolutions(actual, expected);
 
     KDL::Vector k2 = k + KDL::Vector(2, 0, 0);
-    PardosGotorThree pg3b(0, exp, p, k2);
+    PardosGotorThree pg3b(exp, p, k2);
     KDL::Frame rhs2(delta - (p - k2));
 
     ASSERT_FALSE(pg3b.solve(rhs2, KDL::Frame::Identity(), actual));
 
-    sol1[0] = sol2[0] = {0, 2};
-
-    expected[0] = sol1;
-    expected[1] = sol2;
+    expected = {{2.0}, {2.0}};
 
     checkSolutions(actual, expected);
 }
 
-TEST_F(ScrewTheoryTest, PardosFour)
+TEST_F(ScrewTheoryTest, PardosGotorFour)
 {
     KDL::Vector p(0, 1, 0);
     KDL::Vector k(3, 1, 1);
 
     MatrixExponential exp1(MatrixExponential::ROTATION, KDL::Vector(0, 1, 0), KDL::Vector(2, 0, 0));
     MatrixExponential exp2(MatrixExponential::ROTATION, KDL::Vector(0, 1, 0), KDL::Vector(1, 0, 0));
-    PardosGotorFour pg4(0, 1, exp1, exp2, p);
+    PardosGotorFour pg4(exp1, exp2, p);
 
     ASSERT_EQ(pg4.solutions(), 2);
 
@@ -808,17 +744,10 @@ TEST_F(ScrewTheoryTest, PardosFour)
     ASSERT_EQ(actual[0].size(), 2);
     ASSERT_EQ(actual[1].size(), 2);
 
-    ScrewTheoryIkSubproblem::Solutions expected(2);
-    ScrewTheoryIkSubproblem::JointIdsToSolutions sols1(2), sols2(2);
-
-    sols1[0] = {0, KDL::PI / 2};
-    sols1[1] = {1, KDL::PI / 2};
-
-    sols2[0] = {0, KDL::PI};
-    sols2[1] = {1, -KDL::PI / 2};
-
-    expected[0] = sols1;
-    expected[1] = sols2;
+    ScrewTheoryIkSubproblem::Solutions expected = {
+        {KDL::PI / 2, KDL::PI / 2},
+        {KDL::PI, -KDL::PI / 2}
+    };
 
     checkSolutions(actual, expected);
 
@@ -830,15 +759,61 @@ TEST_F(ScrewTheoryTest, PardosFour)
 
     KDL::Vector p3 = p + KDL::Vector(0.75, 0, 0);
     KDL::Vector k3 = k + KDL::Vector(-0.75, 0, -0.75);
-    PardosGotorFour pg4c(0, 1, exp1, exp2, p3);
+    PardosGotorFour pg4c(exp1, exp2, p3);
     KDL::Frame rhs3(k3 - p3);
     ASSERT_FALSE(pg4c.solve(rhs3, KDL::Frame::Identity(), actual));
 
-    sols1[0].second = sols2[0].second = 3 * KDL::PI / 4;
-    sols1[1].second = sols2[1].second = KDL::PI;
+    expected = {
+        {3 * KDL::PI / 4, KDL::PI},
+        {3 * KDL::PI / 4, KDL::PI}
+    };
+
+    checkSolutions(actual, expected);
+}
+
+TEST_F(ScrewTheoryTest, PardosGotorSix)
+{
+    KDL::Vector p(-1, 0, 0);
+    KDL::Vector k(2, 1, 2);
+
+    MatrixExponential exp1(MatrixExponential::ROTATION, KDL::Vector(0, 1, 0), KDL::Vector(2, 0, 0));
+    MatrixExponential exp2(MatrixExponential::ROTATION, KDL::Vector(0, 0, 1), KDL::Vector(0, 0, 0));
+    PardosGotorSix pg6(exp1, exp2, p);
+
+    ASSERT_EQ(pg6.solutions(), 1);
+
+    KDL::Frame rhs(k - p);
+    ScrewTheoryIkSubproblem::Solutions actual;
+    ASSERT_TRUE(pg6.solve(rhs, KDL::Frame::Identity(), actual));
+
+    ASSERT_EQ(actual.size(), 1);
+    ASSERT_EQ(actual[0].size(), 2);
+
+    ScrewTheoryIkSubproblem::Solutions expected = {{KDL::PI / 2, -KDL::PI / 2}};
+
+    checkSolutions(actual, expected);
+
+    KDL::Vector k2(2, 2, 2);
+    KDL::Frame rhs2(k2 - p);
+    ASSERT_FALSE(pg6.solve(rhs2, KDL::Frame::Identity(), actual));
+
+    checkSolutions(actual, expected);
+
+    KDL::Vector k3(2, 0, 0.25);
+    KDL::Frame rhs3(k3 - p);
+    ASSERT_FALSE(pg6.solve(rhs3, KDL::Frame::Identity(), actual));
+
+    expected = {{KDL::PI / 2, KDL::PI}};
+
+    checkSolutions(actual, expected);
+
+    KDL::Vector p4(-1, 0, 2);
+    KDL::Vector k4(2, 1, -2);
+    KDL::Frame rhs4(k4 - p4);
+    PardosGotorSix pg6d(exp1, exp2, p4);
+    ASSERT_FALSE(pg6d.solve(rhs4, KDL::Frame::Identity(), actual));
 
-    expected[0] = sols1;
-    expected[1] = sols2;
+    expected = {{-3 * KDL::PI / 4, -KDL::PI / 2}};
 
     checkSolutions(actual, expected);
 }