JacobianPosController.h

#pragma once
 
#include "../filters/StateSpaceUtil.h"
#include "../navtypes.h"
#include "../utils/math.h"
#include "../world_interface/data.h"
#include "TrapezoidalVelocityProfile.h"
 
#include <array>
#include <functional>
#include <loguru.hpp>
#include <optional>
 
#include <Eigen/Core>
 
namespace control {

template <int outputDim, int inputDim>
class JacobianPosController {
public:
    JacobianPosController(
        const std::function<navtypes::Vectord<outputDim>(const navtypes::Vectord<inputDim>&)>&
            kinematicsFunc,
        const std::function<navtypes::Matrixd<outputDim, inputDim>(
            const navtypes::Vectord<inputDim>&)>& jacobianFunc,
        double maxVel, double maxAccel)
        : kinematicsFunc(kinematicsFunc),
          jacobianFunc(jacobianFunc ? jacobianFunc
                                    : std::bind(util::numericalJacobian, kinematicsFunc,
                                                std::placeholders::_1, outputDim)),
          velocityProfile(maxVel, maxAccel) {
        CHECK_NOTNULL_F(this->kinematicsFunc);
        CHECK_NOTNULL_F(this->jacobianFunc);
    }

    navtypes::Vectord<inputDim> getCommand(robot::types::datatime_t currTime,
                                           const navtypes::Vectord<inputDim>& currPos) const {
        double unused;
        return getCommand(currTime, currPos, unused);
    }

    navtypes::Vectord<inputDim> getCommand(robot::types::datatime_t currTime,
                                           const navtypes::Vectord<inputDim>& currPos,
                                           double& cosineSim) const {
        if (!velocityProfile.hasTarget()) {
            return currPos;
        }
 
        // The jacobian of the kinematics function represents a linearization of the kinematics
        // around the current position. We then use this approximation to find the position
        // delta in the input space that yields the required position delta in the output space
 
        navtypes::Vectord<outputDim> currTarget = velocityProfile.getCommand(currTime);
        navtypes::Vectord<outputDim> currPosOutput = kinematicsFunc(currPos);
        navtypes::Vectord<outputDim> outputPosDiff = currTarget - currPosOutput;
        navtypes::Matrixd<outputDim, inputDim> jacobian = jacobianFunc(currPos);
        navtypes::Vectord<inputDim> inputPosDiff =
            jacobian.colPivHouseholderQr().solve(outputPosDiff);
 
        // the cosine similarity of the actual and required position deltas in the output space
        // is metric of how much the kinematics allows the commanded movement.
        navtypes::Vectord<outputDim> trueOutputPosDiff = jacobian * inputPosDiff;
        cosineSim = outputPosDiff.dot(trueOutputPosDiff) /
                    (outputPosDiff.norm() * trueOutputPosDiff.norm());
 
        return currPos + inputPosDiff;
    }

    void setTarget(robot::types::datatime_t currTime,
                   const navtypes::Vectord<inputDim>& currPos,
                   const navtypes::Vectord<outputDim>& target) {
        navtypes::Vectord<outputDim> currPosOutput = kinematicsFunc(currPos);
        velocityProfile.setTarget(currTime, currPosOutput, target);
    }

    bool hasTarget() const {
        return velocityProfile.hasTarget();
    }

    void reset() {
        velocityProfile.reset();
    }
 
private:
    std::function<navtypes::Vectord<outputDim>(const navtypes::Vectord<inputDim>&)>
        kinematicsFunc;
    std::function<Eigen::Matrix<double, outputDim, inputDim>(
        const Eigen::Matrix<double, inputDim, 1>&)>
        jacobianFunc;
    TrapezoidalVelocityProfile<outputDim> velocityProfile;
};
 
} // namespace control