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;
};
 
class PardosGotorFive : public ScrewTheoryIkSubproblem
{
public:
    using ScrewTheoryIkSubproblem::solve;
 
    PardosGotorFive(const MatrixExponential & exp, const MatrixExponential & exp_next, 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 "PG5"; }
 
private:
    const MatrixExponential exp;
    const MatrixExponential exp_next;
    const KDL::Vector p;
    const KDL::Rotation axisPow;
};
 
class PardosGotorSix : public ScrewTheoryIkSubproblem
{
public:
    using ScrewTheoryIkSubproblem::solve;
    PardosGotorSix(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 "PG6"; }
 
private:
    const MatrixExponential exp1, exp2;
    const KDL::Vector p, axesCross;
    const KDL::Rotation axisPow1, axisPow2;
    const double axesDot;
};
 
class PardosGotorSeven : public ScrewTheoryIkSubproblem
{
public:
    using ScrewTheoryIkSubproblem::solve;
 
    PardosGotorSeven(const MatrixExponential & exp1, const MatrixExponential & exp2, const MatrixExponential & exp3, 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 4; }
 
    const char * describe() const override
    { return "PG7"; }
 
private:
    const MatrixExponential exp1, exp2, exp3;
    const KDL::Vector p, axesCross, axesCross_inverted;
    const KDL::Rotation axisPow1, axisPow2;
    const double axesDot;
};
 
class PardosGotorEight : public ScrewTheoryIkSubproblem
{
public:
    using ScrewTheoryIkSubproblem::solve;
 
    PardosGotorEight(const MatrixExponential & exp1, const MatrixExponential & exp2, const MatrixExponential & exp3, const KDL::Vector & p, const int firstID, const int lastId, const PoeExpression poe);
 
 
    bool solve(const KDL::Frame & rhs, const KDL::Frame & pointTransform, const JointConfig & reference, Solutions & solutions, const KDL::Frame & H_S_T, const KDL::JntArray & c_solutions, const KDL::Frame & H_S_T_0) const override;
 
    bool solve(const KDL::Frame & rhs, const KDL::Frame & pointTransform, const JointConfig & reference, Solutions & solutions) const override
    {
        return false;
    }
    int solutions() const override
    { return 2; }//two sets of triple solutions
 
    const char * describe() const override
    { return "PG8"; }
 
private:
    const MatrixExponential exp1, exp2, exp3;
    const KDL::Vector p, n;
    const KDL::Rotation axisPow;
    const int firstID, lastID;
    const PoeExpression poe;
};
 
class PardosGotorThreePadenKahanOne : public ScrewTheoryIkSubproblem
{
public:
    using ScrewTheoryIkSubproblem::solve;
 
    PardosGotorThreePadenKahanOne(const MatrixExponential & exp, const MatrixExponential & exp_pk1, 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+PK1"; }
 
private:
    const MatrixExponential exp_pg3, exp_pk1;
    const KDL::Vector p, k;
    const KDL::Rotation axisPow;
};
 
class Algebraic_UR : public ScrewTheoryIkSubproblem
{
public:
    using ScrewTheoryIkSubproblem::solve;
 
    Algebraic_UR(int j1, int j2);
 
    bool solve(const KDL::Frame & rhs, const KDL::Frame & pointTransform, const JointConfig & reference, Solutions & solutions, const KDL::Frame & H_S_T, const KDL::JntArray & c_solutions) const override;
 
    bool solve(const KDL::Frame & rhs, const KDL::Frame & pointTransform, const JointConfig & reference, Solutions & solutions, const KDL::Frame & H_S_T, const KDL::JntArray & c_solutions, const KDL::Frame & H_S_T_0) const override
    {
        return solve(rhs, pointTransform, reference, solutions, H_S_T, c_solutions);
    }
 
    bool solve(const KDL::Frame & rhs, const KDL::Frame & pointTransform, const JointConfig & reference, Solutions & solutions) const override
    {
        return false;
    }
 
    int solutions() const override
    { return 1; }
 
    const char * describe() const override
    { return "Algebraic"; }
 
private:
    const int j1, j2;
};
 
} // namespace roboticslab
 
#endif // __SCREW_THEORY_IK_SUBPROBLEMS_HPP__