InverseArmKinematics.h

#pragma once
 
#include "../navtypes.h"
#include "ForwardArmKinematics.h"
 
#include <memory>
 
#include <Eigen/Core>
 
namespace kinematics {

template <unsigned int D, unsigned int N>
class InverseArmKinematics {
public:
    InverseArmKinematics(std::shared_ptr<const ForwardArmKinematics<D, N>> fk) : fk(fk) {}
 
    virtual ~InverseArmKinematics() = default;

    navtypes::Vectord<N> eePosToJointPos(const navtypes::Vectord<D>& eePos,
                                         const navtypes::Vectord<N>& currJointPos) const {
        bool success;
        return eePosToJointPos(eePos, currJointPos, success);
    }

    virtual navtypes::Vectord<N> eePosToJointPos(const navtypes::Vectord<D>& eePos,
                                                 const navtypes::Vectord<N>& jointAngles,
                                                 bool& success) const {
        double armLen = fk->getSegLens().sum();
        if (eePos.norm() >= armLen) {
            success = false;
            return jointAngles;
        }
        navtypes::Vectord<N> sol = solve(eePos, jointAngles, success);
        if (success) {
            success = fk->satisfiesConstraints(sol);
        }
        return success ? sol : jointAngles;
    }
 
protected:
    const std::shared_ptr<const ForwardArmKinematics<D, N>> fk;

    virtual navtypes::Vectord<N> solve(const navtypes::Vectord<D>& eePos,
                                       const navtypes::Vectord<N>& jointAngles,
                                       bool& success) const = 0;
};
 
} // namespace kinematics