JointCommandFinger.h

Go to the documentation of this file.

#ifndef JOINTCOMMANDFINGER_H
#define JOINTCOMMANDFINGER_H

#include "robodyn_mechanisms/JointCommandInterface.h"
#include "robot_instance/StringUtilities.h"
#include "diagnostic_msgs/DiagnosticArray.h"
#include "robodyn_mechanisms/FingerController.h"
#include "EmbeddedSmoother.h"
#include <boost/ptr_container/ptr_array.hpp>
#include "robodyn_utilities/GeneralUtilities.h"
#include <boost/lexical_cast.hpp>

/***************************************************************************/
template <unsigned int N, unsigned int NHall = N>
class JointCommandFinger : public JointCommandInterface
{
public:
    JointCommandFinger(const std::string& mechanism, IoFunctions ioFunctions);
    ~JointCommandFinger();

    sensor_msgs::JointState           getSimpleMeasuredState();
    sensor_msgs::JointState           getCompleteMeasuredState();
    r2_msgs::JointCommand getCommandedState();
    void setCommand(r2_msgs::JointCommand msg, r2_msgs::JointControlData control);
    void updateMeasuredState(r2_msgs::JointControlData msg);
    void setFaultState();

    r2_msgs::JointCapability getCapability();
    void loadCoeffs();

private:
    FingerController<N, NHall> controller;
    boost::array<std::string, NHall>        hallStateElements;
    boost::array < std::string, N + 1 >     encoderStateElements;
    boost::array < std::string, N + 1 >     tensionAStateElements;
    boost::array < std::string, N + 1 >     tensionBStateElements;
    boost::array < std::string, N + 1 >     motComCommandElements;


    typename FingerController<N, NHall>::JointVectorType    jointVec;
    typename FingerController<N, NHall>::HallVectorType     hallVecRaw, hallVec;
    typename FingerController<N, NHall>::SliderVectorType   encoderVec, encoderOffset, sliderVec, desiredSliders, desiredSliderVels;
    typename FingerController<N, NHall>::SliderVectorType   tensionAVec, tensionBVec, tensionVec, kTendon, fingerBoostMotCom;
    typename FingerController<N, NHall>::SliderVectorType   sliderMotComs, sliderVel;

    bool tubeTared;
    double kTendonDefault;
    bool activeStiffnessOn;
    bool useCartesian;
    bool isLeft;
    double tensionMin;
    double fingerBoostTime;
    ros::Duration fingerBoostCurrentDuration;
    ros::Time fingerBoostTimerStart;

    std::vector<double> positionVals;
    std::vector<double> velocityVals;
    std::vector<double> effortVals;

    // util
    std::vector<std::string>::iterator strVecIt;
    boost::ptr_array<EmbeddedSmoother, N> smoother;
    typename FingerController<N, NHall>::JointVectorType smoothedPos;
    typename FingerController<N, NHall>::JointVectorType prevSmoothedPos;

    // filter values
    typename FingerController<N, NHall>::SliderVectorType filteredSliderVec;
    typename FingerController<N, NHall>::HallVectorType   filteredHallVec;
    std::vector<double> prevPositionVals;
    double timestep;
    double positionAlpha;
    bool filterInitialized;

    // live coeff names
    std::string nameIsCalibrated;
    std::string nameCoeffState;
    boost::array < std::string, N + 1 >          encoderOffsetElements;
    boost::array < std::string, N + 1 >          sliderOffsetElements;
    boost::array < std::string, N + 1 >          sliderTarePosElements;
    boost::array < std::string, N + 1 >          tensionOffsetElements;

    // param names
    std::string nameActiveStiffnessOn;
    std::string nameUseCartesian;
    std::string nameFingerBoostTime;
    boost::array < std::string, N + 1 >          nameKTendon;
    boost::array < std::string, N + 1 >          nameFingerBoost;
};

