JacobianVelController.h

#pragma once
 
#include "../filters/StateSpaceUtil.h"
#include "../navtypes.h"
#include "../utils/math.h"
#include "../utils/time.h"
#include "../world_interface/data.h"
 
#include <functional>
#include <loguru.hpp>
#include <optional>
 
#include <Eigen/Core>

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

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

    navtypes::Vectord<inputDim> getCommand(robot::types::datatime_t currTime,
                                           const navtypes::Vectord<inputDim>& currPos,
                                           double& cosineSim) {
        if (!controllerState) {
            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
        // This is similar to JacobianController, but the command position is calculated
        // from a velocity instead of a profile.
 
        navtypes::Vectord<outputDim> currPosOutput = kinematicsFunc(currPos);
        state_t state = controllerState.value();
        double dt = util::durationToSec(currTime - state.timestamp);
        navtypes::Vectord<outputDim> lastTarget = state.lastTarget.value_or(currPosOutput);
        navtypes::Vectord<outputDim> currTarget = lastTarget + dt * state.targetVel;
        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());
 
        state_t newState = {
            .timestamp = currTime, .targetVel = state.targetVel, .lastTarget = currTarget};
        controllerState = newState;
 
        return currPos + inputPosDiff;
    }

    void setTarget(robot::types::datatime_t currTime,
                   const navtypes::Vectord<outputDim>& targetVel) {
        state_t state = {.timestamp = currTime, .targetVel = targetVel, .lastTarget = {}};
        this->controllerState = state;
    }

    bool hasTarget() {
        return controllerState.has_value();
    }

    void reset() {
        controllerState.reset();
    }
 
private:
    struct state_t {
        robot::types::datatime_t timestamp;
        navtypes::Vectord<outputDim> targetVel;
        std::optional<navtypes::Vectord<outputDim>> lastTarget;
    };
 
    std::function<navtypes::Vectord<outputDim>(const navtypes::Vectord<inputDim>&)>
        kinematicsFunc;
    std::function<navtypes::Matrixd<outputDim, inputDim>(const navtypes::Vectord<inputDim>&)>
        jacobianFunc;
    std::optional<state_t> controllerState;
};