robot.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: robot.h,v 1.55 2010/07/09 02:33:45 cmatei Exp $
//
//######################################################################

#ifndef ROBOT_H

#include <vector>
#include <map>
#include <QTextStream>
#include <QString>

#include "collisionStructures.h"
#include "mytools.h"
#include "kinematicChain.h"
#include "dof.h"
#include "worldElement.h"
#include "body.h"
#include "joint.h"

class Grasp;
class GloveInterface;
class CyberGlove;
class EigenGraspInterface;
class Matrix;
class TiXmlElement;


class Robot : public WorldElement {
  Q_OBJECT

signals:
  void configurationChanged();

  void userInteractionStart();

  void userInteractionEnd();

  void moveDOFStepTaken(int numCols, bool &stopRequest);

private:
  // Connection to a parent robot 
  Robot *parent;
  int parentChainNum;
  transf tranToParentEnd;
  Link *mountPiece;

protected:

  // The structure of the robot itself  
  int numChains;
  int numDOF;
  int numJoints;
  std::vector<KinematicChain *> chainVec;
  std::vector<DOF *> dofVec;
  Link *base;

  // Save and restore state
  transf savedTran;
  QString savedDOF;
  bool savedState;

  bool mRenderGeometry;

  // Dynamic simulation: parameters and trajectory generation
  double dofUpdateTime;
  double defaultTranslVel;
  double defaultRotVel;

  // Input from external hardware
  bool mUseCyberGlove;
  GloveInterface *mGloveInterface;
  bool mUsesFlock;
  int mBirdNumber;
  SoSeparator *IVFlockRoot;
  FlockTransf mFlockTran;

  EigenGraspInterface* mEigenGrasps;


  transf approachTran;
  SoSeparator *IVApproachRoot;

  void addFlockSensorGeometry();
  void addApproachGeometry();

  virtual bool simpleContactsPreventMotion(const transf& motion) const;

  inline virtual void simpleSetTran(transf const& tr);

  virtual void setJointValues(const double* jointVals);
  virtual void setJointValuesAndUpdate(const double* jointVals);
  inline void getJointValues(double* jointVals) const;
  inline void updateDofVals(double *dofVals);
  virtual bool getJointValuesFromDOF(const double *desireddofVals, double *actualDofVals,
                                                                         double *jointVals, int *stoppedJoints);
  Matrix getJacobianJointToDOF(int chainNum);

  bool jumpDOFToContact(double *desiredVals, int *stoppedJoints, int *numCols = NULL);
  int interpolateJoints(double *initialVals, double *finalVals, CollisionReport *colReport, 
                                                double *interpolationTime);
  void stopJointsFromLink(Link *link, double *desiredJointVals, int *stoppedJoints);

  virtual void getBodyList(std::vector<Body*> *bodies);

  friend void KinematicChain::updateLinkPoses();
  friend void KinematicChain::attachRobot(Robot *r,const transf &offsetTr);

 public:

  Robot(World *w,const char *name) : WorldElement(w,name) {
    parent=NULL; parentChainNum = -1;  mountPiece=NULL;
    numChains = numDOF = 0; base = NULL; dofUpdateTime=0.0;
        mUseCyberGlove = false; mGloveInterface = NULL; mUsesFlock = false;
        mEigenGrasps = NULL; mRenderGeometry = true; savedState = false;
        approachTran = transf::IDENTITY;
        // temporary
        defaultTranslVel = 50; defaultRotVel = M_PI/4.0;
  }
  
  virtual ~Robot();

  //--------------------------load and populate a robot-------------------------------

  virtual int loadFromXml(const TiXmlElement* root,QString rootPath);

  virtual void cloneFrom(Robot *original);

  //--------------------------statics-------------------------------------------------

  bool moveDOFToContacts(double *desiredVals, double *desiredSteps, bool stopAtContact, 
                                                 bool renderIt=false);

  virtual int setTran(transf const& tr);

  inline void forceDOFVal(int dofNum,double val);

  inline void forceDOFVals(double *dofVals);

  //-------------------------dynamics-------------------------------------------------

  void setDesiredDOFVals(double *dofVals);

  bool contactSlip();

  bool dynamicAutograspComplete();

  virtual void updateJointValuesFromDynamics();

  void applyJointPassiveInternalWrenches();

  virtual void DOFController(double timeStep);