template <unsigned int N, unsigned int NHall>
JointCommandFinger<N, NHall>::JointCommandFinger(const std::string& mechanism, IoFunctions ioFunctions)
    : JointCommandInterface(mechanism, ioFunctions)
    , desiredSliders(FingerController<N, NHall>::SliderVectorType::Zero())
    , tubeTared(false), kTendonDefault(0.0) , activeStiffnessOn(false), useCartesian(false), isLeft(false), tensionMin(0.0), filterInitialized(false)
{
    if (mechanism == "")
    {
        std::stringstream err;
        err << "Constructor requires mechanism be non-empty.";
        NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandFinger", log4cpp::Priority::FATAL, err.str());
        throw std::invalid_argument(err.str());
    }

    if (io.getUInt16.empty())
    {
        std::stringstream err;
        err << "Constructor requires 'io.getUInt16' be non-empty.";
        NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandFinger", log4cpp::Priority::FATAL, err.str());
        throw std::invalid_argument(err.str());
    }

    if (io.getInt16.empty())
    {
        std::stringstream err;
        err << "Constructor requires 'io.getInt16' be non-empty.";
        NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandFinger", log4cpp::Priority::FATAL, err.str());
        throw std::invalid_argument(err.str());
    }

    if (io.setInt16.empty())
    {
        std::stringstream err;
        err << "Constructor requires 'io.setInt16' be non-empty.";
        NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandFinger", log4cpp::Priority::FATAL, err.str());
        throw std::invalid_argument(err.str());
    }

    if (io.getBrainstemCoeff.empty())
    {
        std::stringstream err;
        err << "Constructor requires 'io.getBrainstemCoeff' be non-empty.";
        NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandFinger", log4cpp::Priority::FATAL, err.str());
        throw std::invalid_argument(err.str());
    }

    if (io.getMotorCoeff.empty())
    {
        std::stringstream err;
        err << "Constructor requires 'io.getMotorCoeff' be non-empty.";
        NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandFinger", log4cpp::Priority::FATAL, err.str());
        throw std::invalid_argument(err.str());
    }

    if (io.hasLiveCoeff.empty())
    {
        std::stringstream err;
        err << "Constructor requires 'io.hasLiveCoeff' be non-empty.";
        NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandFinger", log4cpp::Priority::FATAL, err.str());
        throw std::invalid_argument(err.str());
    }

    if (io.getLiveCoeff.empty())
    {
        std::stringstream err;
        err << "Constructor requires 'io.getLiveCoeff' be non-empty.";
        NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandFinger", log4cpp::Priority::FATAL, err.str());
        throw std::invalid_argument(err.str());
    }

    if (io.setLiveCoeff.empty())
    {
        std::stringstream err;
        err << "Constructor requires 'io.setLiveCoeff' be non-empty.";
        NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandFinger", log4cpp::Priority::FATAL, err.str());
        throw std::invalid_argument(err.str());
    }

    if (io.hasParam.empty())
    {
        std::stringstream err;
        err << "Constructor requires 'io.hasParam' be non-empty.";
        NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandFinger", log4cpp::Priority::FATAL, err.str());
        throw std::invalid_argument(err.str());
    }

    if (io.getDoubleParam.empty())
    {
        std::stringstream err;
        err << "Constructor requires 'io.getDoubleParam' be non-empty.";
        NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandFinger", log4cpp::Priority::FATAL, err.str());
        throw std::invalid_argument(err.str());
    }

    if (io.setDoubleParam.empty())
    {
        std::stringstream err;
        err << "Constructor requires 'io.setDoubleParam' be non-empty.";
        NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandFinger", log4cpp::Priority::FATAL, err.str());
        throw std::invalid_argument(err.str());
    }

    if (io.getBoolParam.empty())
    {
        std::stringstream err;
        err << "Constructor requires 'io.getBoolParam' be non-empty.";
        NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandFinger", log4cpp::Priority::FATAL, err.str());
        throw std::invalid_argument(err.str());
    }

    if (io.setBoolParam.empty())
    {
        std::stringstream err;
        err << "Constructor requires 'io.setBoolParam' be non-empty.";
        NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandFinger", log4cpp::Priority::FATAL, err.str());
        throw std::invalid_argument(err.str());
    }


    if (io.getJointNames.empty())
    {
        std::stringstream err;
        err << "Constructor requires 'io.getJointNames' be non-empty.";
        NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandFinger", log4cpp::Priority::FATAL, err.str());
        throw std::invalid_argument(err.str());
    }

    if (io.getAllJointNames.empty())
    {
        std::stringstream err;
        err << "Constructor requires 'io.getAllJointNames' be non-empty.";
        NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandFinger", log4cpp::Priority::FATAL, err.str());
        throw std::invalid_argument(err.str());
    }

    if (io.getActuatorNames.empty())
    {
        std::stringstream err;
        err << "Constructor requires 'io.getActuatorNames' be non-empty.";
        NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandFinger", log4cpp::Priority::FATAL, err.str());
        throw std::invalid_argument(err.str());
    }

    if (io.getCommandFile.empty())
    {
        std::stringstream err;
        err << "Constructor requires 'io.getCommandFile' be non-empty.";
        NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandFinger", log4cpp::Priority::FATAL, err.str());
        throw std::invalid_argument(err.str());
    }

    roboDynJoints    = io.getJointNames(mechanism);
    roboDynAllJoints = io.getAllJointNames(mechanism);
    roboDynActuators = io.getActuatorNames(mechanism);

    if (roboDynJoints.size() != N or roboDynAllJoints.size() != NHall or roboDynActuators.size() != N + 1)
    {
        std::stringstream err;
        err << "Constructor requires roboDynJoints to have N, roboDynAllJoints to have NHall, and roboDynActuators have N+1 values, where N = " << N << " and NHall = " << NHall << " for this instance\n";

        err << "The current size of roboDynJoints: " << roboDynJoints.size() << " [";

        for (size_t i = 0; i < roboDynJoints.size(); ++i)
        {
            err << " " << roboDynJoints[i];
        }

        err << " ]\n";

        err << "The current size of roboDynAllJoints: " << roboDynAllJoints.size() << " [";

        for (size_t i = 0; i < roboDynAllJoints.size(); ++i)
        {
            err << " " << roboDynAllJoints[i];
        }

        err << " ]\n";

        err << "The current size of roboDynActuators: " << roboDynActuators.size() << " [";

        for (size_t i = 0; i < roboDynActuators.size(); ++i)
        {
            err << " " << roboDynActuators[i];
        }

        err << " ]\n";

        NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandFinger", log4cpp::Priority::FATAL, err.str());
        throw std::invalid_argument(err.str());

    }

    loadCoeffs();
}

template <unsigned int N, unsigned int NHall>
JointCommandFinger<N, NHall>::~JointCommandFinger()
{}

