Skip to content

Commit

Permalink
Place joint id out of solution vector and ctors
Browse files Browse the repository at this point in the history
  • Loading branch information
@PeterBowman
PeterBowman committed Dec 29, 2024
1 parent 6fdf066 commit 18d1cd6
Show file tree
Hide file tree
Showing 8 changed files with 141 additions and 288 deletions.
68 changes: 22 additions & 46 deletions libraries/ScrewTheoryLib/PadenKahanSubproblems.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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()))
{}
Expand All @@ -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;

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

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

Expand Down
78 changes: 24 additions & 54 deletions libraries/ScrewTheoryLib/PardosGotorSubproblems.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,37 +21,29 @@ 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)
{}

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

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

Expand All @@ -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)
{}
Expand All @@ -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();
Expand All @@ -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)),
Expand All @@ -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;

Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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);
}
Expand All @@ -240,15 +208,17 @@ 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;
}

Expand Down
Loading

0 comments on commit 18d1cd6

Please sign in to comment.