bhkinematic.h

Go to the documentation of this file.

// ***************************************************************************
//                           bhkinematic.h
//
//    copyright            : (C) 2004 by tbaier
//    email                : tbaier@informatik.uni-hamburg.de
// ***************************************************************************
#ifndef BHKINEMATIC_H
#define BHKINEMATIC_H

#include <math.h>
#include <iostream>
#include "rccl.h"

#define NUM_FINGERS 3

#define JOINTS_PER_FINGER 2

namespace BHandKin{

  enum FINGER{
    F1 = 1,     
    F2 = 2,     
    F3 = 3      
  };
}
  struct FingerTransforms{
    TRSF_PTR Pos[NUM_FINGERS];  
  };

struct FingerJoints{
  double Joint[JOINTS_PER_FINGER]; 
};

struct BHandJoints{
  FingerJoints F[NUM_FINGERS];     
  FingerJoints Spread;             
  bool TorqueSwitch[NUM_FINGERS];  
  double TSActivationPoint[NUM_FINGERS]; 
};

// order of this strings is important (used by checkError)
  static const char* BHKinematicException_info[] = {
          "Angle value to big",
          "Angle value to small (< 0)",
          "Position value to big.",
          "Position value to small (< 0).",
          "Angle out of Range",
          "Position out of Range",
          "UNDEF"};


  class BHKinematicException
  {
   public:

    enum error
      {
        EXCP_ANGLE_TO_BIG,           
        EXCP_ANGLE_TO_SMALL,         
        EXCP_POSITION_TO_BIG,        
        EXCP_POSITION_TO_SMALL,      
        EXCP_ANGLE_OUT_OF_RANGE,     
        EXCP_POSITION_OUT_OF_RANGE,  
        EXCP_UNDEF                   
      };


    BHKinematicException(const error e);
    BHKinematicException(){};


    const void show();

    friend std::ostream& operator<<(std::ostream& os,const BHKinematicException& e);

    bool operator==(error e);

    error Err;

   protected:
    std::string Msg;
  };

//****************************//
//**                        **//
//****************************//
inline BHKinematicException::BHKinematicException(const error e)
{
   Msg = BHKinematicException_info[e];
   Err = e;
}

//****************************//
//**                        **//
//****************************//
inline const void BHKinematicException::show ()
{
  std::cout << Msg << std::endl;
}


//****************************//
//**                        **//
//****************************//
inline std::ostream& operator<<(std::ostream& os,const BHKinematicException& e)
{
  os << e.Msg << std::endl;
  return os;
}


//****************************//
//**                        **//
//****************************//
inline bool BHKinematicException:: operator==(error e)
{
  return (Err == e);
}



// *****************************************************************
// **                                                             **
// **                                                             **
// **                                                             **
// *****************************************************************

class BHKinematic
{

 public:
  BHKinematic();

  ~BHKinematic();


  void motor2Joint(double *result, int *motorValues);

  void motor2Joint(const int &finger, const int &motor, double &joint0, double &joint1);


  unsigned int joint2Motor(const BHandKin::FINGER finger, const double &joint);

  void joint2MotorJoint1(const BHandKin::FINGER finger , double &joint, int &motor, double &joint1);

  int joint2Spread(const double &joint);

  void joint2SpreadJoint1(double &joint, int &motor, double &joint1);

  void joint2Motor(int result[4], double joints[8]);


  void joint2Motor(int result[4], double joints[3], double spread);

  void updateJointValues(int *motorValues);

  //  const double * getActualJointValues();

  void showJointValues();

  void forwardKinematics();

  void forwardKinematics(const BHandKin::FINGER finger);

  TRSF_PTR getTrsf(const BHandKin::FINGER finger);

  void showTrsf() const;

  void showTrsf(const BHandKin::FINGER finger) const;

  void setTS(const BHandKin::FINGER finger, const bool flag = true);

  bool getTS(const BHandKin::FINGER finger);

  double getTSActivationPoint(const BHandKin::FINGER finger) const;
  

  bool invKinematic(const BHandKin::FINGER f, const VECT ziel, float &angle, float &spread);


  void resetKinematic();


  static const double FM2JNoTorque;
  static const double FM2JTorque;
  static const double SM2J;

  static const double J2FMNoTorque;
  static const double J2FMTorque;
  static const double J2SM;


  static const double JOINT1STDVALUE_DEG;
  static const double JOINT1MAX_DEG;

  static const double JOINT1STDVALUE_RAD;
  static const double JOINT1MAX_RAD;

  static const double JOINT0STDVALUE_DEG;
  static const double JOINT0MAX_DEG;

  static const double JOINT0STDVALUE_RAD;
  static const double JOINT0MAX_RAD;

  static const double JOINT1STDVALUE_KIN_DEG;
  static const double JOINT1STDVALUE_KIN_RAD;

  static const double JOINT1STDVALUE_DIFF_KIN_DEG;
  static const double JOINT1STDVALUE_DIFF_KIN_RAD;


  static const double Aw;
  static const double A1;
  static const double A2;
  static const double A3;
  static const double Dw;
  static const double D3;
  static const double Rho2;
 

 protected:

  bool reducedProblem(VECT &ziel, float &angle);


  double clipJ1(const double &value)const;

  double clipJ0(const double &value) const;



  BHandJoints JointValues;

  FingerTransforms FingerTrsf;

};


// *************************************
// **                                 **
// *************************************
inline double BHKinematic::clipJ1(const double &value)const
{
  double result = (value < JOINT1STDVALUE_DEG)? 45.0 : value;
  result        = (result > JOINT1MAX_DEG) ? JOINT1MAX_DEG : result;
  return result;
}

// *************************************
// **                                 **
// *************************************
inline double BHKinematic::clipJ0(const double &value)const
{
  double result = (value < JOINT0STDVALUE_DEG) ? 2.46 : value;
  result        = (result > JOINT0MAX_DEG) ? JOINT0MAX_DEG : result;
  return result;
}

// *************************************
// **                                 **
// *************************************
inline TRSF_PTR BHKinematic::getTrsf(const BHandKin::FINGER finger)
{
  return FingerTrsf.Pos[finger-1];
}

// *************************************
// **                                 **
// *************************************
inline void BHKinematic::setTS(const BHandKin::FINGER finger, const bool flag)
{
  JointValues.TorqueSwitch[finger-1] = flag;
  
  JointValues.TSActivationPoint[finger-1] = 
    (flag) ? JointValues.F[finger-1].Joint[0] :  0.0;
    
  std::cout << " inline void BHKinematic::setTS(const BHandKin::FINGER finger, const bool flag)\n"
            << " *** Value: " << JointValues.TSActivationPoint[finger-1] << std::endl;
    

}


// *************************************
// **                                 **
// *************************************
inline bool BHKinematic::getTS(const BHandKin::FINGER finger)
{
  return JointValues.TorqueSwitch[finger-1];
}

// *************************************
// **                                 **
// *************************************
inline double BHKinematic::getTSActivationPoint(const BHandKin::FINGER finger) const
{
  return JointValues.TSActivationPoint[finger-1];
}


#endif

---

Tim Baier bhandlib v0.25

Generated Wed Aug 16 14:57:09 2006 by 1.4.7