template <unsigned int N, unsigned int NHall>
void JointCommandFinger<N, NHall>::loadCoeffs()
{
    tubeTared = false;
    kTendonDefault = 0.0;
    activeStiffnessOn = false;
    useCartesian = false;
    isLeft = false;
    tensionMin = 0.0;
    filterInitialized = false;

    std::string parameterFile = io.getCommandFile(mechanism);

    unsigned int numSimpleStates = NHall; // n joints, n+1 sliders (with tension/effort)
    simpleMeasuredStateMsg.name.resize(numSimpleStates);
    simpleMeasuredStateMsg.position.resize(numSimpleStates, 0.);
    simpleMeasuredStateMsg.velocity.resize(numSimpleStates, 0.);
    simpleMeasuredStateMsg.effort.resize(numSimpleStates, 0.);

    // NHall hall sensors, N joint embedded commands, and (N + 1) slider embedded commands
    unsigned int numCompleteStates = (N) + (NHall) + (N) + 2 * (N + 1);
    completeMeasuredStateMsg.name.resize(numCompleteStates);
    completeMeasuredStateMsg.position.resize(numCompleteStates, 0.);
    completeMeasuredStateMsg.velocity.resize(numCompleteStates, 0.);
    completeMeasuredStateMsg.effort.resize(numCompleteStates, 0.);

    commandedStateMsg.name = roboDynJoints;
    commandedStateMsg.desiredPosition.resize(N, 0.);
    commandedStateMsg.desiredPositionVelocityLimit.resize(N, 0.);
    commandedStateMsg.feedForwardTorque.resize(N, 0.);
    commandedStateMsg.proportionalGain.resize(N, 0.);
    commandedStateMsg.derivativeGain.resize(N, 0.);
    commandedStateMsg.integralGain.resize(N, 0.);
    commandedStateMsg.positionLoopTorqueLimit.resize(N, 0.);
    commandedStateMsg.positionLoopWindupLimit.resize(N, 0.);
    commandedStateMsg.torqueLoopVelocityLimit.resize(N, 0.);

    jointCapabilityMsg.name = roboDynJoints;
    jointCapabilityMsg.positionLimitMax.resize(N, 0.);
    jointCapabilityMsg.positionLimitMin.resize(N, 0.);
    jointCapabilityMsg.velocityLimit.resize(N, 30.);  
    jointCapabilityMsg.torqueLimit.resize(N, 0.);

    for (unsigned int i = 0; i < numSimpleStates; ++i)
    {
        simpleMeasuredStateMsg.name[i] = roboDynAllJoints[i];
    }

//    for (unsigned int i = 0; i < N + 1; ++i)
//    {
//        simpleMeasuredStateMsg.name[N + i] = roboDynActuators[i];
//    }

    for (unsigned int i = 0; i < N; ++i)
    {
        completeMeasuredStateMsg.name[i] = roboDynJoints[i];
        completeMeasuredStateMsg.name[2 * N + 1 + i] = roboDynJoints[i] + "/embeddedCommand";
    }

    for (unsigned int i = 0; i < N + 1; ++i)
    {
        completeMeasuredStateMsg.name[N + i] = roboDynActuators[i];
        completeMeasuredStateMsg.name[3 * N + 1 + i] = roboDynActuators[i] + "/embeddedCommand";
    }

    for (unsigned int i = 0; i < NHall; ++i)
    {
        completeMeasuredStateMsg.name[4 * N + 2 + i] = roboDynAllJoints[i] + "/halls";
    }

    for (unsigned int i = 0; i < (N + 1); ++i)
    {
        kTendon[i] = kTendonDefault;
        fingerBoostMotCom[i] = 0.0;
    }

    fingerBoostTime = 0.0;
    fingerBoostCurrentDuration = ros::Duration(0.0);
    unsigned int numStates = 2 * N + 1;
    positionVals.resize(numStates, 0.);
    velocityVals.resize(numStates, 0.);
    effortVals.resize(numStates, 0.);
    prevPositionVals = positionVals;
    smoothedPos.setZero();
    prevSmoothedPos.setZero();

    TiXmlDocument file(parameterFile.c_str());
    bool loadOkay = file.LoadFile();

    if (!loadOkay)
    {
        std::stringstream err;
        err << "Failed to load file [" << parameterFile << "]";
        NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandFinger", log4cpp::Priority::FATAL, err.str());
        throw std::runtime_error(err.str());
    }

    TiXmlHandle doc(&file);
    NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandFinger", log4cpp::Priority::INFO, "CommandFile [" + parameterFile + "] successfully loaded.");

    // Check for ApiMap
    TiXmlHandle parametersElement(doc.FirstChildElement("ApiMap"));

    if (parametersElement.ToElement())
    {
        std::stringstream str;

        for (unsigned int i = 0; i < N + 1; ++i)
        {
            str.str("");
            str << "Actuator" << i << "EncoderState";
            encoderStateElements[i] = StringUtilities::makeFullyQualifiedRoboDynElement(roboDynActuators[i], ApiMap::getXmlElementValue(parametersElement, str.str()));
            str.str("");
            str << "Actuator" << i << "TensionAState";
            tensionAStateElements[i] = StringUtilities::makeFullyQualifiedRoboDynElement(roboDynActuators[i], ApiMap::getXmlElementValue(parametersElement, str.str()));
            str.str("");
            str << "Actuator" << i << "TensionBState";
            tensionBStateElements[i] = StringUtilities::makeFullyQualifiedRoboDynElement(roboDynActuators[i], ApiMap::getXmlElementValue(parametersElement, str.str()));
            str.str("");
            str << "Actuator" << i << "MotComCommand";
            motComCommandElements[i] = StringUtilities::makeFullyQualifiedRoboDynElement(roboDynActuators[i], ApiMap::getXmlElementValue(parametersElement, str.str()));
        }

        for (unsigned int i = 0; i < NHall; ++i)
        {
            str.str("");
            str << "Joint" << i << "HallState";
            hallStateElements[i] = StringUtilities::makeFullyQualifiedRoboDynElement(roboDynAllJoints[i], ApiMap::getXmlElementValue(parametersElement, str.str()));
        }

        // finger controller coeffs
        float tempVal;
        tempVal = io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "MillimetersPerCount")));
        controller.setMillimetersPerCount(static_cast<double>(tempVal));

        double hallScale;
        hallScale = io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "HallScaleFactor")));

        typename FingerController<N, NHall>::HallVectorType hallCoeffs0, hallCoeffs1, hallCoeffs2, hallCoeffs3;

        for (unsigned int i = 0; i < hallCoeffs0.rows(); ++i)
        {
            str.str("");
            str << "Joint" << i << "HallCoeff0";
            hallCoeffs0[i] = io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, str.str())));
            str.str("");
            str << "Joint" << i << "HallCoeff1";
            hallCoeffs1[i] = io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, str.str())));
            str.str("");
            str << "Joint" << i << "HallCoeff2";
            hallCoeffs2[i] = io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, str.str())));
            str.str("");
            str << "Joint" << i << "HallCoeff3";
            hallCoeffs3[i] = io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, str.str())));
        }

        controller.setHallAngleParameters(hallScale, hallCoeffs0, hallCoeffs1, hallCoeffs2, hallCoeffs3);

        typename FingerController<N, NHall>::SliderVectorType tensionParam1, tensionParam2, tensionParam3, tensionParam4;

        for (unsigned int i = 0; i < tensionParam1.rows(); ++i)
        {
            str.str("");
            str << "Actuator" << i << "TensionAGain";
            tensionParam1[i] = io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, str.str())));
            str.str("");
            str << "Actuator" << i << "TensionBGain";
            tensionParam2[i] = io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, str.str())));
            str.str("");
            str << "Actuator" << i << "TensionSensorCalOffset";
            tensionParam3[i] = io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, str.str())));
            str.str("");
            str << "Actuator" << i << "CalibratedStrain";
            tensionParam4[i] = io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, str.str())));
        }

        controller.setTensionParameters(tensionParam1, tensionParam2, tensionParam3, tensionParam4);

        nameActiveStiffnessOn = StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "ActiveStiffnessOn"));
        nameUseCartesian      = StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "UseCartesian"));

        activeStiffnessOn = boost::lexical_cast<bool>(io.getMotorCoeff(nameActiveStiffnessOn));
        io.setBoolParam(nameActiveStiffnessOn, activeStiffnessOn);

        useCartesian = boost::lexical_cast<bool>(io.getMotorCoeff(nameUseCartesian));
        io.setBoolParam(nameUseCartesian, useCartesian);

        isLeft = !(mechanism.find("left") > 10);

        double kd, tensionMax, tensionKp, tFreq, deadband;
        typename FingerController<N, NHall>::JointVectorType stiffnessK, jointKp;
        kd = io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "KdGain")));
        tensionMin = io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "MinTension")));
        tensionMax = io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "MaxTension")));
        tensionKp = io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "tensionKpGain")));
        tFreq = io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "tensionFilterCutoffFreq")));
        deadband = io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "DeadbandSize")));

        for (unsigned int i = 0; i < stiffnessK.rows(); ++i)
        {
            str.str("");
            str << "Joint" << i << "stiffnessKGain";
            stiffnessK[i] = io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, str.str())));
            str.str("");
            str << "Joint" << i << "torqueKpGain";
            jointKp[i] = io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, str.str())));
        }

        controller.setStiffnessGains(stiffnessK, kd, tensionMin, tensionMax, jointKp, tensionKp, tFreq, deadband);

        tempVal = io.getBrainstemCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "BusVoltage")));
        controller.setBusVoltage(static_cast<double>(tempVal));

        // filter
        positionAlpha = io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "PositionAlpha")));

        // "embedded" smoothing
        EmbeddedSmoother::Settings smoothSettings;
        timestep = io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "Timestep")));

        if (timestep <= 0.)
        {
            throw std::runtime_error("invalid timestep");
        }

        smoothSettings.timestep = timestep;

        for (unsigned int i = 0; i < N; ++i)
        {
            str.str("");
            str << "Joint" << i << "MinimumVelocity";
            smoothSettings.minVel = io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, str.str())));
            str.str("");
            str << "Joint" << i << "MaximumAcceleration";
            smoothSettings.maxAcc = io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, str.str())));
            str.str("");
            str << "Joint" << i << "AccelerationGain";
            smoothSettings.accGain = io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, str.str())));
            smoother.replace(i, new EmbeddedSmoother(smoothSettings));
        }

        MultiLoopController tempController;
        tempController.setLoopRate(1. / timestep);

        double bypass =             io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "PositionLoopBypass")));
        double sliderMinPosition =  io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "SliderMinPositionCommand")));
        double sliderMaxPosition =  io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "SliderMaxPositionCommand")));
        double pKp               =  io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "SliderKpGainCommand")));

        tempController.setPositionLoopParameters(sliderMinPosition, sliderMaxPosition, pKp, !(bypass <= 0));

        bypass =               io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "TorqueLoopBypass")));
        double minTension =    io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "SliderMinTensionCommand")));
        double maxTension =    io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "SliderMaxTensionCommand")));
        double tKp        =    io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "SliderTensionKpGainCommand")));
        double offset     =    io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "SliderTensionOffsetCommand")));

        tempController.setTorqueLoopParameters(minTension, maxTension, tKp, offset, !(bypass <= 0));

        bypass =                io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "VelocityLoopBypass")));
        double minVelocity =    io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "SliderMinVelocityCommand" )));
        double maxVelocity =    io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "SliderMaxVelocityCommand" )));
        double vKp =            io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "SliderVelocityKpGainCommand" )));
        double vKi =            io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "SliderVelocityKiGainCommand" )));
        double windup =         io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "SliderIntegratorWindupLimit" )));

        tempController.setVelocityLoopParameters(minVelocity, maxVelocity, vKp, vKi, windup, !(bypass <= 0));

        bypass =                io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "CurrentLoopBypass")));
        double minCurrent =     io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "SliderMinCurrentCommand" )));
        double maxCurrent =     io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "SliderMaxCurrentCommand" )));
        double maxDutyCycle =   io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "SliderMaxDutyCycleCommand" )));

        tempController.setCurrentLoopParameters(minCurrent, maxCurrent, maxDutyCycle, !(bypass <= 0));

        double motorViscousDampingComp =    io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "MotorViscousDampingCompensation" )));
        double motorInertia =               io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "MotorInertia" )));
        double motorTorqueConst =           io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "MotorTorqueConstant" )));
        double motorPhaseRes =              io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "MotorPhaseResistance" )));
        double motorBackEMF =               io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "MotorBackEMFConstant" )));
        double motorImductance =            io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "MotorInductance" )));
        double motionRatio =                io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "MotionRatio" )));
        double PWMFreq =                    io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "PWMFrequency" )));
        double bridgeDeadTime =             io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "BridgeDeadTime" )));
        double bridgeSwitchTime =           io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "BridgeSwitchTime" )));

        tempController.setHardwareParameters(motorViscousDampingComp, motorInertia, motorTorqueConst, motorPhaseRes, motorBackEMF, motorImductance, motionRatio, PWMFreq, bridgeDeadTime, bridgeSwitchTime);

        for (unsigned int i = 0; i < N + 1; ++i)
        {
            controller.setMultiLoopController(tempController, i);
        }

        for (unsigned int i = 0; i < N; ++i)
        {
            str.str("");
            str << "Joint" << i << "PositionMin";
            jointCapabilityMsg.positionLimitMin[i] = io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, str.str())));
            str.str("");
            str << "Joint" << i << "PositionMax";
            jointCapabilityMsg.positionLimitMax[i] = io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, str.str())));
        }

        typename FingerController<N, NHall>::ReferenceMatrixType refMatrix;

        for (unsigned int i = 0; i < refMatrix.rows(); ++i)
        {
            for (unsigned int j = 0; j < refMatrix.cols(); ++j)
            {
                str.str("");
                str << "ReferenceMatrix" << i << j;
                refMatrix(i, j) = io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, str.str())));
            }
        }

        controller.setReferenceMatrix(refMatrix);

        nameIsCalibrated          = StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, "CalibrationMode");
        nameCoeffState            = StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, "CoeffState");



        for (unsigned int i = 0; i < N + 1; ++i)
        {
            nameKTendon[i]           = StringUtilities::makeFullyQualifiedRoboDynElement(roboDynActuators[i], "KTendon");
            io.setDoubleParam(nameKTendon[i], kTendon[i]);

            nameFingerBoost[i]       = StringUtilities::makeFullyQualifiedRoboDynElement(roboDynActuators[i], "FingerBoostMotCom");
            io.setDoubleParam(nameFingerBoost[i], fingerBoostMotCom[i]);

            encoderOffsetElements[i] = StringUtilities::makeFullyQualifiedRoboDynElement(roboDynActuators[i], "EncoderOffset");
            sliderOffsetElements[i]  = StringUtilities::makeFullyQualifiedRoboDynElement(roboDynActuators[i], "SliderOffset");
            sliderTarePosElements[i] = StringUtilities::makeFullyQualifiedRoboDynElement(roboDynActuators[i], "SliderTarePosition");
            tensionOffsetElements[i] = StringUtilities::makeFullyQualifiedRoboDynElement(roboDynActuators[i], "TensionOffset");
        }

        nameFingerBoostTime          = StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, "FingerBoostTime");
        io.setDoubleParam(nameFingerBoostTime, fingerBoostTime);

        io.setLiveCoeff(nameIsCalibrated,   r2_msgs::JointControlCalibrationMode::UNCALIBRATED);
        io.setLiveCoeff(nameCoeffState,     r2_msgs::JointControlCoeffState::LOADED);
    }
    else
    {
        std::stringstream err;
        err << "The file " << parameterFile << " has no element named [ApiMap]";
        NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandFinger", log4cpp::Priority::ERROR, err.str());
        throw std::runtime_error(err.str());
    }
}

