/opt/ros/diamondback/stacks/graspit_simulator/graspit/graspit_source/include/contact.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, Claire Lackner and Matei T. Ciocarlie
//
// $Id: contact.h,v 1.37 2009/06/16 22:53:24 cmatei Exp $
//
//######################################################################

#ifndef CONTACT_HXX

#include <Inventor/nodes/SoSeparator.h>
#include <list>
#include <vector>

#include "collisionStructures.h"

class transf;
class Body;
class SoSeparator;
class SoMaterial;
class Matrix;

#ifdef ARIZONA_PROJECT_ENABLED
#include <arizona/Arizona_Raw_Exp.h>
#endif

#define DISPLAY_CONE_SCALE 20.0
#define MAX_FRICTION_EDGES 100
  

enum frictionT {FL,PCWF,SFCE,SFCL};

struct Wrench {
  vec3 force;
  vec3 torque;
};


class Contact
{
friend class VirtualContact;

protected:
  Body *body1;
    
  Body *body2;

  Contact *mate;

  double cof;

  double kcof;

  frictionT frictionType;

  int contactDim;

  int lmiDim;

  position loc;

  vec3 normal; 

  transf frame;
 
  transf body1Tran;

  transf body2Tran;

  SoSeparator *contactForcePointers;

  double normalForceLimit; 

  double *optimalCoeffs;    

  double dynamicForce[6];

  double prevCn;
  double prevLambda;
  double *prevBetas;
  bool inheritanceInfo;

  bool mSlip;

  void wrenchFromFrictionEdge(double *edge, const vec3 &radius, Wrench *wr);  
  
  int setUpFrictionEllipsoid(int numLatitudes,int numDirs[], double phi[],double eccen[]);

public:


  double frictionEdges[6*MAX_FRICTION_EDGES];

  int numFrictionEdges;


  Wrench *wrench;

  int numFCWrenches;

  Contact() : body1(NULL),body2(NULL),mate(NULL),cof(0.0),
        contactForcePointers(NULL), optimalCoeffs(NULL), prevBetas(NULL), wrench(NULL) {}

  Contact(Body *b1,Body *b2, position pos, vec3 norm);

  virtual ~Contact();

  virtual void computeWrenches();

  virtual int setUpFrictionEdges(bool dynamicsOn = false)=0;

  void setMate(Contact *m) {mate = m;}

  position getPosition(){return loc;}

  vec3 getNormal(){return normal;}

  Contact *getMate() const {return mate;}

  Body *getBody1() const {return body1;}

  Body *getBody2() const {return body2;}

  transf getBody1Tran() const {return body1Tran;}

  transf getBody2Tran() const {return body2Tran;}

  double getCof() const;

  position getLocation() const {return loc;}

  transf getFrame() const { return frame;}

  void updateCof();

  frictionT getFrictionType() const {return frictionType;}

  transf getContactFrame() const {return frame;}

  double *getDynamicContactWrench() {return dynamicForce;}

  SoSeparator *getContactForcePointers() const {return contactForcePointers;}

  bool preventsMotion(const transf& motion) const;

  void setContactForce(double *optmx);

  int getContactDim() const {return contactDim;}
  
  int getLmiDim() const {return lmiDim;}

  void setNormalForceLimit(double nfl) {normalForceLimit = nfl;}

  double getNormalForceLimit() const {return normalForceLimit;}

  //------------------- QuasiStatic equilibrium matrices and helper functions ---------------------

  Matrix frictionConstraintsMatrix() const;
  static Matrix frictionConstraintsBlockMatrix(const std::list<Contact*> &contacts);

  Matrix frictionForceMatrix() const;
  static Matrix frictionForceBlockMatrix(const std::list<Contact*> &contacts);

  Matrix localToWorldWrenchMatrix() const;
  static Matrix localToWorldWrenchBlockMatrix(const std::list<Contact*> &contacts);

  static Matrix normalForceSumMatrix(const std::list<Contact*> &contacts);

  //---------------------------------

  void setDynamicContactWrench(double f[6])
    {memcpy(dynamicForce,f,6*sizeof(double));}

