FingerController.h

Go to the documentation of this file.

#ifndef FINGERCONTROLLER_H
#define FINGERCONTROLLER_H

#include "nasa_common_logging/Logger.h"
#include <eigen3/Eigen/Dense>
#include "robodyn_mechanisms/FingerKinematics.h"
#include <boost/array.hpp>
#include "robodyn_utilities/MultiLoopController.h"
#include "robodyn_mechanisms/HallsToAngle.h"

template<unsigned int N, unsigned int NHall = N>
class FingerController
{
public:
    typedef typename FingerKinematics<N>::ReferenceMatrixType   ReferenceMatrixType;
    typedef typename FingerKinematics<N>::JointVectorType       JointVectorType;
    typedef typename FingerKinematics<N>::SliderVectorType      SliderVectorType;
    typedef typename FingerKinematics<N>::JacobianType          JacobianType;
    typedef Eigen::Matrix<double, NHall, 1> HallVectorType;

    typedef boost::array < MultiLoopController, N + 1 >              MultiLoopVectorType;

    FingerController();
    virtual ~FingerController() {}

    void setReferenceMatrix(const ReferenceMatrixType& refMatrix_in)
    {
        kinematics.setReferenceMatrix(refMatrix_in);
    }

    void reset();
    void getDesiredSliders(const JointVectorType& desiredJoints, const SliderVectorType& actualSliders, SliderVectorType& desiredSliders);
    void getDesiredSlidersStiffness(const JointVectorType& desiredPos, const JointVectorType& actualJoints, const SliderVectorType& actualSliders, const SliderVectorType& actualTensions, const SliderVectorType& sliderVelocities, const bool useCartesian, const bool isLeft, SliderVectorType& desiredSliders);
    void getMotComs(const SliderVectorType& desiredSliders, const SliderVectorType& kTendon, const SliderVectorType& actualSliders, const SliderVectorType& actualTensions, const SliderVectorType& sliderVelocities, SliderVectorType& sliderMotComs, SliderVectorType& desiredSliderVels, const SliderVectorType& fingerBoostMotCom);

    // set parameters
    void setMillimetersPerCount(double mmPerCount_in) {mmPerCount = mmPerCount_in;}
    void setHallAngleParameters(double scale, const HallVectorType& coeffs0, const HallVectorType& coeffs1, const HallVectorType& coeffs2, const HallVectorType& coeffs3);
    void setTensionParameters(const SliderVectorType& aGains, const SliderVectorType& bGains, const SliderVectorType& offsets, const SliderVectorType& calibratedStrain_in);
    void setTubeTareParameters(const SliderVectorType& sliderOffsets_in,
                               const SliderVectorType& sliderTarePos_in, const SliderVectorType& tensionOffsets_in);
    void setStiffnessGains(const JointVectorType& K_in, const double kd_in, const double fmin_in, const double fmax_in, const JointVectorType& jointKp_in, const double tensionKp_in, const double tFreq, const double deadband_in);
    void setMultiLoopController(const MultiLoopController& mlc_in, unsigned int controllerIndex);
    void setBusVoltage(double busVoltage_in) {busVoltage = busVoltage_in;}

    void getHallAngles(const HallVectorType& halls_in, HallVectorType& jointAngles);
    void getCalibratedTensions(const SliderVectorType& tensionA, const SliderVectorType& tensionB, double timestep, SliderVectorType& tensionsCalibrated);
    void getXYPos(const JointVectorType& jointAngles, const bool isLeft, JointVectorType& xy, JointVectorType& desiredTorques);

    inline void getSlidersFromJoints(const JointVectorType& jointAngles, SliderVectorType& sliderPositions)
    {
        kinematics.inverse(jointAngles, sliderPositions);
    }

    inline void getSlidersFromEncoders(const SliderVectorType& encoderPositions, SliderVectorType& sliderPositions)
    {
        sliderPositions = ((encoderPositions - sliderOffsets) * mmPerCount + sliderTarePos) ;
    }

    inline void getJointsFromSliders(const SliderVectorType& sliderPositions, JointVectorType& jointAngles)
    {
        kinematics.forward(sliderPositions, jointAngles);
    }

private:
    FingerKinematics<N> kinematics;
    double              mmPerCount;
    double              hallScale;
    HallVectorType      hallCoeffs0;
    HallVectorType      hallCoeffs1;
    HallVectorType      hallCoeffs2;
    HallVectorType      hallCoeffs3;
    SliderVectorType    tensionAGains;
    SliderVectorType    tensionBGains;
    SliderVectorType    tensionSensorCalOffsets;
    SliderVectorType    calibratedStrain;
    SliderVectorType    sliderOffsets;
    SliderVectorType    sliderTarePos;
    SliderVectorType    tensionOffsets;
    MultiLoopVectorType multiLoopControllers;
    double              smoothingHz;
    SliderVectorType    smoothingFactors;
    double              busVoltage;
    JointVectorType     K;
    double              kd;
    double              fmin;
    double              fmax;
    JointVectorType     jointKp;
    double              tensionKp;
    SliderVectorType    tensionsPrev;
    double              tensionFilterCutoffFreq;
    double              deadbandSize;
    JointVectorType     desTorques;
};

