/opt/ros/diamondback/stacks/graspit_simulator/graspit/graspit_source/include/humanHand.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): Matei T. Ciocarlie
//
// $Id: humanHand.h,v 1.15 2009/08/17 22:16:52 cmatei Exp $
//
//######################################################################

#ifndef HUMAN_HAND_H
#define HUMAN_HAND_H

#include <vector>
#include <limits>
#include <set>

#include <Inventor/SoType.h>
#include "robot.h"

class SoSeparator;
class SoSphere;
class SoCylinder;
class SoTransform;
class SoDrawStyle;
class Tendon;

class TendonInsertionPoint {
private:
  vec3 mAttachPoint;
  

  bool mPermanent;


  int mAttachChainNr;
  int mAttachLinkNr;
  
  SoSeparator *mIVInsertion;
  SoMaterial *mIVInsertionMaterial;
  SoTransform *mIVInsertionTran;
  SoSphere *mIVInsertionGeom;
  
  SoSeparator *mIVConnector;
  SoMaterial *mIVConnectorMaterial;
  SoTransform *mIVConnectorTran;
  SoCylinder *mIVConnectorGeom;
  
  Tendon* mOwner;

  void createInsertionGeometry();
  void createConnectorGeometry();
public:

  vec3 mInsertionForce;
  
  TendonInsertionPoint(Tendon* myOwner,int chain,int link,vec3 point,bool isPerm=true);

  void removeAllGeometry();
  
  Tendon* getTendon(){return mOwner;}

  void setPermanent(bool p){mPermanent=p;}

  bool isPermanent() const {return mPermanent;}

  SbVec3f getWorldPosition();

  vec3 getAttachPoint() const {return mAttachPoint;}

  void setAttachPoint(vec3 attachPoint);
  

  Link* getAttachedLink();
  
  SoSeparator* getIVInsertion(){return mIVInsertion;}
  SoSphere* getIVInsertionGeom(){return mIVInsertionGeom;}
  SoMaterial* getIVInsertionMaterial(){return mIVInsertionMaterial;}
  
  SoSeparator* getIVConnector(){return mIVConnector;}
  SoMaterial* getIVConnectorMaterial(){return mIVConnectorMaterial;}
  SoTransform* getIVConnectorTran(){return mIVConnectorTran;}
  SoCylinder* getIVConnectorGeom(){return mIVConnectorGeom;}

  static const double INSERTION_POINT_RADIUS;
  static const double CONNECTOR_RADIUS;
};

class TendonWrapper{
private:
  int attachLinkNr;
  int attachChainNr;
  Robot *owner;

  SoSeparator *IVWrapper;
  SoMaterial *IVWrapperMaterial;
  SoTransform *IVWrapperTran;
  SoCylinder *IVWrapperGeom;

  std::list<QString> mExemptList;
  
public:
  vec3 location, orientation;
  double radius;
  
  TendonWrapper(Robot* myOwner);
  Link* getAttachedLink();
  Robot* getRobot(){return owner;}
  
  void setLocation(vec3 loc);
  vec3 getLocation() const {return location;}

  void setOrientation(vec3 orient);
  vec3 getOrientation() const {return orientation;}

  void setRadius(double r);
  double getRadius() const {return radius;}

  void createGeometry();
  void updateGeometry();
  SoSeparator* getIVRoot(){return IVWrapper;}
  int getChainNr(){return attachChainNr;}
  int getLinkNr(){return attachLinkNr;}
  bool loadFromXml(const TiXmlElement* root);
  bool isExempt(QString name);
};

class Tendon{
private:
  Robot* mOwner;

  SoSeparator *mIVRoot;
  
  SoDrawStyle* mIVVisibleToggle;

  SoSeparator* mIVForceIndRoot;

  SoDrawStyle* mIVForceIndToggle;

  SoMaterial *mIVForceIndMaterial;
  
  SoSeparator *mIVForceIndicators;

  float mActiveForce;
  
  float mPassiveForce;
  
  bool mApplyPassiveForce;
  
  double mK;
  

  std::list<TendonInsertionPoint *> mInsPointList;
  
  QString mTendonName;

  bool mVisible;

  bool mForcesVisible;

  bool mSelected;
  
  float mCurrentLength;

  float mRestLength;

  float mPreTensionLength;
  
  float mDefaultRestLength;

  void updateForceIndicators();

public:
  
  Tendon(Robot* myOwner);

  Robot *getRobot(){return mOwner;}

  SoSeparator* getIVRoot(){return mIVRoot;}
  
  void addInsertionPoint(int chain,int link,vec3 point, bool isPerm);
  
  std::list<TendonInsertionPoint*>::iterator 
  insertInsertionPoint(std::list<TendonInsertionPoint*>::iterator itPos, 
                       int chain, int link, vec3 point, bool isPerm);
  
  std::list<TendonInsertionPoint*>::iterator 
  removeInsertionPoint(std::list<TendonInsertionPoint*>::iterator itPos);
  
  void updateGeometry();
  
  void updateInsertionForces();
  