  void setDynamicContactForce(const vec3 &force) {
    dynamicForce[0] = force.x(); dynamicForce[1] = force.y();dynamicForce[2] = force.z();
        dynamicForce[3] = dynamicForce[4] = dynamicForce[5] = 0;
  }

  void inherit(Contact *c);

  double getConstraintError();

  static const double THRESHOLD;

  static const double INHERITANCE_THRESHOLD;

  static const double INHERITANCE_ANGULAR_THRESHOLD;

  virtual SoSeparator* getVisualIndicator(){return NULL;}

  double getPrevCn(){return prevCn;}
  double getPrevLambda(){return prevLambda;}
  double *getPrevBetas(){return prevBetas;}
  void setLCPInfo(double cn, double l, double *betas);

  bool isSlipping() const {return mSlip;}

  bool inherits(){return inheritanceInfo;}

  SoMaterial *coneMat;
  float coneR, coneG, coneB;
};


class PointContact : public Contact
{
public:
        PointContact(Body *b1, Body *b2, position pos, vec3 norm);
        ~PointContact();
        int setUpFrictionEdges(bool dynamicsOn = false);
        SoSeparator* getVisualIndicator();
};


class SoftContact : public Contact
{
protected:
        vec3 *bodyNghbd;

        int numPts;

        double r1, r2;
        double a, b, c;

        double relPhi;

        mat3 fitRot;

        double fitRotAngle;

        double majorAxis;
        double minorAxis;

        double r1prime;
        double r2prime;

        //calculates the relative angle between the frames with 2 radii of curvature between
        //the contact and its mate
        int CalcRelPhi();

        int CalcRprimes();

        void FitPoints( );

        double CalcContact_Mattress( double nForce );

public:
        SoftContact( Body *b1, Body *b2, position pos, vec3 norm, Neighborhood *bn );

        ~SoftContact( );

        int setUpFrictionEdges(bool dynamicsOn = false );

        SoSeparator* getVisualIndicator();

        virtual void computeWrenches();
};


class VirtualContact : public Contact 
{
private:
        position mCenter;
        float mMaxRadius;
        
        SoSeparator *mWorldInd;
        SoMaterial *mZaxisMat;

        int mFingerNum;
        int mLinkNum;

        void init();
public:
        
        VirtualContact();
        VirtualContact(int f, int l, Contact *original);
        VirtualContact(const VirtualContact *original);
        ~VirtualContact();

        void setBody(Body *b){body1 = b;}
        void setObject(Body *b){body2 = b;}

        void writeToFile(FILE *fp);
        void readFromFile(FILE *fp);

        void computeWrenches(bool useObjectData = false, bool simply = false);
        
        void scaleWrenches(double factor);

        int setUpFrictionEdges(bool dynamicsOn = false);

        void updateContact(Body* object);

        void setRadius(float r){mMaxRadius = r;}
        void setCenter(position c){mCenter = c;}
        double getMaxRadius(){return mMaxRadius;}
        position getCenter(){return mCenter;}

        SoSeparator* getVisualIndicator();
        void getWorldIndicator(bool useObjectData = false);

        position getWorldLocation();
        vec3 getWorldNormal();
        void getObjectDistanceAndNormal(Body *body, vec3 *objDistance, vec3 *objNormal);

        void mark(bool m);

        int getFingerNum(){return mFingerNum;}
        int getLinkNum(){return mLinkNum;}
};
/* just like what VirtualContact class has done, VirtualContactOnObject Class is nothing weird except that
it changes the virtual contacts' loaction from the finger to an object imported before.
with these virtual contacts, we continue to study the grasp quality. Hao 10.04
*/
class VirtualContactOnObject : public VirtualContact
{
public:
        VirtualContactOnObject ();
        ~VirtualContactOnObject ();
        void readFromFile(FILE * fp);
#ifdef ARIZONA_PROJECT_ENABLED
        void readFromRawData(ArizonaRawExp* are, QString file, int index, bool flipNormal = false);
#endif
        void writeToFile(FILE * fp);
};
#define CONTACT_HXX
#endif

---

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