JointLimitConstraint.cpp

Go to the documentation of this file.

#include "ocra/control/JointLimitConstraint.h"


using namespace ocra;



JointLimitFunction::JointLimitFunction(const Model& model, Variable& var)
    : NamedInstance("Joint Limit Equation Function")
    , CoupledInputOutputSize(false)
    , LinearFunction(var, 2*model.nbInternalDofs())
    , hpos(.2)
{
    // Set limit from model
    setJointLowerLimits( model.getJointLowerLimits() );
    setJointUpperLimits( model.getJointUpperLimits() );
}

JointLimitFunction::~JointLimitFunction()
{

}

double JointLimitFunction::getHorizonOfPrediction() const
{
    return hpos;
}

void   JointLimitFunction::setHorizonOfPrediction(double newHpos)
{
    hpos = newHpos;
}

void JointLimitFunction::setJointLimits(const Eigen::VectorXd& lowerLimits, const Eigen::VectorXd& upperLimits)
{
    setJointLowerLimits(lowerLimits);
    setJointUpperLimits(upperLimits);
}

void JointLimitFunction::setJointLowerLimits(const Eigen::VectorXd& newLowerLimits)
{
    jointLowerLimits = newLowerLimits;
}

void JointLimitFunction::setJointUpperLimits(const Eigen::VectorXd& newUpperLimits)
{
    jointUpperLimits = newUpperLimits;
}

void JointLimitFunction::setJointLimit(int i, double newLowerLimit, double newUpperLimit)
{
    setJointLowerLimit(i, newLowerLimit);
    setJointUpperLimit(i, newUpperLimit);
}

void JointLimitFunction::setJointLowerLimit(int i, double newLowerLimit)
{
    jointLowerLimits(i) = newLowerLimit;
}

void JointLimitFunction::setJointUpperLimit(int i, double newUpperLimit)
{
    jointUpperLimits(i) = newUpperLimit;
}

const Eigen::VectorXd& JointLimitFunction::getJointLowerLimits() const
{
    return jointLowerLimits;
}

const Eigen::VectorXd& JointLimitFunction::getJointUpperLimits() const
{
    return jointUpperLimits;
}

double JointLimitFunction::getJointLowerLimit(int i) const
{
    return jointLowerLimits(i);
}

double JointLimitFunction::getJointUpperLimit(int i) const
{
    return jointUpperLimits(i);
}

void JointLimitFunction::computeFullJacobian(int varDofs, int nIDofs, Eigen::MatrixXd& fullJacobian) const
{
    Eigen::MatrixXd Icut = Eigen::MatrixXd::Identity(varDofs, varDofs).bottomRows(nIDofs);

    fullJacobian                    =  Eigen::MatrixXd::Zero(2*nIDofs, varDofs);
    fullJacobian.topRows(nIDofs)    = -Icut;
    fullJacobian.bottomRows(nIDofs) =  Icut;
}

void JointLimitFunction::computeFullb(const Eigen::VectorXd& gpos, const Eigen::VectorXd& gvel, Eigen::VectorXd& fullb) const
{
    // We compute the min and max accelerations of q to avoid joint limits:
    // there is two min and max:
    // - the first one is computed of a horizon h. The system brakes far from the limit, but can pass inside the constraint when close to it.
    // - the second one is computed with the time of inflexion. The system brakes when close to the limits, but some singularities appears when it is on the limits.
    //
    // The two computation have their drawbacks, but together they allow limits avoidance properly.

    int nIDofs = gpos.size();

    Eigen::VectorXd maxDdqTotal = 2 * (jointUpperLimits - gpos - hpos * gvel)/(hpos*hpos);
    Eigen::VectorXd minDdqTotal = 2 * (jointLowerLimits - gpos - hpos * gvel)/(hpos*hpos);


//    tmin = -2*(gpos - qlim[:, 0]) / gvel
//    tmax = -2*(gpos - qlim[:, 1]) / gvel
//
//    tmin[tmin <=  0  ] = nan
//    tmin[tmin > hpos] = nan
//    tmin[isfinite(tmin)] = 1.
//    tmax[tmax <=  0  ] = nan
//    tmax[tmax > hpos] = nan
//    tmax[isfinite(tmax)] = 1.

//    bmin_pos_time = gvel**2/(2*(gpos - qlim[:, 0])) * tmin
//    bmax_pos_time = gvel**2/(2*(gpos - qlim[:, 1])) * tmax


    for (int i=0; i<nIDofs; i++)
    {
        double tmax = -2*(gpos(i) - jointUpperLimits(i)) / gvel(i);
        double tmin = -2*(gpos(i) - jointLowerLimits(i)) / gvel(i);

        if ((0 <= tmax) && (tmax <= hpos))
        {
            double ddqlim = (gvel(i)*gvel(i))/(2*(gpos(i) - jointUpperLimits(i)));
            if (maxDdqTotal(i) >= ddqlim)
            {
                maxDdqTotal(i) = ddqlim;
            }
        }
        if ((0 <= tmin) && (tmin <= hpos))
        {
            double ddqlim = (gvel(i)*gvel(i))/(2*(gpos(i) - jointLowerLimits(i)));
            if (minDdqTotal(i) <= ddqlim)
            {
                minDdqTotal(i) = ddqlim;
            }
        }
    }

    fullb.head(nIDofs) =   maxDdqTotal;
    fullb.tail(nIDofs) = - minDdqTotal;

}

