template<unsigned int N, unsigned int NHall>
FingerController<N, NHall>::FingerController()
    : mmPerCount(0.0174427)
    , hallScale(0.)
    , hallCoeffs0(HallVectorType::Zero())
    , hallCoeffs1(HallVectorType::Zero())
    , hallCoeffs2(HallVectorType::Zero())
    , hallCoeffs3(HallVectorType::Zero())
    , tensionAGains(SliderVectorType::Zero())
    , tensionBGains(SliderVectorType::Zero())
    , tensionSensorCalOffsets(SliderVectorType::Zero())
    , calibratedStrain(SliderVectorType::Zero())
    , sliderOffsets(SliderVectorType::Zero())
    , sliderTarePos(SliderVectorType::Zero())
    , tensionOffsets(SliderVectorType::Zero())
    , busVoltage(48)
    , K(JointVectorType::Zero())
    , kd(0.01)
    , fmin(1.0)
    , fmax(15.0)
    , jointKp(JointVectorType::Zero())
    , tensionKp(0.003)
    , tensionsPrev(SliderVectorType::Zero())
    , tensionFilterCutoffFreq(20.)
    , deadbandSize(0.5)
    , desTorques(JointVectorType::Zero())
{
    reset();
}

template<unsigned int N, unsigned int NHall>
void FingerController<N, NHall>::setHallAngleParameters(double scale, const HallVectorType& coeffs0, const HallVectorType& coeffs1, const HallVectorType& coeffs2, const HallVectorType& coeffs3)
{
    hallScale = scale;
    hallCoeffs0 = coeffs0;
    hallCoeffs1 = coeffs1;
    hallCoeffs2 = coeffs2;
    hallCoeffs3 = coeffs3;
}

template<unsigned int N, unsigned int NHall>
void FingerController<N, NHall>::setTensionParameters(const SliderVectorType& aGains, const SliderVectorType& bGains, const SliderVectorType& offsets, const SliderVectorType& calibratedStrain_in)
{
    tensionAGains = aGains;
    tensionBGains = bGains;
    tensionSensorCalOffsets = offsets;
    calibratedStrain = calibratedStrain_in;
}

template<unsigned int N, unsigned int NHall>
void FingerController<N, NHall>::setTubeTareParameters(const SliderVectorType& sliderOffsets_in,
        const SliderVectorType& sliderTarePos_in, const SliderVectorType& tensionOffsets_in)
{
    sliderOffsets = sliderOffsets_in;
    tensionOffsets = tensionOffsets_in;

    double roundedSlider;

    for (unsigned int i = 0; i < sliderTarePos_in.size(); ++i)
    {
        roundedSlider = fmod(sliderTarePos_in[i], mmPerCount);

        if (roundedSlider < 0.5)
        {
            roundedSlider = sliderTarePos_in[i] - roundedSlider;
        }
        else
        {
            roundedSlider = sliderTarePos_in[i] + mmPerCount - roundedSlider;
        }

        sliderTarePos[i] = roundedSlider;
    }

}

template<unsigned int N, unsigned int NHall>
void FingerController<N, NHall>::setMultiLoopController(const MultiLoopController& mlc_in, unsigned int controllerIndex)
{
    if (controllerIndex > N)
    {
        std::stringstream err;
        err << "setMultiLoopController() controllerIndex out of range" << std::endl;
        NasaCommonLogging::Logger::log("gov.nasa.controllers.FingerController", log4cpp::Priority::ERROR, err.str());
        throw std::runtime_error(err.str());
        return;
    }

    multiLoopControllers[controllerIndex] = mlc_in;
}

template<unsigned int N, unsigned int NHall>
void FingerController<N, NHall>::setStiffnessGains(const JointVectorType& K_in, const double kd_in, const double fmin_in, const double fmax_in, const JointVectorType& jointKp_in, const double tensionKp_in, const double tFreq, const double deadband_in)
{
    K = K_in;
    kd = kd_in;
    fmin = fmin_in;
    fmax = fmax_in;
    jointKp = jointKp_in;
    tensionKp = tensionKp_in;
    tensionFilterCutoffFreq = tFreq;
    deadbandSize = deadband_in;
}

