ScrewTheoryIkSubproblems.hpp

// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
 
#ifndef __SCREW_THEORY_IK_SUBPROBLEMS_HPP__
#define __SCREW_THEORY_IK_SUBPROBLEMS_HPP__
 
#include <kdl/frames.hpp>
 
#include "ScrewTheoryIkProblem.hpp"
#include "MatrixExponential.hpp"
 
namespace roboticslab
{
 
class PadenKahanOne : public ScrewTheoryIkSubproblem
{
public:
    using ScrewTheoryIkSubproblem::solve;
 
    PadenKahanOne(const MatrixExponential & exp, const KDL::Vector & p);
 
    bool solve(const KDL::Frame & rhs, const KDL::Frame & pointTransform, const JointConfig & reference, Solutions & solutions) const override;
 
    int solutions() const override
    { return 1; }
 
    const char * describe() const override
    { return "PK1"; }
 
private:
    const MatrixExponential exp;
    const KDL::Vector p;
    const KDL::Rotation axisPow;
};
 
class PadenKahanTwo : public ScrewTheoryIkSubproblem
{
public:
    using ScrewTheoryIkSubproblem::solve;
 
    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, const JointConfig & reference, Solutions & solutions) const override;
 
    int solutions() const override
    { return 2; }
 
    const char * describe() const override
    { return "PK2"; }
 
private:
    const MatrixExponential exp1, exp2;
    const KDL::Vector p, r, axesCross;
    const KDL::Rotation axisPow1, axisPow2;
    const double axesDot;
};
 
class PadenKahanThree : public ScrewTheoryIkSubproblem
{
public:
    using ScrewTheoryIkSubproblem::solve;
 
    PadenKahanThree(const MatrixExponential & exp, const KDL::Vector & p, const KDL::Vector & k);
 
    bool solve(const KDL::Frame & rhs, const KDL::Frame & pointTransform, const JointConfig & reference, Solutions & solutions) const override;
 
    int solutions() const override
    { return 2; }
 
    const char * describe() const override
    { return "PK3"; }
 
private:
    const MatrixExponential exp;
    const KDL::Vector p, k;
    const KDL::Rotation axisPow;
};
 
class PardosGotorOne : public ScrewTheoryIkSubproblem
{
public:
    using ScrewTheoryIkSubproblem::solve;
 
    PardosGotorOne(const MatrixExponential & exp, const KDL::Vector & p);
 
    bool solve(const KDL::Frame & rhs, const KDL::Frame & pointTransform, const JointConfig & reference, Solutions & solutions) const override;
 
    int solutions() const override
    { return 1; }
 
    const char * describe() const override
    { return "PG1"; }
 
private:
    const MatrixExponential exp;
    const KDL::Vector p;
};
 
class PardosGotorTwo : public ScrewTheoryIkSubproblem
{
public:
    using ScrewTheoryIkSubproblem::solve;
 
    PardosGotorTwo(const MatrixExponential & exp1, const MatrixExponential & exp2, const KDL::Vector & p);
 
    bool solve(const KDL::Frame & rhs, const KDL::Frame & pointTransform, const JointConfig & reference, Solutions & solutions) const override;
 
    int solutions() const override
    { return 1; }
 
    const char * describe() const override
    { return "PG2"; }
 
private:
    const MatrixExponential exp1, exp2;
    const KDL::Vector p, crossPr2;
    const double crossPr2Norm;
};
 
class PardosGotorThree : public ScrewTheoryIkSubproblem
{
public:
    using ScrewTheoryIkSubproblem::solve;
 
    PardosGotorThree(const MatrixExponential & exp, const KDL::Vector & p, const KDL::Vector & k);
 
    bool solve(const KDL::Frame & rhs, const KDL::Frame & pointTransform, const JointConfig & reference, Solutions & solutions) const override;
 
    int solutions() const override
    { return 2; }
 
    const char * describe() const override
    { return "PG3"; }
 
private:
    const MatrixExponential exp;
    const KDL::Vector p, k;
};
 
class PardosGotorFour : public ScrewTheoryIkSubproblem
{
public:
    using ScrewTheoryIkSubproblem::solve;
 
    PardosGotorFour(const MatrixExponential & exp1, const MatrixExponential & exp2, const KDL::Vector & p);
 
    bool solve(const KDL::Frame & rhs, const KDL::Frame & pointTransform, const JointConfig & reference, Solutions & solutions) const override;
 
    int solutions() const override
    { return 2; }
 
    const char * describe() const override
    { return "PG4"; }
 
private:
    const MatrixExponential exp1, exp2;
    const KDL::Vector p, n;
    const KDL::Rotation axisPow;
};
 
} // namespace roboticslab
 
#endif // __SCREW_THEORY_IK_SUBPROBLEMS_HPP__