template <unsigned int N, unsigned int NHall>
void JointCommandFinger<N, NHall>::updateMeasuredState(r2_msgs::JointControlData msg)
{
    unsigned int i;

    // get params

    if (!io.getBoolParam(nameActiveStiffnessOn, activeStiffnessOn))
    {
        std::stringstream err;
        err << "rosparam [" << nameActiveStiffnessOn << "] does not exist.";
        NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandFinger", log4cpp::Priority::ERROR, err.str());
        throw std::runtime_error(err.str());
    }

    if (!io.getBoolParam(nameUseCartesian, useCartesian))
    {
        std::stringstream err;
        err << "rosparam [" << nameUseCartesian << "] does not exist.";
        NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandFinger", log4cpp::Priority::ERROR, err.str());
        throw std::runtime_error(err.str());
    }

    // If coeffs are not loaded, override with zeros RDEV-1075
    if (msg.coeffState.state != r2_msgs::JointControlCoeffState::LOADED)
    {
        unsigned int numStates = 2 * N + 1; // n joints, n+1 sliders (with tension/effort)
        positionVals.assign(numStates, 0.0);
        velocityVals.assign(numStates, 0.0);
        effortVals.assign(numStates, 0.0);
        return;
    }

    for (i = 0; i < encoderStateElements.size(); ++i)
    {
        encoderVec[i] = io.getInt16(encoderStateElements[i]);
    }

    for (i = 0; i < hallStateElements.size(); ++i)
    {
        hallVecRaw[i] = io.getUInt16(hallStateElements[i]);
    }

    for (i = 0; i < tensionAStateElements.size(); ++i)
    {
        tensionAVec[i] = io.getUInt16(tensionAStateElements[i]);
    }

    for (i = 0; i < tensionBStateElements.size(); ++i)
    {
        tensionBVec[i] = io.getUInt16(tensionBStateElements[i]);
    }

    // get hall positions
    controller.getHallAngles(hallVecRaw, hallVec);

    if (io.getLiveCoeff(nameIsCalibrated) == r2_msgs::JointControlCalibrationMode::DISABLE)
    {
        // calibrated
        if (!tubeTared)
        {
            typename FingerController<N, NHall>::SliderVectorType sliderOffset, sliderTarePos, tensionOffset;

            for (i = 0; i < encoderOffset.rows(); ++i)
            {
                encoderOffset[i] = io.getLiveCoeff(encoderOffsetElements[i]);
                sliderOffset[i]  = io.getLiveCoeff(sliderOffsetElements[i]);
                sliderTarePos[i] = io.getLiveCoeff(sliderTarePosElements[i]);
                tensionOffset[i] = io.getLiveCoeff(tensionOffsetElements[i]);
            }

            sliderOffset -= encoderOffset;
            controller.reset();

            //check that tensions are above the minimum tension, or adjust tensionOffset
            controller.getCalibratedTensions(tensionAVec, tensionBVec, 0.0, tensionVec);

            for (i = 0; i < encoderOffset.rows(); ++i)
            {
                if (tensionVec[i] < tensionMin)
                {
                    tensionOffset[i] += tensionVec[i] - tensionMin;    //tensionVec.sum()/tensionVec.rows();
                }
            }

            controller.setTubeTareParameters(sliderOffset, sliderTarePos, tensionOffset);
            filterInitialized = false;
            tubeTared = true;
        }

        // get positions
        encoderVec -= encoderOffset;
        controller.getSlidersFromEncoders(encoderVec, sliderVec);

        // filter sliders and halls
        if (!filterInitialized)
        {
            filteredSliderVec = sliderVec;
            filteredHallVec = hallVec;
        }

        filteredSliderVec = filteredSliderVec * positionAlpha + sliderVec * (1 - positionAlpha);
        filteredHallVec = filteredHallVec * positionAlpha + hallVec * (1 - positionAlpha);

        controller.getJointsFromSliders(filteredSliderVec, jointVec);

        //use hall sensor data
        // TODO: Check functionality of activeStiffness given passive hall additions
        if (activeStiffnessOn)
        {
            if (sliderVec.rows() >= 4)                 //limits operation to thumb, rows == 5, and primaries, rows == 4 (secondaries only have 3 actuators)
            {
                jointVec.tail(3) = hallVec.tail(3);    //don't use roll joint hall on thumb
            }
        }


        //don't use secondary finger halls (doesn't use combined proximal/medial joint angles)

        // get tensions
        controller.getCalibratedTensions(tensionAVec, tensionBVec, timestep, tensionVec);
        typename FingerController<N, NHall>::JointVectorType xyPos, desTorques;

        for (i = 0; i < N; ++i)
        {
            Robodyn::limit(jointCapabilityMsg.positionLimitMin[i], jointCapabilityMsg.positionLimitMax[i], jointVec[i]);
        }

        if (useCartesian)
        {
            controller.getXYPos(jointVec, isLeft, xyPos, desTorques);
        }
        else
        {
            xyPos = jointVec;
            desTorques = FingerController<N, NHall>::JointVectorType::Zero();
        }

        for (i = 0; i < N; ++i)
        {
            positionVals[i] = xyPos[i];
            effortVals[i] = desTorques[i];
        }

        for (i = 0; i < N + 1; ++i)
        {
            positionVals[N + i] = filteredSliderVec[i];
            effortVals[N + i]   = tensionVec[i];
        }
    }
    else
    {
        // not calibrated
        tubeTared = false;

        for (i = 0; i < N; ++i)
        {
            positionVals[i] = 0.;
        }

        for (i = 0; i < N + 1; ++i)
        {
            positionVals[N + i] = 0.;
            effortVals[N + i]   = 0.;
        }
    }

    // get velocities
    if (!filterInitialized)
    {
        velocityVals.assign(velocityVals.size(), 0.);
        filterInitialized = true;
    }
    else
    {
        for (unsigned int i = 0; i < velocityVals.size(); ++i)
        {
            velocityVals[i] = (positionVals[i] - prevPositionVals[i]) / timestep;
        }
    }

    prevPositionVals = positionVals;
}