  virtual void buildDOFCouplingConstraints(std::map<Body*,int> &islandIndices, int numBodies, 
                                                                                   double* Nu,double *eps,int &ncn);

  virtual void buildDOFLimitConstraints(std::map<Body*,int> &islandIndices, int numBodies, 
                                                                            double* H, double *g, int &hcn);

  //-------------------------get position and pose information------------------------

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

  virtual void fwdKinematics(double *dofVals,std::vector<transf>& trVec,int chainNum);

  virtual int invKinematics(const transf& endTran,double* dofVals, int chainNum); 

  inline void getDOFVals(double *dofVals) const;

  inline void storeDOFVals(double *dofVals) const;

  //------------------------- static checks and range of motion

  inline bool checkDOFVals(double *dofVals) const;

  inline bool checkSetDOFVals(double *dofVals) const;

  virtual bool contactsPreventMotion(const transf& motion) const;

  bool checkDOFPath(double *desiredVals, double desiredStep);

  inline void updateDOFFromJoints(double *jointVals);

  inline double jointLimitDist() const;

  //-------------------------connections to real input devices (CyberGlove, FLock etc.)

  bool useCyberGlove(){return mUseCyberGlove;}

  void setGlove(CyberGlove *glove);

  void processCyberGlove();

  GloveInterface* getGloveInterface(){return mGloveInterface;}

  int getBirdNumber(){return mBirdNumber;}

  bool usesFlock(){return mUsesFlock;}

  const FlockTransf* getFlockTran() const {return &mFlockTran;}

  FlockTransf* getFlockTran(){return &mFlockTran;}

  //-------------------------eigengrasps----------------------------------------------

  int loadEigenData(QString filename);

  int useIdentityEigenData();

  const EigenGraspInterface* getEigenGrasps() const {return mEigenGrasps;}

  EigenGraspInterface* getEigenGrasps() {return mEigenGrasps;}

  //-------------------------save and load the state----------------------------------

  virtual QTextStream &readDOFVals(QTextStream &is);

  virtual QTextStream &writeDOFVals(QTextStream &os);

  virtual void saveState();

  virtual void restoreState();

  //-------------------------contacts-------------------------------------------------

  int getNumContacts(Body* body = NULL);

  std::list<Contact*> getContacts(Body* body = NULL);

  void breakContacts();

  int getNumVirtualContacts();

  void showVirtualContacts(bool on);

  int loadContactData(QString filename);

  //-------------------------attached robots------------------------------------------

  Robot *getBaseRobot() {if (parent) return parent->getBaseRobot(); return this;}

  Robot *getParent() {return parent;}

  int getParentChainNum() {return parentChainNum;}

  const transf &getTranToParentEnd() {return tranToParentEnd;}

  void getAllAttachedRobots(std::vector<Robot *> &robotVec);

  void attachRobot(Robot *r,int chainNum,const transf &offsetTr);

  void detachRobot(Robot *r);

  Link *importMountPiece(QString filename);

  Link *getMountPiece() const {return mountPiece;}

  //-------------------------other functions and accessors----------------------------

  bool snapChainToContacts(int chainNum, CollisionReport colReport);

  void setName(QString newName);

  int getNumLinks() const;

  int getNumDOF() const {return numDOF;}

  DOF *getDOF(int i) const {return dofVec[i];}

  float getDOFDraggerScale(int i) const {return dofVec[i]->getDraggerScale();}

  int getNumChains() const {return numChains;}

  int getNumJoints() const {return numJoints;}

  KinematicChain *getChain(int i) const {return chainVec[i];}

  Link *getBase() const {return base;}

  void setTransparency(float t);

  double getDefaultTranslVel() const {return defaultTranslVel;}

  double getDefaultRotVel() const  {return defaultRotVel;}

  void setDefaultTranslVel(double v) {defaultTranslVel = v;}

  void setDefaultRotVel(double v) {defaultRotVel = v;}

  void setRenderGeometry(bool s);

  bool getRenderGeometry() const {return mRenderGeometry;}

  void getAllLinks(std::vector<DynamicBody *> &allLinkVec);

  void setChainEndTrajectory(std::vector<transf>& traj,int chainNum);

  void generateCartesianTrajectory(const transf &startTr, const transf &endTr, 
                                                                   std::vector<transf> &traj,
                                                                   double startVel, double endVel=0.0, double timeNeeded=-1.0);

  Matrix staticJointTorques(bool useDynamicDofForce);
        
  transf getApproachTran() const {return approachTran;}