struct FullJointLimitFunction::Pimpl
{
    const Model&   _model;

    Pimpl(const Model& model)
        : _model(model)
    {

    }
};


FullJointLimitFunction::FullJointLimitFunction(const Model& model)
    : NamedInstance("Full Joint Limit Equation Function")
    , AbilitySet(PARTIAL_X)
    , CoupledInputOutputSize(false)
    , JointLimitFunction(model, model.getAccelerationVariable())
    , pimpl(new Pimpl(model))
{
    //_model.connect<EVT_CHANGE_VALUE>(*this, &FullJointLimitFunction::invalidateAll); // not necessary  because A (jacobian) is constant
    pimpl->_model.connect<EVT_CHANGE_VALUE>(*this, &FullJointLimitFunction::invalidateb); //to update b at each time step

    int varDofs = model.getAccelerationVariable().getSize();

    computeFullJacobian(varDofs, pimpl->_model.nbInternalDofs(), _jacobian);

}

FullJointLimitFunction::~FullJointLimitFunction()
{
    //_model.disconnect<EVT_CHANGE_VALUE>(*this, &FullJointLimitFunction::invalidateAll);
    pimpl->_model.disconnect<EVT_CHANGE_VALUE>(*this, &FullJointLimitFunction::invalidateb);
}

void FullJointLimitFunction::updateb() const
{
    computeFullb(pimpl->_model.getJointPositions(), pimpl->_model.getJointVelocities(), _b);
}

















struct ReducedJointLimitFunction::Pimpl
{
    const Model&          _model;
    const FullDynamicEquationFunction& _dynamicEquation;

    Eigen::MatrixXd            _fullJacobian;

    Pimpl(const Model& model, const FullDynamicEquationFunction& dynamicEquation)
        : _model(model)
        , _dynamicEquation(dynamicEquation)
    {

    }
};


ReducedJointLimitFunction::ReducedJointLimitFunction(const Model& model, const FullDynamicEquationFunction& dynamicEquation)
    : NamedInstance("Reduced Joint Limit Equation Function")
    , AbilitySet(PARTIAL_X)
    , CoupledInputOutputSize(false)
    , JointLimitFunction(model, dynamicEquation.getActionVariable())
    , pimpl(new Pimpl(model, dynamicEquation))
{
    pimpl->_model.connect<EVT_CHANGE_VALUE>(*this, &ReducedJointLimitFunction::invalidateAll);
    pimpl->_model.connect<EVT_CHANGE_VALUE>(*this, &ReducedJointLimitFunction::invalidateb); //to update b at each time step

    computeFullJacobian(pimpl->_model.nbDofs(), pimpl->_model.nbInternalDofs(), pimpl->_fullJacobian);
}

ReducedJointLimitFunction::~ReducedJointLimitFunction()
{
    pimpl->_model.disconnect<EVT_CHANGE_VALUE>(*this, &ReducedJointLimitFunction::invalidateAll);
    pimpl->_model.disconnect<EVT_CHANGE_VALUE>(*this, &ReducedJointLimitFunction::invalidateb);
}



void ReducedJointLimitFunction::updateJacobian() const
{

    _jacobian = - pimpl->_fullJacobian * pimpl->_dynamicEquation.getInertiaMatrixInverseJchiT();
}


void ReducedJointLimitFunction::updateb() const
{
    Eigen::VectorXd _fullb(_b.size());
    computeFullb(pimpl->_model.getJointPositions(), pimpl->_model.getJointVelocities(), _fullb);

    //_jacobian =          pimpl->_fullJacobian * pimpl->_model.getInertiaMatrixInverseJchiT();
    _b        = _fullb - pimpl->_fullJacobian * pimpl->_dynamicEquation.getInertiaMatrixInverseLinNonLinGrav();
}



