aibojoint.h

Go to the documentation of this file.

// ***************************************************************************
//                           aibojoint.h
//
//    copyright            : (C) 2003 by tbaier
//    email                : tbaier@informatik.uni-hamburg.de
// ***************************************************************************

#ifndef AIBOJOINT_H
#define AIBOJOINT_H


//std includes
#include <math.h>

//AIBO includes
#include <OPENR/ODataFormats.h>
#include <OPENR/OObject.h>
#include <OPENR/OPENRAPI.h>
#include <OPENR/OSyslog.h>
#include <Types.h>

#include "aibosensor.h"
#include "defines.h"

enum JOINT_STATE{
  J_UNDEF,
  J_START
};

enum JOINT_UNIT{
  ANGLE_DEG, 
  ANGLE_RAD  
};

static const int NUM_JOINTS = 15;


class AiboJoint  {

 public: 
    AiboJoint();

    virtual ~AiboJoint();


    virtual void OpenPrimitives() = 0;
    
    OPrimitiveID GetJointID(int j) const;


    RCRegion* FindFreeRegion() const;
    
    int FindFreeRegionIndex() const;

    RCRegion* GetRegion(int index = 0);

    
    virtual void NewCommandVectorData() = 0;

    void SetCommandVector(int index, OCommandVectorData *vec);

   

    
    void SetJointOPENR(slongword angle, 
                       OPrimitiveID joint, int jointIndex, 
                       int cmdVecIndex);

    void SetJointOPENR(double angle, 
                       OPrimitiveID joint, int jointIndex, 
                       int cmdVecIndex);

    void SetJoint(double start, double angle,
                  OPrimitiveID joint, int jointIndex, 
                  int cmdVecIndex);
    
    
    void SetJoint(double start, double angle,
                  OPrimitiveID joint, int jointIndex, 
                  int numFrames, int cmdVecIndex);
    
    void SetJoint(double angle,
                  OPrimitiveID joint, int jointIndex, 
                  int cmdVecIndex);


    void SetJoint(double angle,
                  OPrimitiveID joint, int jointIndex, 
                  int numFrames, int cmdVecIndex);
    
    
    void SetJoint(slongword angle, 
                  OPrimitiveID joint, int jointIndex, 
                  int cmdVecIndex);


    void SetJoint(slongword angle,
                  OPrimitiveID joint, int jointIndex, 
                  int numFrames, int cmdVecIndex);


    
    
    void SetJointRelOPENR(slongword angle, 
                          OPrimitiveID joint, int jointIndex, 
                          int cmdVecIndex);
    
       
    void SetJointRelOPENR(double angle, 
                          OPrimitiveID joint, int jointIndex, 
                          int cmdVecIndex);
   
    void SetJointRel(double start, double angle, 
                     OPrimitiveID joint, int jointIndex, 
                     int cmdVecIndex);

     
    void SetJointRel(double start, double angle, 
                     OPrimitiveID joint, int jointIndex, 
                     int numFrames, int cmdVecIndex);



    void SetJointRel(double angle, 
                     OPrimitiveID joint, int jointIndex, 
                     int cmdVecIndex);
    
    void SetJointRel(double angle,
                     OPrimitiveID joint, int jointIndex, 
                     int numFrames, int cmdVecIndex);



    void SetJointRel(slongword angle, 
                     OPrimitiveID joint, int jointIndex, 
                     int cmdVecIndex);
    
    void SetJointRel(slongword angle,
                     OPrimitiveID joint, int jointIndex, 
                     int numFrames, int cmdVecIndex);




    void RefreshCommandVector(int cmdVecIndex);
    
     void RefreshCommandVectors();

     void RefreshJointValue(int index);

     void RefreshJointValues();



    
    double GetJointValue(int index) const;

    
    double GetOPENRJointValue(OPrimitiveID joint) const;


    double GetOPENRJointValue(int joint) const;
    
    
    virtual double GetMaxJointValue(int index, JOINT_UNIT unit = ANGLE_RAD) const = 0;


    virtual double GetMinJointValue(int index, JOINT_UNIT unit = ANGLE_RAD ) const = 0;
    
    virtual double GetMaxJointVelocity(int index, JOINT_UNIT unit = ANGLE_RAD) const = 0 ;
    


    
    virtual void SetJointGain()= 0;
 
    void EnableJointGain();
       
    void EnableJointGain(OPrimitiveID id);
    
    void DisableJointGain();

    void DisableJointGain(OPrimitiveID id);
      



  

    JOINT_STATE GetState() const;

    void SetState(JOINT_STATE st);


    int GetNumJoints() const;

    static const int    MaxJointRegions = 2;
    
    RCRegion*           Region[MaxJointRegions];

 protected:
    
    // VARS---------------------------------------------------------------------

    JOINT_STATE         State;

    int                 NumJoints;

    double              *JointValues; 
  
    OPrimitiveID        *JointID;
    
    static const word   PSHIFT   = 0x000e;
    static const word   ISHIFT   = 0x0002;
    static const word   DSHIFT   = 0x000f;
    
    double FrameFactor[ocommandMAX_FRAMES];

};


// ******************************************
// **                                      **
// ******************************************
inline OPrimitiveID AiboJoint::GetJointID(int j) const
{
  return JointID[j];
};


// ******************************************
// **                                      **
// ******************************************
inline int AiboJoint::GetNumJoints() const
{
  return NumJoints;
};