  double getApproachDistance(Body *object, double maxDist);

  //---------------------------emit signals-----------------------------------------

  void emitConfigChange(){emit configurationChanged();}
  void emitUserInteractionStart(){emit userInteractionStart();}
  void emitUserInteractionEnd(){emit userInteractionEnd();}

  static const double AUTO_GRASP_TIME_STEP;
};

void Robot::updateDofVals(double *dofVals)
{
        for (int d=0; d<numDOF; d++) {
                dofVec[d]->updateVal(dofVals[d]);
        }
}

void Robot::getJointValues(double *jointVals) const
{
        for (int c=0; c<numChains; c++) {
                chainVec[c]->getJointValues(jointVals);
        }
}

double Robot::jointLimitDist() const
{
        double dist, minDist = 1.0e10;
        for (int c=0; c<numChains; c++) {
                for (int j=0; j<chainVec[c]->getNumJoints(); j++) {
                        double val = chainVec[c]->getJoint(j)->getVal();
                        dist = std::min( chainVec[c]->getJoint(j)->getMax() - val, 
                                                         val - chainVec[c]->getJoint(j)->getMin() );
                        minDist = std::min(dist, minDist);
                }
        }
        return minDist;
}

void Robot::simpleSetTran(transf const& tr) {
    base->setTran(tr);
    if (mountPiece) mountPiece->setTran(tr);
    for (int f=0;f<numChains;f++) chainVec[f]->updateLinkPoses();
}

void Robot::getDOFVals(double *dofVals) const {
    for (int d=0;d<numDOF;d++) dofVals[d]=dofVec[d]->getVal();
}

void Robot::storeDOFVals(double *dofVals) const {
    for (int d=0;d<numDOF;d++) dofVals[d]=dofVec[d]->getSaveVal();
}

void Robot::forceDOFVal(int dofNum,double val) {
        double *jointVals = new double[numJoints];
        getJointValues(jointVals);
        dofVec[dofNum]->reset();
        dofVec[dofNum]->accumulateMove(val, jointVals, NULL);
        setJointValuesAndUpdate(jointVals);
        dofVec[dofNum]->updateVal(val);
        delete [] jointVals;
}

void Robot::forceDOFVals(double *dofVals) {
        double *jointVals = new double[numJoints];
        getJointValues(jointVals);
        for (int d=0; d<numDOF; d++) {
                dofVec[d]->reset();
                dofVec[d]->accumulateMove(dofVals[d], jointVals, NULL);
        }
        setJointValuesAndUpdate(jointVals);
        updateDofVals(dofVals);
        delete [] jointVals;
}

void Robot::updateDOFFromJoints(double *jointVals) {
        for (int d=0; d<numDOF; d++) {
                dofVec[d]->updateFromJointValues(jointVals);
        }
}

bool Robot::checkDOFVals(double *dofVals) const {
          for (int d=0;d<numDOF;d++) {
                  if ( dofVals[d] > dofVec[d]->getMax()) return false;
                  if ( dofVals[d] < dofVec[d]->getMin()) return false;
          }
          return true;
}

bool Robot::checkSetDOFVals(double *dofVals) const {
          bool atLeastOneValid = false;
          for (int d=0;d<numDOF;d++) {
                  if ( dofVals[d] > dofVec[d]->getMax()) {
                          dofVals[d] = dofVec[d]->getMax();
                  } else if ( dofVals[d] < dofVec[d]->getMin()) {
                          dofVals[d] = dofVec[d]->getMin();
                  }
                  else atLeastOneValid = true;
          }
          return atLeastOneValid;
}



class Hand : public Robot {
  Q_OBJECT

protected:
  Grasp *grasp;

public:
  Hand(World *w,const char *name);
  virtual ~Hand();

  virtual void cloneFrom(Hand *original);

  int getNumFingers() const {return numChains;}

  KinematicChain *getFinger(int i) const {return chainVec[i];}

  Link *getPalm() const {return base;}

  Grasp *getGrasp() const {return grasp;}

  virtual bool autoGrasp(bool renderIt, double speedFactor = 1.0, bool stopAtContact = false);

  virtual bool quickOpen(double speedFactor = 0.1);

  virtual bool approachToContact(double moveDist, bool oneStep = true);

  virtual bool findInitialContact(double moveDist);
};

#define ROBOT_H
#endif

---

graspit
Author(s):
autogenerated on Fri Jan 11 11:19:01 2013