taoControlJt.h

Go to the documentation of this file.

#ifndef _taoControlJt_h
#define _taoControlJt_h

#include "taoTypes.h"
#include "taoJoint.h"
#include "taoControl.h"
#include "deMath.h"

class taoJoint;
class taoDNode;
class taoNodeRoot;

class taoJCParam
{
        friend class taoControlJt;
public:
        taoJCParam(taoJoint* joint)
        {
                _joint = joint;
                _controlOn = 1;
                _useGlobalGains = 1;
                _goalVelocityAuto = 0;
                _jtLimitSoftOn = 0;
                _jlU = 0;
                _jlL = 0;
                _kpU = 0;
                _kvU = 0;
                _kpL = 0;
                _kvL = 0;
                _kp = 0;
                _kv = 0;
                reset(-1);
        }

        taoDVar* getDVar() { return _joint->getDVar(); }

        virtual void reset(deFloat time) { _tg = time; _tgo = time; }
        virtual void moveGoal2Old() {}

        virtual void PDLaw(taoJCParam* jcpRoot, deFloat s, deFloat periodInv) {}
        virtual void ZeroLaw() {}
        virtual void addJointLimitSoft() {}

        virtual void setGoalPosition(deFrame* goal) {}

        deInt& controlOn() { return _controlOn; }               
        deInt& useGlobalGains() { return _useGlobalGains; }     
        deInt& goalVelocityAuto() { return _goalVelocityAuto; } 
        deInt& jtLimitSoftOn() { return _jtLimitSoftOn; }       
        deFloat& jlU() { return _jlU; }                 
        deFloat& jlL() { return _jlL; }                 
        deFloat& kpU() { return _kpU; }                 
        deFloat& kvU() { return _kvU; }                 
        deFloat& kpL() { return _kpL; }                 
        deFloat& kvL() { return _kvL; }                 
        deFloat& kp() { return _kp; }                   
        deFloat& kv() { return _kv; }

protected:
        deInt _controlOn;               
        deInt _useGlobalGains;  
        deInt _goalVelocityAuto;        
        deInt _jtLimitSoftOn;   
        deFloat _jlU;                   
        deFloat _jlL;                   
        deFloat _kpU;                   
        deFloat _kvU;                   
        deFloat _kpL;                   
        deFloat _kvL;                   
        deFloat _kp;                    
        deFloat _kv;                    

protected:
        taoJoint* _joint;
        deFloat _tg;
        deFloat _tgo;
};

class taoJCParamSpherical : public taoJCParam
{
public:
        taoJCParamSpherical(taoJoint* j) : taoJCParam(j) { reset(0); }
        virtual void reset(deFloat time) 
        {
                taoJCParam::reset(time);
                _Qo.identity();
                _dQo.zero();
                _Qg.identity();
                _Qgo.identity();
                _dQg.zero();
        }
        taoVarSpherical* getVarSpherical() { return (taoVarSpherical*)getDVar(); }

        virtual void moveGoal2Old() { _tgo = _tg; _Qgo = _Qg; }

        virtual void PDLaw(taoJCParam* jcpRoot, deFloat s, deFloat periodInv);
        virtual void ZeroLaw();

        virtual void addJointLimitSoft();

        virtual void setGoalPosition(deFrame* goal);

        deQuaternion& Qg() { return _Qg; }
        deVector3& dQg() { return _dQg; }

private:
        deQuaternion _Qo;
        deVector3 _dQo;
        deQuaternion _Qg;
        deQuaternion _Qgo;
        deVector3 _dQg;
};

class taoJCParamDOF1 : public taoJCParam
{
public:
        taoJCParamDOF1(taoJoint* j) : taoJCParam(j) { reset(0); }
        virtual void reset(deFloat time)
        {
                taoJCParam::reset(time);
                _Qo = 0;
                _dQo = 0;
                _Qg = 0;
                _Qgo = 0;
                _dQg = 0;
        }
        taoVarDOF1* getVarDOF1() { return (taoVarDOF1*)getDVar(); }

        virtual void moveGoal2Old() { _tgo = _tg; _Qgo = _Qg; }

        virtual void PDLaw(taoJCParam* jcpRoot, deFloat s, deFloat periodInv);
        virtual void ZeroLaw();

        virtual void addJointLimitSoft();

        virtual void setGoalPosition(deFrame* goal) {}

        deFloat& Qg() { return _Qg; }
        deFloat& dQg() { return _dQg; }

protected:
        deFloat _Qo;
        deFloat _dQo;
        deFloat _Qg;
        deFloat _Qgo;
        deFloat _dQg;
};

class taoJCParamPrismatic : public taoJCParamDOF1
{
public:
        taoJCParamPrismatic(taoJoint* j) : taoJCParamDOF1(j) {}
        virtual void setGoalPosition(deFrame* goal);
};

class taoJCParamRevolute : public taoJCParamDOF1
{
public:
        taoJCParamRevolute(taoJoint* j) : taoJCParamDOF1(j) {}
        virtual void setGoalPosition(deFrame* goal);
};

class taoControlJt : public taoControl
{
public:
        taoControlJt(taoNodeRoot* root);

        virtual ~taoControlJt();
        virtual void deleteParamTree(taoDNode* node);

        virtual deFloat time() { return _time; }
        virtual void reset(deFloat time);

        virtual void control(deFloat time);


        virtual void setGoalPosition(taoDNode* node, deFrame* F, deFloat timestamp);

        virtual taoControlJt* getControlJt() { return this; }

        virtual taoJCParam* taoControlJt::createJCParam(taoJoint* joint);

        virtual void jointForceTree(taoDNode* node);
        virtual void zeroTorqueControlOffTree(taoDNode* node);

        void resetParamTree(taoDNode* node);

        taoNodeRoot* root() { return _root; }
        
        taoJCParam* jcpGlobal() { return _jcpGlobal; }                  
        deInt& doDynamicControl() { return _doDynamicControl; }         
        deInt& includeG() { return _includeG; }                         
        taoControlType& controlLawType() { return _controlLawType; }    

private:
        deFloat _time;
        taoNodeRoot* _root;
        
        taoJCParam* _jcpGlobal;                 
        deInt _doDynamicControl;                
        deInt _includeG;                                
        taoControlType _controlLawType; 

private:
        void _PDLaw(taoDNode* node);
        void _ZeroLaw(taoDNode* node);
        void _FloatLaw(taoDNode* node);

        void _NewParamTree(taoDNode* node);
};

#endif // _taoControlJt_h

---