ContactAvoidanceConstraint.cpp

Go to the documentation of this file.

 #include "ocra/control/ContactAvoidanceConstraint.h"


using namespace ocra;




ContactAvoidanceFunction::ContactAvoidanceFunction(const Model& model, Variable& var)
    : NamedInstance("wOcra Contact Avoidance Function")
    , CoupledInputOutputSize(false)
    , LinearFunction(var, 0)
//    , _model(model)
//    , _q_ddot(model.getAccelerationVariable())
    , hpos(.5)
    , margin(.05)
{
    model.connect<EVT_CHANGE_VALUE>(*this, &ContactAvoidanceFunction::invalidateAll);
    model.connect<EVT_CHANGE_VALUE>(*this, &ContactAvoidanceFunction::invalidateb);
}

ContactAvoidanceFunction::~ContactAvoidanceFunction()
{

}

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

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

double ContactAvoidanceFunction::getMargin() const
{
    return margin;
}

void   ContactAvoidanceFunction::setMargin(double newMargin)
{
    margin = newMargin;
}

void ContactAvoidanceFunction::updateContactInformation(const Eigen::MatrixXd& _JObst, const Eigen::VectorXd& _dJdqObst, const Eigen::VectorXd& _distObst, const Eigen::VectorXd& _velObst)
{
    JObst    = _JObst;
    dJdqObst = _dJdqObst;
    distObst = _distObst;
    velObst  = _velObst;

    changeFunctionDimension(_distObst.size());
}

void ContactAvoidanceFunction::computeFullJacobian(const Eigen::MatrixXd& _JObst, Eigen::MatrixXd& fullJacobian) const
{
    fullJacobian = - _JObst;
}

void ContactAvoidanceFunction::computeFullb(const Eigen::VectorXd& _dJdqObst, const Eigen::VectorXd& _distObst, const Eigen::VectorXd& _velObst, Eigen::VectorXd& fullb) const
{
    fullb = - _dJdqObst + ( (_distObst - Eigen::VectorXd::Constant(_distObst.size(), margin)) - _velObst * hpos ) * 2/(hpos*hpos);


    for (int i=0; i<_distObst.size(); i++)
    {
        double tmax = 2*(_distObst(i) - margin) / _velObst(i);
        if ((0 <= tmax) && (tmax <= hpos))
        {
            double ddqlim = - ( _velObst(i)*_velObst(i) ) / ( 2.*(_distObst(i) - margin) );
            if (fullb(i) >= ddqlim)
            {
                fullb(i) = ddqlim;
            }
        }
    }
}

void ContactAvoidanceFunction::updateJacobian() const
{
    //TODO change _jacobian
}

void ContactAvoidanceFunction::updateb() const
{
    //TODO change _b
}

void ContactAvoidanceFunction::doUpdateDimensionBegin(int newDimension)
{

}

void ContactAvoidanceFunction::doUpdateDimensionEnd(int oldDimension)
{

}












FullContactAvoidanceFunction::FullContactAvoidanceFunction(const Model& model)
    : NamedInstance("Full Contact Avoidance Function")
    , AbilitySet(PARTIAL_X)
    , CoupledInputOutputSize(false)
    , ContactAvoidanceFunction(model, model.getAccelerationVariable())
    //, pimpl(new Pimpl(model))
{

}

FullContactAvoidanceFunction::~FullContactAvoidanceFunction()
{

}

void FullContactAvoidanceFunction::updateJacobian() const
{
    computeFullJacobian(JObst, _jacobian);
}

void FullContactAvoidanceFunction::updateb() const
{
    computeFullb(dJdqObst, distObst, velObst, _b);
}













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

    Eigen::MatrixXd             _fullJacobian;
    Eigen::VectorXd             _fullb;

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

    }
};

ReducedContactAvoidanceFunction::ReducedContactAvoidanceFunction(const Model& model, const FullDynamicEquationFunction& dynamicEquation)
    : NamedInstance("Reduced Contact Avoidance Function")
    , AbilitySet(PARTIAL_X)
    , CoupledInputOutputSize(false)
    , ContactAvoidanceFunction(model, dynamicEquation.getActionVariable())
    , pimpl(new Pimpl(model, dynamicEquation))
{
    pimpl->_fullJacobian.setZero(_jacobian.rows(), _jacobian.cols());
    pimpl->_fullb.setZero(_b.size());
}

ReducedContactAvoidanceFunction::~ReducedContactAvoidanceFunction()
{

}


void ReducedContactAvoidanceFunction::updateb() const
{
    computeFullJacobian(JObst, pimpl->_fullJacobian);
    computeFullb(dJdqObst, distObst, velObst, pimpl->_fullb);

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


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

void ReducedContactAvoidanceFunction::doUpdateInputSizeEnd()
{
    pimpl->_fullJacobian.setZero(_jacobian.rows(), _jacobian.cols());
    pimpl->_fullb.setZero(_b.size());
}







ContactAvoidanceConstraint::ContactAvoidanceConstraint(const Model& model, double hpos, double margin)
    : _model(model)
    , _is_connected(false)
    , ControlConstraint()
{
    setHorizonOfPrediction(hpos);
    setMargin(margin);
}

double ContactAvoidanceConstraint::getHorizonOfPrediction() const
{
    return _hpos;
}

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


double ContactAvoidanceConstraint::getMargin() const
{
    return _margin;
}

void   ContactAvoidanceConstraint::setMargin(double newMargin)
{
    _margin = newMargin;
    if (_is_connected)
        _contactAvoidanceFunction->setMargin(newMargin);
}


void ContactAvoidanceConstraint::updateContactInformation(const Eigen::MatrixXd& _JObst, const Eigen::VectorXd& _dJdqOst, const Eigen::VectorXd& _distObst, const Eigen::VectorXd& _velObst)
{
    if (_is_connected)
        _contactAvoidanceFunction->updateContactInformation(_JObst, _dJdqOst, _distObst, _velObst);
    else
        throw std::runtime_error("[ContactAvoidanceConstraint::updateContactInformation] Constraint is not connected to the controller!");
}


void ContactAvoidanceConstraint::connectToController(const FullDynamicEquationFunction& dynamicEquation, bool useReducedProblem)
{
    LinearFunction* f = NULL;
    if (useReducedProblem)
        f = createReducedContactAvoidanceFunction(_model, dynamicEquation);
    else
        f = createFullContactAvoidanceFunction(_model);

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

    setHorizonOfPrediction(_hpos);
    setMargin(_margin);
}

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

ContactAvoidanceFunction* ContactAvoidanceConstraint::createFullContactAvoidanceFunction(const Model& model)
{
    _contactAvoidanceFunction = new FullContactAvoidanceFunction(model);
    return _contactAvoidanceFunction;
}

ContactAvoidanceFunction* ContactAvoidanceConstraint::createReducedContactAvoidanceFunction(const Model& model, const FullDynamicEquationFunction& dynamicEquation)
{
    _contactAvoidanceFunction = new ReducedContactAvoidanceFunction(model, dynamicEquation);
    return _contactAvoidanceFunction;
}