template <unsigned int N, unsigned int NHall>
sensor_msgs::JointState JointCommandFinger<N, NHall>::getSimpleMeasuredState()
{
    for (unsigned int i = 0; i < N; ++i)
    {
        // joint vals
        simpleMeasuredStateMsg.velocity[i] = velocityVals[i];
        simpleMeasuredStateMsg.effort[i] = effortVals[i];
    }

    for (unsigned int i = 0; i < NHall; ++i)
    {
        simpleMeasuredStateMsg.position[i] = hallVec[i];
    }

    simpleMeasuredStateMsg.header.stamp = ros::Time::now();
    return simpleMeasuredStateMsg;
}

template <unsigned int N, unsigned int NHall>
sensor_msgs::JointState JointCommandFinger<N, NHall>::getCompleteMeasuredState()
{
    prevSmoothedPos = (smoothedPos - prevSmoothedPos) / timestep;
    unsigned int index = 0;

    for (unsigned int i = 0; i < N; ++i)
    {
        // joint vals
        completeMeasuredStateMsg.position[i] = positionVals[i];
        completeMeasuredStateMsg.velocity[i] = velocityVals[i];
        completeMeasuredStateMsg.effort[i] = effortVals[i];

        // joint embeddedCommands
        index = 2 * N + 1 + i;
        completeMeasuredStateMsg.position[index] = smoothedPos[i];
        completeMeasuredStateMsg.velocity[index] = prevSmoothedPos[i];
    }

    // halls
    for (unsigned int i = 0; i < NHall; ++i)
    {
        index = 4 * N + 2 + i;
        completeMeasuredStateMsg.position[index] = hallVec[i];
    }

    for (unsigned int i = 0; i < N + 1; ++i)
    {
        // slider vals
        index = N + i;
        completeMeasuredStateMsg.position[index] = positionVals[index];
        completeMeasuredStateMsg.velocity[index] = velocityVals[index];
        completeMeasuredStateMsg.effort[index] = effortVals[index];

        // slider embeddedCommands
        index = 3 * N + 1 + i;
        completeMeasuredStateMsg.position[index] = desiredSliders[i];
        completeMeasuredStateMsg.velocity[index] = desiredSliderVels[i];
        completeMeasuredStateMsg.effort[index] = sliderMotComs[i];
    }

    completeMeasuredStateMsg.header.stamp = ros::Time::now();
    prevSmoothedPos = smoothedPos;
    return completeMeasuredStateMsg;
}

