JointLimitConstraint.h

Go to the documentation of this file.

#ifndef __JOINTLIMITCONSTRAINT_H__
#define __JOINTLIMITCONSTRAINT_H__


#include "ocra/control/ControlConstraint.h"


namespace ocra
{

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

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

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

        void  setJointLimits(const Eigen::VectorXd& lowerLimits, const Eigen::VectorXd& upperLimits);
        void  setJointLowerLimits(const Eigen::VectorXd& newLowerLimits);
        void  setJointUpperLimits(const Eigen::VectorXd& newUpperLimits);

        const Eigen::VectorXd& getJointLowerLimits() const;
        const Eigen::VectorXd& getJointUpperLimits() const;

        void  setJointLimit(int i, double newLowerLimit, double newUpperLimit);
        void  setJointLowerLimit(int i, double newLowerLimit);
        void  setJointUpperLimit(int i, double newUpperLimit);

        double getJointLowerLimit(int i) const;
        double getJointUpperLimit(int i) const;

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

    protected:
        Eigen::VectorXd     jointLowerLimits;
        Eigen::VectorXd     jointUpperLimits;
        double              hpos; // the horizon of prediction for the future joint position;

        void                computeFullJacobian(int varDofs, int nIDofs, Eigen::MatrixXd& fullJacobian) const;
        void                computeFullb(const Eigen::VectorXd& gpos, const Eigen::VectorXd& gvel, Eigen::VectorXd& fullb) const;
};






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

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

    protected:
        virtual void updateb() const;

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





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

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

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

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

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













class JointLimitConstraint: public ControlConstraint
{
public:
    JointLimitConstraint(const Model& model, double hpos=.2);
    JointLimitConstraint(const Model& model, const Eigen::VectorXd& lowerLimits, const Eigen::VectorXd& upperLimits, double hpos=.2);
    virtual ~JointLimitConstraint() {};

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

    void  setJointLimits(const Eigen::VectorXd& lowerLimits, const Eigen::VectorXd& upperLimits);
    void  setJointLowerLimits(const Eigen::VectorXd& newLowerLimits);
    void  setJointUpperLimits(const Eigen::VectorXd& newUpperLimits);

    const Eigen::VectorXd& getJointLowerLimits() const;
    const Eigen::VectorXd& getJointUpperLimits() const;

    void  setJointLimit(int i, double newLowerLimit, double newUpperLimit);
    void  setJointLowerLimit(int i, double newLowerLimit);
    void  setJointUpperLimit(int i, double newUpperLimit);

    double getJointLowerLimit(int i) const;
    double getJointUpperLimit(int i) const;

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


private:
    JointLimitFunction* createFullJointLimitFunction(const Model& model);
    JointLimitFunction* createReducedJointLimitFunction(const Model& model, const FullDynamicEquationFunction& dynamicEquation);

    JointLimitFunction* _jointLimitFunction;
    const Model&   _model;
    bool                _is_connected;

    double              _hpos;
    Eigen::VectorXd     _lowerLimits;
    Eigen::VectorXd     _upperLimits;
};







 // end group constraint

}


#endif