  void computeSimplePassiveForces();
  
  void checkWrapperIntersections();
  
  void removeWrapperIntersections();

  int getNumInsPoints() const {return mInsPointList.size();}
  
  int getNumPermInsPoints() const;

  TendonInsertionPoint* getPermInsPoint(int n);

  void removeTemporaryInsertionPoints();

  double minInsPointDistance();

  void select();
  void deselect();
  bool isSelected(){return mSelected;}
  
  void setActiveForce(float f);

  void setPassiveForce(float f);

  float getActiveForce(){return mActiveForce;}

  float getPassiveForce(){return mPassiveForce;}

  float getTotalForce(){if (mApplyPassiveForce) return mActiveForce+mPassiveForce; else return mActiveForce;}

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

  double getStiffness() {return mK;}

  float getDefaultRestLength(){return mDefaultRestLength;}

  void setDefaultRestLength(double l){mDefaultRestLength = l;}
  
  float getPreTensionLength(){return mPreTensionLength;}

  void setPreTensionLength(double l){mPreTensionLength = l;}

  void setName(QString name){mTendonName=name;}

  QString getName(){return mTendonName;}
  
  void setVisible(bool v);

  bool isVisible(){return mVisible;}

  void setForcesVisible(bool v);

  bool forcesVisible() {return mForcesVisible;}
  

  void applyForces();

  void setApplyPassiveForce(bool b){mApplyPassiveForce=b;}
  
  /*sets the state of the tendon as rest state*/
  void setRestLength(double length);
  

  float getExcursion(){return mCurrentLength - mRestLength;}

  float getCurrentLength(){return mCurrentLength;}

  bool loadFromXml(const TiXmlElement *root);

  transf getInsertionPointWorldTransform(std::list<TendonInsertionPoint*>::iterator insPt);


  void getInsertionPointTransforms(std::vector<transf> &insPointTrans);
  
  void getInsertionPointForceMagnitudes(std::vector<double> &magnitudes);

  void getInsertionPointLinkTransforms(std::list< std::pair<transf, Link*> > &insPointLinkTrans);
  
  bool insPointInsideWrapper();
};


class HumanHand : public Hand {
  Q_OBJECT
protected:
  std::vector<Tendon *> mTendonVec;

  std::vector<TendonWrapper *> mTendonWrapperVec;

  void applyTendonForces();

public:
  HumanHand(World*,const char*);
  virtual ~HumanHand() {}

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

  void updateTendonGeometry();

  int getNumTendons(){return mTendonVec.size();}

  int getNumTendonWrappers(){return mTendonWrapperVec.size();}

  Tendon* getTendon(int i){return mTendonVec[i];}

  TendonWrapper* getTendonWrapper(int i){return mTendonWrapperVec[i];}

  void selectTendon(int i){mTendonVec[i]->select();}

  void deselectTendon(int i){mTendonVec[i]->deselect();}

  virtual void DOFController(double timeStep);


  int tendonEquilibrium(const std::set<size_t> &activeTendons,
                        const std::set<size_t> &passiveTendons,
                        bool compute_tendon_forces,
                        std::vector<double> &activeTendonForces,
                        std::vector<double> &jointResiduals,
                        double& unbalanced_magnitude,
                        bool useJointSprings = true);

  int contactEquilibrium(std::list<Contact*> contacts,
                         const std::set<size_t> &activeTendons,
                         std::vector<double> &activeTendonForces,
                         double &unbalanced_magnitude);

  int contactForcesFromTendonForces(std::list<Contact*> contacts,
                                    std::vector<double> &contactForces,
                                    const std::set<size_t> &activeTendons,
                                    const std::vector<double> &activeTendonForces);

  int contactTorques(std::list<Contact*> contacts,
                     std::vector<double> &jointTorques);

  bool insPointInsideWrapper();

  void setRestPosition()
  {
    for(size_t i=0; i<mTendonVec.size(); i++)
    {
      if (mTendonVec[i]->getDefaultRestLength() < 0.0)
      {
        if (mTendonVec[i]->getPreTensionLength() < 0.0)
        {
          mTendonVec[i]->setRestLength( mTendonVec[i]->getCurrentLength() );
        }
        else
        {
          mTendonVec[i]->setRestLength( mTendonVec[i]->getCurrentLength() - 
                                        mTendonVec[i]->getPreTensionLength() );
        }
      }
      else
      {
        mTendonVec[i]->setRestLength( mTendonVec[i]->getDefaultRestLength() );
      }
    }
  }

  void removeTemporaryInsertionPoints()
  {
    for(size_t i=0; i<mTendonVec.size(); i++)
    {
      mTendonVec[i]->removeTemporaryInsertionPoints();
    }
  }

  double minInsPointDistance()
  {
    double minDist = std::numeric_limits<double>::max();
    for(size_t i=0; i<mTendonVec.size(); i++)
    {
      minDist = std::min(minDist, mTendonVec[i]->minInsPointDistance());
    }
    return minDist;
  }

};

#endif

---

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