template <unsigned int N, unsigned int NHall>
r2_msgs::JointCommand JointCommandFinger<N, NHall>::getCommandedState()
{
    commandedStateMsg.header.stamp = ros::Time::now();
    return commandedStateMsg;
}

template <unsigned int N, unsigned int NHall>
void JointCommandFinger<N, NHall>::setFaultState()
{
}

template <unsigned int N, unsigned int NHall>
void JointCommandFinger<N, NHall>::setCommand(r2_msgs::JointCommand msg, r2_msgs::JointControlData control)
{

    // get KTendon parameter
    for (unsigned int i = 0; i < (N + 1); ++i)
    {
        if (!io.getDoubleParam(nameKTendon[i], kTendon[i]))
        {
            std::stringstream err;
            err << "param [" << nameKTendon[i] << "] does not exist.";
            NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandFinger", log4cpp::Priority::ERROR, err.str());
            throw std::runtime_error(err.str());
        }
    }

    // get fingerBoost parameters if appropriate
    double prevFingerBoostTime = fingerBoostTime;

    if (!io.getDoubleParam(nameFingerBoostTime, fingerBoostTime))
    {
        std::stringstream err;
        err << "param [" << nameFingerBoostTime << "] does not exist.";
        NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandFinger", log4cpp::Priority::ERROR, err.str());
        throw std::runtime_error(err.str());
    }

    if (fingerBoostTime > 0 && fingerBoostTime != prevFingerBoostTime)
    {
        fingerBoostTimerStart = ros::Time::now();

        for (unsigned int i = 0; i < (N + 1); ++i)
        {
            if (!io.getDoubleParam(nameFingerBoost[i], fingerBoostMotCom[i]))
            {
                std::stringstream err;
                err << "param [" << nameFingerBoost[i] << "] does not exist.";
                NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandFinger", log4cpp::Priority::ERROR, err.str());
                throw std::runtime_error(err.str());
            }
        }
    }
    else if (fingerBoostTime > 0)
    {
        fingerBoostCurrentDuration = ros::Time::now() - fingerBoostTimerStart;
    }
    else
    {
        for (unsigned int i = 0; i < N + 1; ++i)
        {
            io.setDoubleParam(nameFingerBoost[i], 0.0);
            io.getDoubleParam(nameFingerBoost[i], fingerBoostMotCom[i]);
        }
    }

    if (fingerBoostCurrentDuration > ros::Duration(fingerBoostTime))
    {
        io.setDoubleParam(nameFingerBoostTime, 0.0);
        io.getDoubleParam(nameFingerBoostTime, fingerBoostTime);

        for (unsigned int i = 0; i < N + 1; ++i)
        {
            io.setDoubleParam(nameFingerBoost[i], 0.0);
            io.getDoubleParam(nameFingerBoost[i], fingerBoostMotCom[i]);
        }

        fingerBoostCurrentDuration = ros::Duration(0.0);
    }

    // end of fingerBoost parameter setting


    if (!msg.name.empty() && !msg.desiredPosition.empty())
    {
        if (msg.desiredPositionVelocityLimit.empty())
        {
            msg.desiredPositionVelocityLimit.resize(msg.name.size(), 0.);
        }

        for (unsigned int i = 0; i < msg.name.size(); ++i)
        {
            strVecIt = std::find(commandedStateMsg.name.begin(), commandedStateMsg.name.end(), msg.name[i]);

            if (strVecIt != commandedStateMsg.name.end())
            {
                unsigned int index = strVecIt - commandedStateMsg.name.begin();

                Robodyn::limit(jointCapabilityMsg.positionLimitMin[index], jointCapabilityMsg.positionLimitMax[index], msg.desiredPosition[i]);

                // smooth if in drive and not in actuator
                if (control.controlMode.state == r2_msgs::JointControlMode::DRIVE
                        && control.commandMode.state != r2_msgs::JointControlCommandMode::ACTUATOR)
                {
                    smoothedPos[index] = smoother[index].update(msg.desiredPosition[i], msg.desiredPositionVelocityLimit[i]);
                }
                else
                {
                    smoothedPos[index] = msg.desiredPosition[i];
                    smoother[index].reset(smoothedPos[index], 0.);
                }


                commandedStateMsg.desiredPosition[index] = msg.desiredPosition[i];
                commandedStateMsg.desiredPositionVelocityLimit[index] = msg.desiredPositionVelocityLimit[i];
            }
        }
    }

    if (control.calibrationMode.state == r2_msgs::JointControlCalibrationMode::ENABLE)
    {
        // do nothing if calibrating (tubetare controls when in this mode)
        return;
    }

    if (control.commandMode.state == r2_msgs::JointControlCommandMode::MOTCOM)
    {
        for (unsigned int i = 0; i < msg.name.size(); ++i)
        {
            strVecIt = std::find(roboDynActuators.begin(), roboDynActuators.end(), msg.name[i]);

            if (strVecIt != roboDynActuators.end())
            {
                sliderMotComs[strVecIt - roboDynActuators.begin()] = msg.desiredPosition[i];
            }
        }
    }
    else if (control.commandMode.state == r2_msgs::JointControlCommandMode::ACTUATOR)
    {
        for (unsigned int i = 0; i < msg.name.size(); ++i)
        {
            strVecIt = std::find(roboDynActuators.begin(), roboDynActuators.end(), msg.name[i]);

            if (strVecIt != roboDynActuators.end())
            {
                desiredSliders[strVecIt - roboDynActuators.begin()] = msg.desiredPosition[i];
            }
        }

        // get equivalent joint command
        controller.getJointsFromSliders(desiredSliders, smoothedPos);

        for (unsigned int i = 0; i < smoothedPos.size(); ++i)
        {
            commandedStateMsg.desiredPosition[i] = smoothedPos[i];
            smoother[i].reset(smoothedPos[i], 0.);
        }

        // get motcoms
        sliderVel = FingerController<N, NHall>::SliderVectorType::Map(&(velocityVals[N]));
        controller.getMotComs(desiredSliders, kTendon, filteredSliderVec, tensionVec, sliderVel, sliderMotComs, desiredSliderVels, fingerBoostMotCom);

    }
    else
    {
        // get motcoms
        sliderVel = FingerController<N, NHall>::SliderVectorType::Map(&(velocityVals[N]));

        if (activeStiffnessOn)
        {
//            typename FingerController<N, NHall>::SliderVectorType sliderVel1;
//            sliderVel1 = FingerController<N, NHall>::SliderVectorType::Map(&(velocityVals[N]));
            controller.getDesiredSlidersStiffness(smoothedPos, jointVec, filteredSliderVec, tensionVec, sliderVel, useCartesian, isLeft, desiredSliders);
        }
        else
        {
            controller.getDesiredSliders(smoothedPos, filteredSliderVec, desiredSliders);
        }

        controller.getMotComs(desiredSliders, kTendon, filteredSliderVec, tensionVec, sliderVel, sliderMotComs, desiredSliderVels, fingerBoostMotCom);
    }


    if (control.controlMode.state == r2_msgs::JointControlMode::DRIVE)
    {
        for (unsigned int i = 0; i < sliderMotComs.rows(); ++i)
        {
            io.setInt16(motComCommandElements[i], static_cast<int16_t>(sliderMotComs[i]));
        }
    }
    else
    {
        desiredSliderVels.setZero();

        for (unsigned int i = 0; i < motComCommandElements.size(); ++i)
        {
            io.setInt16(motComCommandElements[i], 0);
        }
    }
}

template <unsigned int N, unsigned int NHall>
r2_msgs::JointCapability JointCommandFinger<N, NHall>::getCapability()
{
    return jointCapabilityMsg;
}


#endif