ForwardArmKinematics.h

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

template <unsigned int D, unsigned int N>
class ForwardArmKinematics {
public:
    virtual ~ForwardArmKinematics() = default;

    constexpr unsigned int getNumSegments() const {
        return N;
    }

    constexpr unsigned int getNumDimensions() const {
        return D;
    }

    virtual navtypes::Vectord<N> getSegLens() const = 0;

    virtual bool satisfiesConstraints(const navtypes::Vectord<N>& jointPos) const = 0;

    virtual navtypes::Matrixd<D, N>
    getJacobian(const navtypes::Vectord<N>& jointPos) const = 0;

    virtual navtypes::Vectord<D>
    jointPosToEEPos(const navtypes::Vectord<N>& jointPos) const = 0;

    navtypes::Vectord<D> jointVelToEEVel(const navtypes::Vectord<N>& jointPos,
                                         const navtypes::Vectord<N>& jointVel) const {
        return getJacobian(jointPos) * jointVel;
    }

    navtypes::Vectord<N> eeVelToJointVel(const navtypes::Vectord<N>& jointPos,
                                         const Eigen::Vector2d& eeVel) const {
        navtypes::Matrixd<2, N> jacobian = getJacobian(jointPos);
        return jacobian.colPivHouseholderQR().solve(eeVel);
    }
};
 
} // namespace kinematics