ScrewTheoryIkProblem.hpp

// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
 
#ifndef __SCREW_THEORY_IK_PROBLEM_HPP__
#define __SCREW_THEORY_IK_PROBLEM_HPP__
 
#include <utility>
#include <vector>
 
#include <kdl/frames.hpp>
#include <kdl/jntarray.hpp>
 
#include "ProductOfExponentials.hpp"
 
namespace roboticslab
{
 
class ScrewTheoryIkSubproblem
{
public:
    using JointConfig = std::vector<double>;
 
    using Solutions = std::vector<JointConfig>;
 
    virtual ~ScrewTheoryIkSubproblem() = default;
 
    virtual bool solve(const KDL::Frame & rhs, const KDL::Frame & pointTransform, const JointConfig & reference, Solutions & solutions) const = 0;
 
    virtual 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
    {
        return solve(rhs, pointTransform, reference, solutions);
    }
 
    virtual 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
    {
        return solve(rhs, pointTransform, reference, solutions);
    }
 
    bool solve(const KDL::Frame & rhs, const KDL::Frame & pointTransform, Solutions & _solutions) const
    {
        return solve(rhs, pointTransform, JointConfig(solutions()), _solutions);
    }
 
    virtual int solutions() const = 0;
 
    virtual const char * describe() const = 0;
};
 
class ScrewTheoryIkProblem final
{
public:
    using JointIdsToSubproblem = std::pair<std::vector<int>, const ScrewTheoryIkSubproblem *>;
 
    using Steps = std::vector<JointIdsToSubproblem>;
 
    using Solutions = std::vector<KDL::JntArray>;
 
    ~ScrewTheoryIkProblem();
 
    // disable these, avoid issues related to dynamic alloc
    ScrewTheoryIkProblem(const ScrewTheoryIkProblem &) = delete;
    ScrewTheoryIkProblem & operator=(const ScrewTheoryIkProblem &) = delete;
 
    std::vector<bool> solve(const KDL::Frame & H_S_T, const KDL::JntArray & reference, Solutions & solutions);
 
    std::vector<bool> solve(const KDL::Frame & H_S_T, Solutions & solutions)
    {
        return solve(H_S_T, KDL::JntArray(poe.size()), solutions);
    }
 
    int solutions() const
    { return soln; }
 
    const Steps & getSteps() const
    { return steps; }
 
    bool isReversed() const
    { return reversed; }
 
    static ScrewTheoryIkProblem * create(const PoeExpression & poe, const Steps & steps, bool reversed = false);
 
private:
    enum poe_term
    {
        EXP_KNOWN,
        EXP_COMPUTED,
        EXP_UNKNOWN
    };
 
    using Frames = std::vector<KDL::Frame>;
    using PoeTerms = std::vector<poe_term>;
 
    // disable instantiation, force users to call builder class
    ScrewTheoryIkProblem(const PoeExpression & poe, const Steps & steps, bool reversed);
 
    void recalculateFrames(const Solutions & solutions, Frames & frames, PoeTerms & poeTerms);
    bool recalculateFrames(const Solutions & solutions, Frames & frames, PoeTerms & poeTerms, bool backwards);
 
    KDL::Frame transformPoint(const KDL::JntArray & jointValues, const PoeTerms & poeTerms);
 
    const PoeExpression poe;
 
    // we own these, resources freed in destructor
    const Steps steps;
 
    PoeTerms poeTerms;
    Frames rhsFrames;
    std::vector<bool> reachability;
 
    const bool reversed;
    const int soln;
};
 
class ScrewTheoryIkProblemBuilder
{
public:
    struct PoeTerm
    {
        PoeTerm() : known(false), simplified(false) {}
        bool known, simplified;
    };
 
    ScrewTheoryIkProblemBuilder(const PoeExpression & poe);
 
    ScrewTheoryIkProblem * build();
 
private:
    static std::vector<KDL::Vector> searchPoints(const PoeExpression & poe);
 
    ScrewTheoryIkProblem::Steps searchSolutions();
 
    void refreshSimplificationState();
 
    void simplify(int depth);
    void simplifyWithPadenKahanOne(const KDL::Vector & point);
    void simplifyWithPadenKahanThree(const KDL::Vector & point);
    void simplifyWithPardosOne();
    void simplifyWithPardosFive();
    bool simplifyWithPardosThree(MatrixExponential & exp1, MatrixExponential & exp2, KDL::Vector & point);
 
    ScrewTheoryIkProblem::JointIdsToSubproblem trySolve(int depth);
 
    PoeExpression poe;
 
    std::vector<KDL::Vector> points;
    std::vector<KDL::Vector> testPoints;
 
    std::vector<PoeTerm> poeTerms;
 
    static const int MAX_SIMPLIFICATION_DEPTH = 2;
};
 
} // namespace roboticslab
 
#endif // __SCREW_THEORY_IK_PROBLEM_HPP__