TorqueLimitConstraint.cpp

Go to the documentation of this file.

#include "ocra/control/TorqueLimitConstraint.h"



namespace ocra
{




TorqueLimitFunction::TorqueLimitFunction(const Model& model)
    : NamedInstance("Torque Limit Equation Function")
    , AbilitySet(PARTIAL_X)
    , CoupledInputOutputSize(false)
    , _torqueDim(model.getJointTorqueVariable().getSize())
    , LinearFunction(model.getJointTorqueVariable(), 2*model.getJointTorqueVariable().getSize())
{
    //_model.connect<EVT_CHANGE_VALUE>(*this, &DynamicEquationFunction::invalidateAll); // not necessary  because A (jacobian) is constant
    //_model.connect<EVT_CHANGE_VALUE>(*this, &JointLimitFunction::invalidateb);   // b is constant, unless someone changes it by calling setTorqueLimits

    /* The function is on the form:
     *      | O -I |         |  taumax |
     *      | O  I | tau  +  | -taumax |
     *
     * where O (nx6) is defined if the model has a free-floating base (we cannot limit the range of this "joint"), else O is a void matrix (nx0)
     *
     * The associated constraint is:
     *      A ddq + b >= 0
     */

    // Building of A, the jacobian, which is constant:
    Eigen::MatrixXd I = Eigen::MatrixXd::Identity(_torqueDim, _torqueDim);

    _jacobian = Eigen::MatrixXd::Zero(2*_torqueDim, _torqueDim);
    _jacobian.topRows(_torqueDim)    = -I;
    _jacobian.bottomRows(_torqueDim) =  I;

    // Set limit from model
    setTorqueLimits( Eigen::VectorXd::Constant(_torqueDim, 1000) );
}

TorqueLimitFunction::~TorqueLimitFunction()
{

}

void TorqueLimitFunction::updateJacobian() const
{

}

void TorqueLimitFunction::updateb() const
{
    _b.head(_torqueDim) =   _torqueLimits;
    _b.tail(_torqueDim) =   _torqueLimits;
}


void  TorqueLimitFunction::setTorqueLimits(const Eigen::VectorXd& torqueLimits)
{
    _torqueLimits = torqueLimits;
    updateb();
}

const Eigen::VectorXd& TorqueLimitFunction::getTorqueLimits() const
{
    return _torqueLimits;
}




} // end namespace ocra