ContactAvoidanceConstraint.h

Go to the documentation of this file.

#ifndef __CONTACTAVOIDANCECONSTRAINT_H__
#define __CONTACTAVOIDANCECONSTRAINT_H__


#include "ocra/control/ControlConstraint.h"

namespace ocra
{

class ContactAvoidanceFunction: public LinearFunction
{
    public:
        typedef LinearFunction  functionType_t;     //< alias on the type of the mother class. Needed to duplicate the function tree.

        ContactAvoidanceFunction(const Model& m, Variable& var);
        ~ContactAvoidanceFunction();

        // get/set horizon of prediction
        double getHorizonOfPrediction() const;
        void   setHorizonOfPrediction(double newHpos);
        double getMargin() const;
        void   setMargin(double newMargin);

        void updateContactInformation(const Eigen::MatrixXd& _JObst, const Eigen::VectorXd& _dJdqOst, const Eigen::VectorXd& _distObst, const Eigen::VectorXd& _velObst);

    private: // Forbid copy
        ContactAvoidanceFunction(ContactAvoidanceFunction&);
        ContactAvoidanceFunction& operator= (const ContactAvoidanceFunction&);

    protected:
        void updateJacobian() const;
        void updateb()        const;
        //    void buildA();

        //    virtual void doUpdateInputSizeBegin();
        //    virtual void doUpdateInputSizeEnd();

        virtual void doUpdateDimensionBegin(int newDimension);
        virtual void doUpdateDimensionEnd(int oldDimension);

    protected:
//        const Model&   _model;
//        Variable&      _q_ddot;

        double hpos; // the horizon of prediction for the future joint position;
        double margin;

        Eigen::MatrixXd JObst;
        Eigen::VectorXd dJdqObst;
        Eigen::VectorXd distObst;
        Eigen::VectorXd velObst;

        void computeFullJacobian(const Eigen::MatrixXd& _JObst, Eigen::MatrixXd& fullJacobian) const;
        void computeFullb(const Eigen::VectorXd& _dJdqObst, const Eigen::VectorXd& _distObst, const Eigen::VectorXd& _velObst, Eigen::VectorXd& fullb) const;

};






class FullContactAvoidanceFunction: public ContactAvoidanceFunction
{
    public:
        typedef LinearFunction  functionType_t;     //< alias on the type of the mother class. Needed to duplicate the function tree.

        FullContactAvoidanceFunction(const Model& model);
        ~FullContactAvoidanceFunction();

    protected:
        virtual void updateJacobian() const;
        virtual void updateb()        const;

};




class ReducedContactAvoidanceFunction: public ContactAvoidanceFunction
{
    public:
        typedef LinearFunction  functionType_t;     //< alias on the type of the mother class. Needed to duplicate the function tree.

        ReducedContactAvoidanceFunction(const Model& model, const FullDynamicEquationFunction& dynamicEquation);
        ~ReducedContactAvoidanceFunction();

    protected:
        virtual void updateb()        const;

    private:
        struct Pimpl;
        boost::shared_ptr<Pimpl> pimpl;

        virtual void doUpdateInputSizeBegin();
        virtual void doUpdateInputSizeEnd();
};








class ContactAvoidanceConstraint: public ControlConstraint
{
public:
    ContactAvoidanceConstraint(const Model& model, double hpos, double margin);
    virtual ~ContactAvoidanceConstraint() {};

    double getHorizonOfPrediction() const;
    void   setHorizonOfPrediction(double newHpos);

    double getMargin() const;
    void   setMargin(double newMargin);

    void updateContactInformation(const Eigen::MatrixXd& _JObst, const Eigen::VectorXd& _dJdqOst, const Eigen::VectorXd& _distObst, const Eigen::VectorXd& _velObst);

protected:
    virtual void connectToController(const FullDynamicEquationFunction& dynamicEquation, bool useReducedProblem);
    virtual void disconnectFromController();


private:
    ContactAvoidanceFunction* createFullContactAvoidanceFunction(const Model& model);
    ContactAvoidanceFunction* createReducedContactAvoidanceFunction(const Model& model, const FullDynamicEquationFunction& dynamicEquation);

    ContactAvoidanceFunction* _contactAvoidanceFunction;
    const Model&   _model;
    bool                _is_connected;

    double              _hpos;
    double              _margin;
};

 // end group constraint

}


#endif