template<unsigned int N, unsigned int NHall>
void FingerController<N, NHall>::reset()
{
    for (unsigned int i = 0; i < multiLoopControllers.size(); ++i)
    {
        multiLoopControllers[i].reset();
    }
}


//Position control
template<unsigned int N, unsigned int NHall>
void FingerController<N, NHall>::getDesiredSliders(const JointVectorType& desiredJoints, const SliderVectorType& actualSliders, SliderVectorType& desiredSliders)
{
    SliderVectorType rsSliders;
    getSlidersFromJoints(desiredJoints, desiredSliders);

    // limit desired
    kinematics.projectToRangeSpace(actualSliders, rsSliders);

    for (int i = 0; i < desiredSliders.rows(); ++i)
    {
        if (desiredSliders[i] > rsSliders[i] + 2.54)
        {
            desiredSliders[i] = rsSliders[i] + 2.54;
        }
        else if (desiredSliders[i] < rsSliders[i] - 2.54)
        {
            desiredSliders[i] = rsSliders[i] - 2.54;
        }
    }
}

//Stiffness Control
template<unsigned int N, unsigned int NHall>
void FingerController<N, NHall>::getDesiredSlidersStiffness(const JointVectorType& desiredPos, const JointVectorType& actualJoints, const SliderVectorType& actualSliders, const SliderVectorType& actualTensions, const SliderVectorType& sliderVelocities, const bool useCartesian, const bool isLeft, SliderVectorType& desiredSliders)
{
    //Needed variables -> K, kd, fmin, fmax, jointKp, tensionKp

    //find actual torques from tension sensors
    JointVectorType actualTorques;
    kinematics.tensionToJointTorque(actualTensions, actualTorques);

    JointVectorType desiredTorques;

    if (useCartesian && actualSliders.rows() >= 4)      //limits operation to thumb and primaries (secondaries only have 3 sliders/actuators)
    {
        //find current XY position
        JointVectorType xyActual;
        kinematics.jointToCartesian(actualJoints, isLeft, xyActual);

        //calculate Jacobian
        JacobianType jacobian;
        kinematics.calcJacobian(actualJoints, xyActual, isLeft, jacobian);

        //limit desired positions to be within reach
        JointVectorType desiredForces = desiredPos;

        if (desiredForces.rows() == 3) //limit primary fingers
        {
            if (desiredForces[2] < 0)
            {
                desiredForces[2] = 0;
            }

            double dist = sqrt(desiredForces[1] * desiredForces[1] + desiredForces[2] * desiredForces[2]);

            if (dist > 90.0)
            {
                desiredForces.tail(2) *= 90.0 / dist;
            }
        }

        desiredForces = K.asDiagonal() * (desiredForces - xyActual);

        kinematics.forceToJointTorque(jacobian, desiredForces, desiredTorques);

        //add in repulsive torque to avoid singularity
        if (actualJoints.rows() == 3 && actualJoints[2] < 0.5)
        {
            desiredTorques[2] += 500.0 * (0.5 - actualJoints[2]);
        }

        //keep thumbs away from joint limits
        if (actualJoints.rows() == 4)                   //selects thumb (primaries only have 3 joints defined)
        {
            if (actualJoints[0] > -0.3)
            {
                desiredTorques[0] -= 500.0 * (actualJoints[0] + 0.3);
            }
            else if (actualJoints[0] < -1.4)
            {
                desiredTorques[0] += 500.0 * (-1.4 - actualJoints[0]);
            }

            if (actualJoints[1] < 0.2)
            {
                desiredTorques[1] += 500.0 * (0.2 - actualJoints[1]);
            }
            else if (actualJoints[1] > 1.4)
            {
                desiredTorques[1] -= 500.0 * (actualJoints[1] - 1.4);
            }

            if (actualJoints[2] < 0.5)
            {
                desiredTorques[2] += 500.0 * (0.5 - actualJoints[2]);
            }
            else if (actualJoints[2] > 1.4)
            {
                desiredTorques[2] -= 500.0 * (actualJoints[2] - 1.4);
            }
        }
    }
    else
    {
        desiredTorques = K.asDiagonal() * (desiredPos - actualJoints);
    }

    desTorques = desiredTorques;

    //find internal tension and scale desired torques if necessary
    double tensErr = kinematics.findDesiredInternalTension(fmin, fmax, desiredTorques);
    tensErr -= kinematics.calculateInternalTension(actualTensions);

    //subtract to make desiredTorques into torqueError to use in feedback
    desiredTorques -= actualTorques;
    //compute force feedback term -> xd = P^T K_p * torqueError
    kinematics.jointSpaceTorqueFeedback(desiredTorques, jointKp, tensErr, tensionKp, desiredSliders);

    //add in slider position and velocity terms
    desiredSliders += actualSliders - kd * sliderVelocities;

    //deadband to reduce vibrations
    for (unsigned int i = 0; i < desiredSliders.rows(); ++i)
        if (desiredSliders[i] < actualSliders[i] + deadbandSize && desiredSliders[i] > actualSliders[i] - deadbandSize)
        {
            desiredSliders[i] = actualSliders[i];
        }
}


