/opt/ros/diamondback/stacks/graspit_simulator/graspit/graspit_source/include/joint.h

Go to the documentation of this file.

//######################################################################
//
// GraspIt!
// Copyright (C) 2002-2009  Columbia University in the City of New York.
// All rights reserved.
//
// GraspIt! is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// GraspIt! is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with GraspIt!.  If not, see <http://www.gnu.org/licenses/>.
//
// Author(s): Andrew T. Miller and Matei T. Ciocarlie
//
// $Id: joint.h,v 1.20 2009/08/19 23:17:19 cmatei Exp $
//
//######################################################################

#ifndef JOINT_H

#include <list>
#include <vector>

#include "mytools.h"
#include "matvec3D.h"

class KinematicChain;
class DynJoint;
class Robot;
class Matrix;

class SoTransform;
class TiXmlElement;

enum JointT {REVOLUTE,PRISMATIC};


class DHTransform {

  vec3 dtrans;

  vec3 atrans;

  transf tr4TimesTr3;

  transf tr2;

  transf tr1;

  transf tran;

  double theta;

  double d;

  double a;

  double alpha;

public:
  DHTransform(double thval=0.0,double dval=0.0,double aval=0.0,
              double alval=0.0);
  DHTransform( const DHTransform *d ){
          memcpy( this, d, sizeof(DHTransform));
  }

  double getD() const {return d;}

  double getTheta() const {return theta;}

  const transf& getTran() const {return tran;}
  
  transf getTran(double thetaVal,double dVal) const
    {
      return tr4TimesTr3 * translate_transf(vec3(0,0,dVal)) * 
        rotate_transf(thetaVal,vec3(0,0,1));
    }

  void setD(double q);
  void setTheta(double q);
};


class Joint {
protected:
  int DOFnum;

  KinematicChain *owner;

  int jointNum;

  bool draggerAttached;

  double dynamicsVal;

  double velocity;

  double minVal,maxVal;

  double mCouplingRatio;  

  double c;  

  double f1;

  double f0;

  double mK;

  double mRestVal;

  vec3 worldAxis;

  DHTransform *DH;

  SoTransform *IVTran;

public:
  DynJoint *dynJoint;

 Joint(KinematicChain *k) : DOFnum(0), owner(k), jointNum(-1), draggerAttached(false), dynamicsVal(0.0),
    velocity(0.0),minVal(0.0),maxVal(0.0),mCouplingRatio(1.0),c(0.0),f1(0.0),f0(0.0),mK(0.0),
        mRestVal(0.0), DH(NULL),
    IVTran(NULL), dynJoint(NULL) {IVTran = new SoTransform(); IVTran->ref();}

  virtual ~Joint();

  virtual int initJointFromXml(const TiXmlElement* root, int jnum) =0;

  void cloneFrom(const Joint *original);

  virtual int setVal(double q)=0;

  void setMin(double min) {minVal = min;}

  void setMax(double max) {maxVal = max;}

  virtual void applyInternalWrench(double magnitude)=0;

  virtual void applyPassiveInternalWrenches();
 
  void setVelocity(double v) {velocity = v;}

  void setDynamicsVal(double v) {dynamicsVal = v;}

  void setWorldAxis(const vec3 &wa) {worldAxis = normalise(wa);}

  void setDraggerAttached(bool b) {draggerAttached = b;}

  void setSpringStiffness(double k) {mK = k;}

  void setRestValue(double r){mRestVal = r;}

  //Accessors:

  int getNum() const {return jointNum;}

  int getChainNum() const;

  virtual double getVal() const =0;

  double getDisplacement() const {return getVal() - mRestVal;}

  virtual JointT getType() const =0;

  virtual transf getTran(double jointVal) const =0;

  virtual transf getDynamicsTran() const =0;

  int getDOFNum() const {return DOFnum;}

  double getCouplingRatio() const {return mCouplingRatio;}

  double getSpringStiffness() {return mK;}

  double getOffset() const {return c;}

  double getMin() const {return minVal;}

  double getMax() const {return maxVal;}

  double getVelocity() const {return velocity;}

  double getFriction() const {
    return -f1 * velocity + (velocity<0 ? f0 : (velocity>0 ? -f0 : 0.0));
  }

  double getSpringForce() const;

  double getDynamicsVal() const {return dynamicsVal;}

  transf const& getTran() const {return DH->getTran();}

  vec3 const& getWorldAxis() const {return worldAxis;}

  DHTransform *getDH() const {return DH;}

  SoTransform *getIVTran() const {return IVTran;}

  static Matrix jacobian(const Joint *joint, const transf &jointTran, 
                                                 const transf &toTarget, bool worldCoords);
};



class PrismaticJoint : public Joint {

public:

  PrismaticJoint(KinematicChain *k) : Joint(k) {}

  virtual int initJointFromXml(const TiXmlElement* root, int jnum);
  
  virtual void applyInternalWrench(double magnitude);

  virtual int setVal(double q);

  virtual double getVal() const {return DH->getD() - c;}

  virtual JointT getType() const {return PRISMATIC;}

  transf getTran(double jointVal) const {
    return DH->getTran(DH->getTheta(),jointVal + c);
  }

  transf getDynamicsTran() const {
    return DH->getTran(DH->getTheta(),dynamicsVal);
  }

};


class RevoluteJoint : public Joint {

public:

  RevoluteJoint(KinematicChain *k) : Joint(k) {}

  virtual int initJointFromXml(const TiXmlElement* root, int jnum);

  virtual double getVal() const {return DH->getTheta() - c;}

  virtual void applyInternalWrench(double magnitude);

  virtual int setVal(double q);

  virtual JointT getType() const {return REVOLUTE;}


  transf getTran(double jointVal) const {
    return DH->getTran(jointVal + c,DH->getD());
  }
  transf getDynamicsTran() const {
    return DH->getTran(dynamicsVal,DH->getD());
  }

};

#define JOINT_H
#endif

---

graspit
Author(s):
autogenerated on Wed Jan 25 10:58:51 2012