// ******************************************
// **                                      **
// ******************************************
inline JOINT_STATE AiboJoint::GetState() const
{
  return State;
};


// ******************************************
// **                                      **
// ******************************************
inline void AiboJoint::SetState(JOINT_STATE st)
{
  State = st;
};


// ******************************************
// **                                      **
// ******************************************
inline void AiboJoint::EnableJointGain()
{
  for(int i = NumJoints-1; i >= 0; i--)
    EnableJointGain(JointID[i]);
};


// ******************************************
// **                                      **
// ******************************************
inline void AiboJoint::EnableJointGain(OPrimitiveID id)
{
  OPENR::EnableJointGain(id);
};


// ******************************************
// **                                      **
// ******************************************
inline void AiboJoint::DisableJointGain()
{
  for(int i = NumJoints-1; i >= 0; i--)
    DisableJointGain(JointID[i]);
};


// ******************************************
// **                                      **
// ******************************************
inline void AiboJoint::DisableJointGain(OPrimitiveID id)
{
  OPENR::DisableJointGain(id);
};


// ******************************************
// **                                      **
// ******************************************
inline RCRegion* AiboJoint::GetRegion(int index)
{
  return Region[index];
};
 

// ******************************************
// **                                      **
// ******************************************
inline void AiboJoint::SetJointOPENR(slongword angle, 
                                     OPrimitiveID joint, int jointIndex, 
                                     int cmdVecIndex)
{
  SetJointOPENR(((double) angle)/MICRO,
                joint,jointIndex, 
                cmdVecIndex);
};


// ******************************************
// **                                      **
// ******************************************
inline void AiboJoint::SetJoint(slongword angle,
                                OPrimitiveID joint, int jointIndex,
                                int cmdVecIndex)
{ 
  SetJoint(JointValues[jointIndex], ((double)angle)/MICRO,
           joint, jointIndex,
           cmdVecIndex);
};


// ******************************************
// **                                      **
// ******************************************
inline void AiboJoint::SetJoint(double angle,
                                OPrimitiveID joint, int jointIndex,
                                int cmdVecIndex)
{ 
  SetJoint(JointValues[jointIndex], angle,
           joint, jointIndex,
           cmdVecIndex);
};


// ******************************************
// **                                      **
// ******************************************
inline void AiboJoint::SetJoint(double angle,
                                OPrimitiveID joint, int jointIndex,
                                int numFrames, int cmdVecIndex)
{ 
  SetJoint(JointValues[jointIndex], angle,
           joint, jointIndex,
           numFrames, cmdVecIndex);
};


// ******************************************
// **                                      **
// ******************************************
inline void AiboJoint::SetJoint(slongword angle,
                                OPrimitiveID joint, int jointIndex,
                                int numFrames, int cmdVecIndex)
{ 
  SetJoint(JointValues[jointIndex], (double)angle/MICRO,
           joint, jointIndex,
           numFrames, cmdVecIndex);
};


// ******************************************
// **                                      **
// ******************************************
inline void AiboJoint::SetJointRelOPENR(slongword angle, 
                                        OPrimitiveID joint, int jointIndex, 
                                        int cmdVecIndex)
{
  SetJointRelOPENR(((double) angle)/MICRO,
                   joint, jointIndex,
                   cmdVecIndex);
};


// ******************************************
// **                                      **
// ******************************************
inline void AiboJoint::SetJointRel(double angle, 
                                   OPrimitiveID joint, int jointIndex,
                                   int cmdVecIndex)
{ 
  SetJointRel(JointValues[jointIndex], angle, 
              joint, jointIndex, 
              cmdVecIndex);
  
};


// ******************************************
// **                                      **
// ******************************************
inline void AiboJoint::SetJointRel(double angle, 
                                   OPrimitiveID joint, int jointIndex,
                                   int numFrames, int cmdVecIndex)
{ 
  SetJointRel(JointValues[jointIndex], angle, 
              joint, jointIndex, 
              numFrames, cmdVecIndex);
  
};


// ******************************************
// **                                      **
// ******************************************
inline void AiboJoint::SetJointRel(slongword angle, 
                                   OPrimitiveID joint, int jointIndex,
                                   int cmdVecIndex)
{ 
  SetJointRel(JointValues[jointIndex], ((double) angle)/MICRO, 
              joint, jointIndex, 
              cmdVecIndex);
  
};


// ******************************************
// **                                      **
// ******************************************
inline void AiboJoint::SetJointRel(slongword angle, 
                                   OPrimitiveID joint, int jointIndex,
                                   int numFrames, int cmdVecIndex)
{ 
  SetJointRel(JointValues[jointIndex],  ((double) angle)/MICRO, 
              joint, jointIndex, 
              numFrames, cmdVecIndex);
  
};


// ******************************************
// **                                      **
// ******************************************
inline double AiboJoint::GetJointValue(int index) const
{
  return JointValues[index];
};


// ******************************************
// **                                      **
// ******************************************
inline void AiboJoint::RefreshCommandVectors()
{
  for(int i = MaxJointRegions-1; i >= 0; i--)
    RefreshCommandVector(i);
};


// ******************************************
// **                                      **
// ******************************************
inline void AiboJoint::RefreshJointValues()
{
  for(int i = NumJoints-1; i >= 0; i--)
    RefreshJointValue(i);
};


#endif //define AIBOJOINT_H

---

Tim Baier AiboLib v0.2.4

Generated Thu Jan 19 11:54:29 2006 by 1.4.3