template<unsigned int N, unsigned int NHall>
void FingerController<N, NHall>::getMotComs(const SliderVectorType& desiredSliders, const SliderVectorType& kTendon, const SliderVectorType& actualSliders, const SliderVectorType& actualTensions, const SliderVectorType& sliderVelocities, SliderVectorType& sliderMotComs, SliderVectorType& desiredSliderVels, const SliderVectorType& fingerBoostMotCom)
{
    // get motcoms
    double roundedSlider;

    for (unsigned int i = 0; i < multiLoopControllers.size(); ++i)
    {
        roundedSlider = fmod(desiredSliders[i], mmPerCount);

        if (roundedSlider < 0.5)
        {
            roundedSlider = desiredSliders[i] - roundedSlider;
        }
        else
        {
            roundedSlider = desiredSliders[i] + mmPerCount - roundedSlider;
        }

        if (fingerBoostMotCom[i] != 0)
        {
            sliderMotComs[i] = fingerBoostMotCom[i];
        }
        else
        {
            sliderMotComs[i] = multiLoopControllers[i].update(roundedSlider, kTendon[i], actualSliders[i], actualTensions[i], sliderVelocities[i], busVoltage, desiredSliderVels[i]);
        }
    }
}

template<unsigned int N, unsigned int NHall>
void FingerController<N, NHall>::getHallAngles(const HallVectorType& halls_in, HallVectorType& jointAngles)
{
    for (int i = 0; i < halls_in.rows(); ++i)
    {
        jointAngles[i] = HallsToAngle::getAngleFromHalls(halls_in[i], hallCoeffs0[i], hallCoeffs1[i], hallCoeffs2[i], hallCoeffs3[i], hallScale);
    }

    // Calculate distals from medial if not controlled
    // @todo calibrate raw distal halls to this equation RDEV-2334
    if (N == 2 && NHall == 6)
    {
        jointAngles[2 * N] = HallsToAngle::getAngleFromHalls(jointAngles[N], -0.01376, 0.005286, 0.00006108, -1.57e-7, 1.0);
        jointAngles[2 * N + 1] = HallsToAngle::getAngleFromHalls(jointAngles[N + 1], -0.01376, 0.005286, 0.00006108, -1.57e-7, 1.0);
    }
    else if (N == 3 && NHall == 4)
    {
        jointAngles[N] = HallsToAngle::getAngleFromHalls(jointAngles[N - 1], -0.01376, 0.005286, 0.00006108, -1.57e-7, 1.0);
    }
}

template<unsigned int N, unsigned int NHall>
void FingerController<N, NHall>::getCalibratedTensions(const SliderVectorType& tensionA, const SliderVectorType& tensionB, double timestep, SliderVectorType& tensionsCalibrated)
{
    for (int i = 0; i < tensionA.size(); i++)
    {
        double temp;
        temp = tensionA[i] * tensionAGains[i] + tensionB[i] * tensionBGains[i] + tensionSensorCalOffsets[i];
        tensionsCalibrated[i] = calibratedStrain[i] * (temp - tensionOffsets[i]);

        if (tensionsCalibrated[i] < 0)
        {
            tensionsCalibrated[i] = 0;
        }

        if (tensionFilterCutoffFreq != 0 && timestep != 0.)
        {
            //first-order filter
            temp = tensionFilterCutoffFreq * (tensionsCalibrated[i] - tensionsPrev[i]);
            tensionsCalibrated[i] = tensionsPrev[i] + temp * timestep;
            tensionsPrev[i] = tensionsCalibrated[i];
        }
    }
}

template<unsigned int N, unsigned int NHall>
void FingerController<N, NHall>::getXYPos(const JointVectorType& jointAngles, const bool isLeft, JointVectorType& xy, JointVectorType& desiredTorques)
{
    kinematics.jointToCartesian(jointAngles, isLeft, xy);
    desiredTorques = desTorques;
}

#endif