void ReducedJointLimitFunction::doUpdateInputSizeBegin()
{
    //do nothing : this overload allows to resize
}

void ReducedJointLimitFunction::doUpdateInputSizeEnd()
{
    computeFullJacobian(pimpl->_model.nbDofs(), pimpl->_model.nbInternalDofs(), pimpl->_fullJacobian);
}














JointLimitConstraint::JointLimitConstraint(const Model& model, double hpos)
    : _model(model)
    , _is_connected(false)
    , ControlConstraint()
{
    setHorizonOfPrediction(hpos);
    setJointLimits(model.getJointLowerLimits(), model.getJointUpperLimits() );
}

JointLimitConstraint::JointLimitConstraint(const Model& model, const Eigen::VectorXd& lowerLimits, const Eigen::VectorXd& upperLimits, double hpos)
    : _model(model)
    , _is_connected(false)
    , ControlConstraint()
{
    setHorizonOfPrediction(hpos);
    setJointLimits(lowerLimits, upperLimits);
}



double JointLimitConstraint::getHorizonOfPrediction() const
{
    return _hpos;
}
const Eigen::VectorXd& JointLimitConstraint::getJointLowerLimits() const
{
    return _lowerLimits;
}

const Eigen::VectorXd& JointLimitConstraint::getJointUpperLimits() const
{
    return _upperLimits;
}

double JointLimitConstraint::getJointLowerLimit(int i) const
{
    return _lowerLimits(i);
}

double JointLimitConstraint::getJointUpperLimit(int i) const
{
    return _upperLimits(i);
}

void   JointLimitConstraint::setHorizonOfPrediction(double newHpos)
{
    _hpos = newHpos;
    if (_is_connected)
        _jointLimitFunction->setHorizonOfPrediction(newHpos);
}

void  JointLimitConstraint::setJointLimits(const Eigen::VectorXd& lowerLimits, const Eigen::VectorXd& upperLimits)
{
    _lowerLimits = lowerLimits;
    _upperLimits = upperLimits;
    if (_is_connected)
        _jointLimitFunction->setJointLimits(lowerLimits, upperLimits);
}

void  JointLimitConstraint::setJointLowerLimits(const Eigen::VectorXd& newLowerLimits)
{
    _lowerLimits = newLowerLimits;
    if (_is_connected)
        _jointLimitFunction->setJointLowerLimits(newLowerLimits);
}

void  JointLimitConstraint::setJointUpperLimits(const Eigen::VectorXd& newUpperLimits)
{
    _upperLimits = newUpperLimits;
    if (_is_connected)
        _jointLimitFunction->setJointUpperLimits(newUpperLimits);
}

void  JointLimitConstraint::setJointLimit(int i, double newLowerLimit, double newUpperLimit)
{
    _lowerLimits(i) = newLowerLimit;
    _upperLimits(i) = newUpperLimit;
    if (_is_connected)
        _jointLimitFunction->setJointLimit(i, newLowerLimit, newUpperLimit);
}
void  JointLimitConstraint::setJointLowerLimit(int i, double newLowerLimit)
{
    _lowerLimits(i) = newLowerLimit;
    if (_is_connected)
        _jointLimitFunction->setJointLowerLimit(i, newLowerLimit);
}

void  JointLimitConstraint::setJointUpperLimit(int i, double newUpperLimit)
{
    _upperLimits(i) = newUpperLimit;
    if (_is_connected)
        _jointLimitFunction->setJointUpperLimit(i, newUpperLimit);
}


void JointLimitConstraint::connectToController(const FullDynamicEquationFunction& dynamicEquation, bool useReducedProblem)
{
    LinearFunction* f = NULL;
    if (useReducedProblem)
        f = createReducedJointLimitFunction(_model, dynamicEquation);
    else
        f = createFullJointLimitFunction(_model);

    _constraint.reset(new LinearConstraint(f));
    _is_connected = true;

    setHorizonOfPrediction(_hpos);
    setJointLowerLimits( _lowerLimits );
    setJointUpperLimits( _upperLimits );
}

void JointLimitConstraint::disconnectFromController()
{
    _is_connected = false;
}


JointLimitFunction* JointLimitConstraint::createFullJointLimitFunction(const Model& model)
{
    _jointLimitFunction = new FullJointLimitFunction(model);
    return _jointLimitFunction;
}

JointLimitFunction* JointLimitConstraint::createReducedJointLimitFunction(const Model& model, const FullDynamicEquationFunction& dynamicEquation)
{
    _jointLimitFunction = new ReducedJointLimitFunction(model, dynamicEquation);
    return _jointLimitFunction;
}