Skip to content

Commit

Permalink
Implement PG6
Browse files Browse the repository at this point in the history
  • Loading branch information
@PeterBowman
PeterBowman committed Dec 29, 2024
1 parent 18d1cd6 commit 59f6297
Show file tree
Hide file tree
Showing 3 changed files with 196 additions and 5 deletions.
109 changes: 109 additions & 0 deletions libraries/ScrewTheoryLib/PardosGotorSubproblems.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -223,3 +223,112 @@ bool PardosGotorFour::solve(const KDL::Frame & rhs, const KDL::Frame & pointTran
}

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

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;
}

// -----------------------------------------------------------------------------
39 changes: 38 additions & 1 deletion libraries/ScrewTheoryLib/ScrewTheoryIkSubproblems.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -229,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
Expand Down Expand Up @@ -258,6 +258,43 @@ class PardosGotorFour : public ScrewTheoryIkSubproblem
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__
53 changes: 49 additions & 4 deletions tests/testScrewTheory.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -643,7 +643,7 @@ TEST_F(ScrewTheoryTest, PadenKahanThree)
checkSolutions(actual, expected);
}

TEST_F(ScrewTheoryTest, PardosOne)
TEST_F(ScrewTheoryTest, PardosGotorOne)
{
KDL::Vector p(1, 0, 0);
KDL::Vector k(1, 1, 0);
Expand All @@ -665,7 +665,7 @@ TEST_F(ScrewTheoryTest, PardosOne)
checkSolutions(actual, expected);
}

TEST_F(ScrewTheoryTest, PardosTwo)
TEST_F(ScrewTheoryTest, PardosGotorTwo)
{
KDL::Vector p(1, 1, 0);
KDL::Vector k(2, 3, 0);
Expand All @@ -688,7 +688,7 @@ TEST_F(ScrewTheoryTest, PardosTwo)
checkSolutions(actual, expected);
}

TEST_F(ScrewTheoryTest, PardosThree)
TEST_F(ScrewTheoryTest, PardosGotorThree)
{
KDL::Vector p(1, 0, 0);
KDL::Vector k(1, 2, 0);
Expand Down Expand Up @@ -725,7 +725,7 @@ TEST_F(ScrewTheoryTest, PardosThree)
checkSolutions(actual, expected);
}

TEST_F(ScrewTheoryTest, PardosFour)
TEST_F(ScrewTheoryTest, PardosGotorFour)
{
KDL::Vector p(0, 1, 0);
KDL::Vector k(3, 1, 1);
Expand Down Expand Up @@ -771,6 +771,51 @@ TEST_F(ScrewTheoryTest, PardosFour)
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 k4(2, 2, 0.25);
KDL::Frame rhs4(k4 - p);
ASSERT_FALSE(pg6.solve(rhs4, KDL::Frame::Identity(), actual));

expected = {{KDL::PI / 2, -KDL::PI / 2}};

checkSolutions(actual, expected);
}

TEST_F(ScrewTheoryTest, AbbIrb120Kinematics)
{
KDL::Chain chain = makeAbbIrb120KinematicsFromDH();
Expand Down

0 comments on commit 59f6297

Please sign in to comment.