/opt/ros/diamondback/stacks/graspit_simulator/graspit/graspit_source/src/contact.cpp

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 
//
// $Id: contact.cpp,v 1.56 2009/08/14 18:09:43 cmatei Exp $
//
//######################################################################

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#ifdef USE_DMALLOC
#include "dmalloc.h"
#endif

#include <Inventor/nodes/SoCone.h>
#include <Inventor/nodes/SoCoordinate3.h>
#include <Inventor/nodes/SoCylinder.h>
#include <Inventor/nodes/SoIndexedFaceSet.h>
#include <Inventor/nodes/SoMaterial.h>
#include <Inventor/nodes/SoTransform.h>
#include <Inventor/nodes/SoTranslation.h>
#include <Inventor/nodes/SoSeparator.h>
#include <Inventor/nodes/SoSphere.h>

#include "matvec3D.h"
#include "contact.h"
#include "world.h"
#include "body.h"
#include "mytools.h"
#include "matrix.h"
#include "FitParabola.h"

//#define GRASPITDBG
#include "debug.h"

const double Contact::THRESHOLD = 0.1;
const double Contact::INHERITANCE_THRESHOLD = 1;
const double Contact::INHERITANCE_ANGULAR_THRESHOLD = 0.984; //cosine of 10 degrees

Contact::Contact(Body *b1, Body *b2, position pos, vec3 norm)
{
  body1 = b1;
  body2 = b2;
  mate = NULL;
  wrench = NULL;
  body1Tran = b1->getTran();
  body2Tran = b2->getTran();
  
  updateCof();
  normal = normalise(norm);
  loc = pos;
  vec3 tangentX,tangentY;

  if (fabs(normal % vec3(1,0,0)) > 1.0 - MACHINE_ZERO) {
    tangentX = normalise(normal * vec3(0,0,1));
  } else {
    tangentX = normalise(normal * vec3(1,0,0));
  }
  tangentY = normalise(normal * tangentX);  
  frame = coordinate_transf(loc,tangentX,tangentY);                      
  coneMat = NULL;
  prevBetas = NULL;
  inheritanceInfo = false;
  for (int i=0; i<6; i++) {
          dynamicForce[i] = 0;
  }
}

Contact::~Contact()
{
  if (optimalCoeffs) delete [] optimalCoeffs;
  if (wrench) delete [] wrench;
  if (prevBetas) delete [] prevBetas;

  if (mate) {
    mate->setMate(NULL);
    body2->removeContact(mate);
  }
  DBGP("Contact deleted");
}

void Contact::wrenchFromFrictionEdge(double *edge, const vec3 &radius, Wrench *wr)
{
        vec3 tangentX = frame.affine().row(0);
        vec3 tangentY = frame.affine().row(1);

        GraspableBody *object = (GraspableBody *)body1;

        //max friction is normal_force_magnitude (which is 1) * coeff_of_friction 
        //possible friction for this wrench is (friction_edge * max_friction) in the X and Y direction of the contact
        vec3 forceVec = normal + (tangentX*cof*edge[0])+ (tangentY*cof*edge[1]);
        
        //max torque is contact_radius * normal_force_magnitude (which is 1) * coeff_of_friction
        //possible torque for this wrench is (friction_edge * max_torque) in the direction of the contact normal
        //the contact_radius coefficient is already taken into account in the friction_edge
        vec3 torqueVec = ( (radius*forceVec) + normal*cof*edge[5] )/object->getMaxRadius();
        
        wr->torque = torqueVec;
        wr->force = forceVec;
}

void Contact::computeWrenches()
{
  DBGP("COMPUTING WRENCHES");

  if (wrench) delete [] wrench;
  //one wrench for each vector
  numFCWrenches = numFrictionEdges;
  wrench = new Wrench[numFCWrenches];

  vec3 radius = loc - ((GraspableBody*)body1)->getCoG();
  for (int i=0;i<numFCWrenches;i++) {
          wrenchFromFrictionEdge( &frictionEdges[6*i], radius, &wrench[i] );
        }
}


int
Contact::setUpFrictionEllipsoid(int numLatitudes, int numDirs[], double phi[], double eccen[])
{
        numFrictionEdges = 0;
        for (int i=0;i<numLatitudes;i++) {
                numFrictionEdges += numDirs[i];
        }
        if (numFrictionEdges > MAX_FRICTION_EDGES) return FAILURE;
        prevBetas = new double[numFrictionEdges];

        int col = 0;
        for (int i=0;i<numLatitudes;i++) {
                double cosphi = cos(phi[i]);
                double sinphi = sin(phi[i]);
                for (int j=0;j<numDirs[i];j++) {
                        double theta = j * 2*M_PI/numDirs[i];
      
                        double num = cos(theta)*cosphi;
                        double denom = num*num/(eccen[0]*eccen[0]);
                        num = sin(theta)*cosphi;
                        denom += num*num/(eccen[1]*eccen[1]);
                        num = sinphi;
                        denom += num*num/(eccen[2]*eccen[2]);
                        denom = sqrt(denom);

                        frictionEdges[col*6]   = cos(theta)*cosphi/denom;
                        frictionEdges[col*6+1] = sin(theta)*cosphi/denom;
                        frictionEdges[col*6+2] = 0;
                        frictionEdges[col*6+3] = 0;
                        frictionEdges[col*6+4] = 0;
                        frictionEdges[col*6+5] = sinphi/denom;
                        col++;
                }    
        }
        return SUCCESS;
}

Matrix 
Contact::frictionConstraintsMatrix() const
{
        Matrix F(1, 1+numFrictionEdges);
        F.setAllElements(1.0);
        F.elem(0,0) = -1.0 * getCof();
        return F;
}

Matrix 
Contact::frictionForceMatrix() const
{
        Matrix Rfi(6, numFrictionEdges + 1);
        //the column for the normal force;
        //in local contact coordinates the normal always points in the z direction
        Rfi.elem(0,0) = Rfi.elem(1,0) = 0.0; Rfi.elem(2,0) = 1.0;
        Rfi.elem(3,0) = Rfi.elem(4,0) = Rfi.elem(5,0) = 0.0;
        //the columns for the friction edges
        for(int edge=0; edge<numFrictionEdges; edge++) {
                for(int i=0; i<6; i++) {
                        Rfi.elem(i, edge+1) = frictionEdges[6*edge+i];
                }
        }
        //flip the sign of Rfi to get contact pointing in the right direction
        Rfi.multiply(-1.0);
        return Rfi;
}

Matrix
Contact::frictionForceBlockMatrix(const std::list<Contact*> &contacts)
{
        if (contacts.empty()) {
                return Matrix(0,0);
        }
        std::list<Matrix*> blocks;
        std::list<Contact*>::const_iterator it;
        for (it=contacts.begin(); it!=contacts.end(); it++) {
                blocks.push_back( new Matrix((*it)->frictionForceMatrix()) );
        }
        Matrix Rf(Matrix::BLOCKDIAG<Matrix>(&blocks));
        while (!blocks.empty()) {
                delete blocks.back();
                blocks.pop_back();
        }
        return Rf;
}

Matrix 
Contact::frictionConstraintsBlockMatrix(const std::list<Contact*> &contacts)
{
        if (contacts.empty()) {
                return Matrix(0,0);
        }
        std::list<Matrix*> blocks;
        std::list<Contact*>::const_iterator it;
        for (it=contacts.begin(); it!=contacts.end(); it++) {
                blocks.push_back( new Matrix((*it)->frictionConstraintsMatrix()) );
        }
        Matrix blockF(Matrix::BLOCKDIAG<Matrix>(&blocks));
        while (!blocks.empty()) {
                delete blocks.back();
                blocks.pop_back();
        }
        return blockF;
}

Matrix 
Contact::normalForceSumMatrix(const std::list<Contact*> &contacts)
{
        if (contacts.empty()) {
                return Matrix(0,0);
        }
        std::list<Matrix*> blocks;
        std::list<Contact*>::const_iterator it;
        for (it=contacts.begin(); it!=contacts.end(); it++) {
                blocks.push_back( new Matrix(Matrix::ZEROES<Matrix>(1, (*it)->numFrictionEdges+1)) );
                blocks.back()->elem(0,0) = 1.0;
        }
        Matrix blockF(Matrix::BLOCKROW<Matrix>(&blocks));
        while (!blocks.empty()) {
                delete blocks.back();
                blocks.pop_back();
        }
        return blockF;
}

Matrix 
Contact::localToWorldWrenchMatrix() const
{
        Matrix Ro(Matrix::ZEROES<Matrix>(6,6));
        transf contactTran = getContactFrame() * getBody1()->getTran();
        mat3 R; contactTran.rotation().ToRotationMatrix(R);
        Matrix Rot( Matrix::ROTATION(R) );
        //the force transform is simple, just the matrix that changes coord. systems
        Ro.copySubMatrix(0, 0, Rot);
        Ro.copySubMatrix(3, 3, Rot);
        //for torque we also multiply by a cross product matrix
        vec3 worldLocation = contactTran.translation();
        vec3 cog = getBody2()->getTran().translation();
        mat3 C; C.setCrossProductMatrix(worldLocation - cog); 
        Matrix CR(3,3);
        matrixMultiply(Matrix::ROTATION(C.transpose()), Rot, CR);
        //also scale by object max radius so we get same units as force
        //and optimization does not favor torque over force
        double scale = 1.0;
        if (getBody2()->isA("GraspableBody")) {
                scale = scale / static_cast<GraspableBody*>(getBody2())->getMaxRadius();
        }
        CR.multiply(scale);
        Ro.copySubMatrix(3, 0, CR);
        return Ro;
}

Matrix 
Contact::localToWorldWrenchBlockMatrix(const std::list<Contact*> &contacts)
{
        if (contacts.empty()) {
                return Matrix(0,0);
        }
        std::list<Matrix*> blocks;
        std::list<Contact*>::const_iterator it;
        for (it=contacts.begin(); it!=contacts.end(); it++) {
                blocks.push_back( new Matrix((*it)->localToWorldWrenchMatrix()) );
        }
        Matrix Ro(Matrix::BLOCKDIAG<Matrix>(&blocks));
        while (!blocks.empty()) {
                delete blocks.back();
                blocks.pop_back();
        }
        return Ro;
}

bool Contact::preventsMotion(const transf& motion) const
{  
  if ( (loc*motion - loc) % normal < -MACHINE_ZERO) return true;
  return false;
}


void
Contact::updateCof()
{
  cof = body1->getWorld()->getCOF(body1->getMaterial(),body2->getMaterial());
  kcof = body1->getWorld()->getKCOF(body1->getMaterial(),body2->getMaterial());
  body1->setContactsChanged();
  body2->setContactsChanged();
}

double
Contact::getCof() const
{
  DynamicBody *db;
  vec3 radius,vel1(vec3::ZERO),vel2(vec3::ZERO),rotvel;

  if (body1->isDynamic()) {
    db = (DynamicBody *)body1;
    radius = db->getTran().rotation() * (loc - db->getCoG());
    vel1.set(db->getVelocity()[0],db->getVelocity()[1],db->getVelocity()[2]);
    rotvel.set(db->getVelocity()[3],db->getVelocity()[4],db->getVelocity()[5]);
    vel1 += radius * rotvel;
  }
  if (body2->isDynamic()) {
    db = (DynamicBody *)body2;
    radius = db->getTran().rotation() * (mate->loc - db->getCoG());
    vel2.set(db->getVelocity()[0],db->getVelocity()[1],db->getVelocity()[2]);
    rotvel.set(db->getVelocity()[3],db->getVelocity()[4],db->getVelocity()[5]);
    vel2 += radius * rotvel;
  }
  if ((vel1 - vel2).len() > 1.0) {
    DBGP("SLIDING!");
    return kcof;
  }
  else return cof;
}

void
Contact::setContactForce (double *optmx)
{
  for (int i=0;i<contactDim;i++)
    optimalCoeffs[i] = optmx[i];
}

double
Contact::getConstraintError()
{
        transf cf = frame * body1->getTran();
        transf cf2 = mate->getContactFrame() * body2->getTran();
        return MAX(0.0,Contact::THRESHOLD/2.0 - (cf.translation() - cf2.translation()).len());
}

void
Contact::inherit(Contact *c)
{
        inheritanceInfo = true;
        setLCPInfo( c->getPrevCn(), c->getPrevLambda(), c->getPrevBetas() );
        /*
        if (!coneMat || !c->coneMat) {
                return;
        }
        coneR = c->coneR;
        coneG = c->coneG;
        coneB = c->coneB;
        coneMat->diffuseColor = SbColor( coneR, coneG, coneB);
        coneMat->ambientColor = SbColor( coneR, coneG, coneB);
        coneMat->emissiveColor = SbColor( coneR, coneG, coneB);
        */
}

void
Contact::setLCPInfo(double cn, double l, double *betas)
{
        prevCn = cn;
        prevLambda = l;
        for (int i=0; i<numFrictionEdges; i++) {
                prevBetas[i] = betas[i];
        }
        //lambda is the LCP paramter that indicates contact slip
        if (l>0) mSlip = true;
        else mSlip = false;
}

// Point contact

PointContact::PointContact(Body *b1,Body *b2,position pos, vec3 norm) : 
                                Contact( b1, b2, pos, norm )
{
        //we should really have another class for the FL contact
        if (cof == 0.0) {
                frictionType = FL;
                contactDim = 1;
                lmiDim = 1;
        } else {
                frictionType = PCWF;
                contactDim = 3;
                lmiDim = 3;
        }
        optimalCoeffs = new double[contactDim]; 
        setUpFrictionEdges();
}

PointContact::~PointContact()
{
}

int PointContact::setUpFrictionEdges(bool dynamicsOn)
{

        if (dynamicsOn) {
                //we don't really need to update anything; friction edges for point contacts do not change
                //during dynamic simulation
                return 0;
        }
        double eccen[3] = {1,1,1};
        int numDirs[1] = {8};
        double phi[1] = {0.0};
        return setUpFrictionEllipsoid(1,numDirs,phi,eccen); 
}

SoSeparator* 
PointContact::getVisualIndicator()
{
        double height,alpha,cof;
        SoSeparator *cne;
        SoTransform *tran;
        SoIndexedFaceSet *ifs;
        SoCoordinate3 *coords;

        SbVec3f *points = new SbVec3f[numFrictionEdges+1];
        int32_t *cIndex = new int32_t[5*numFrictionEdges+1];
        int i;

        points[0].setValue(0,0,0);

        //this is now a member of the class so no need to declare it here
        coneMat = new SoMaterial;  

    coneMat->diffuseColor = SbColor(0.8f,0.0f,0.0f);
    coneMat->ambientColor = SbColor(0.2f,0.0f,0.0f);
    coneMat->emissiveColor = SbColor(0.4f,0.0f,0.0f);
/*
        coneR = ((float)rand())/RAND_MAX;
        coneG = ((float)rand())/RAND_MAX;
        coneB = ((float)rand())/RAND_MAX;

        coneMat->diffuseColor = SbColor(coneR, coneG, coneB);
        coneMat->ambientColor = SbColor(coneR, coneG, coneB);
        coneMat->emissiveColor = SbColor(coneR, coneG, coneB);
*/
    coneMat->transparency = 0.8f;

    SoMaterial* zaxisMat = new SoMaterial;  
    zaxisMat->diffuseColor = SbColor(0,0,0);
    zaxisMat->ambientColor = SbColor(0,0,0);

        cof = getCof();

        height = Body::CONE_HEIGHT;
        cne = new SoSeparator;
        coords = new SoCoordinate3;
        ifs = new SoIndexedFaceSet;
        tran = new SoTransform;
  
        alpha = 0.0;
        for (i=0;i<numFrictionEdges;i++) {
                points[i+1].setValue(cos(alpha)*cof,sin(alpha)*cof,1.0);
                points[i+1] *= height;
                cIndex[4*i] = 0;
                cIndex[4*i+1] = (i+2 <= numFrictionEdges ? i+2 : 1);
                cIndex[4*i+2] =  i+1;
                cIndex[4*i+3] = -1;
                cIndex[4*numFrictionEdges+i] = i+1;
                alpha += 2.0*M_PI/numFrictionEdges;
        }
        cIndex[5*numFrictionEdges] = -1;
  
        coords->point.setValues(0,numFrictionEdges+1,points);
        ifs->coordIndex.setValues(0,5*numFrictionEdges+1,cIndex);
        delete [] points;
        delete [] cIndex;

        getContactFrame().toSoTransform(tran);
  
        SoCylinder *zaxisCyl = new SoCylinder;
        zaxisCyl->radius = 0.05f;
        zaxisCyl->height = height;
  
        SoTransform *zaxisTran = new SoTransform;
        zaxisTran->translation.setValue(0,0,height/2.0);
        zaxisTran->rotation.setValue(SbVec3f(1,0,0),(float)M_PI/2.0f);
  
        SoSeparator *zaxisSep = new SoSeparator;
        zaxisSep->addChild(zaxisTran);
        zaxisSep->addChild(zaxisMat);
        zaxisSep->addChild(zaxisCyl);
  
        cne->addChild(tran);
        cne->addChild(zaxisSep);
        cne->addChild(coneMat);
        cne->addChild(coords);
        cne->addChild(ifs);
        return cne;
}

//SoftContact functions

SoftContact::SoftContact( Body *b1, Body *b2, position pos, vec3 norm,
                                                 Neighborhood *bn ) : Contact(b1, b2, pos, norm)
{ 
        frictionType = SFCL;
        contactDim = 4;
        lmiDim = 4;
        optimalCoeffs = new double[contactDim]; 

        bodyNghbd =  new vec3[ (int) bn->size() ];
        Neighborhood::iterator itr;
        int i = 0;
        vec3 temp;
        
        for( itr = bn->begin(); itr != bn->end(); itr++ ) {
                temp.set(itr->x(), itr->y(), itr->z());
                //places bodyNghbd in frame of contact with the z-axis pointing out
                //bodyNghbd[i] = frame.affine().inverse() * ( temp - frame.translation() );
                position posit;
                posit = position( temp.toSbVec3f() ) * frame.inverse();
                bodyNghbd[i] = vec3( posit.toSbVec3f() );
                i++;
        }
        numPts = (int) bn->size();
        majorAxis = 0.0;
        minorAxis = 0.0;
        relPhi = 0.0;
        a = 0; b = 0; c = 0;
        r1 = 0; r2 = 0;
        r1prime = 0; r2prime = 0;

        FitPoints();

        //setUpFrictionEdges();
        //wait to set these up until other contact is made
}

SoftContact::~SoftContact()
{
        delete[]bodyNghbd;
}

int SoftContact::setUpFrictionEdges(bool dynamicsOn )
{
        if( !getMate() ) {
                fprintf(stderr,"Trying to set up friction edges for a contact with no mate!!\n");
                return 1;
        }

        DBGP("Setting up SOFT contact friction edges");
        double eccen[3];

        // magnitude of tangential friction
        eccen[0]=1;
        eccen[1]=1;
        // magnitude of max frictional torque
        double torquedivN;

        // relative radii of curvature at the contact
        CalcRprimes();

        if (!dynamicsOn) {
                // if dynamics are not on, compute it based on some random applied force
                torquedivN = CalcContact_Mattress( 5 );
                eccen[2] = torquedivN*1000;
        } else {
                // if dynamics are on, compute it based on the applied force reported by the LCP
                double nForce = dynamicForce[2];
                //need to figure out what all the numbers in the dynamic force mean
                //I think it is a wrench, and hope it is in the contacts coordinate frame
                //and that the units are in newtons, chekc dynamics.cpp iterateDynamics where
                //it is created
                torquedivN = CalcContact_Mattress( nForce );
                eccen[2] = torquedivN*1000;
        }

        //various possible approximations for the friction ellipsoid

        /*
        int numDirs[9] = {1,3,5,7,8,7,5,3,1};
        double phi[9] = {M_PI_2, M_PI_2*0.6, M_PI_2*0.3, M_PI_2*0.15, 0.0,
                     -M_PI_2*0.15, -M_PI_2*0.3, -M_PI_2*0.6, -M_PI_2};
        return Contact::setUpFrictionEdges(9,numDirs,phi,eccen);
        */

        /*
        int numDirs[9] = {1,8,8,8,8,8,8,8,1};
        double phi[9] = {M_PI_2, M_PI_2*0.66, M_PI_2*0.33, M_PI_2*0.165, 0.0,
                                  -M_PI_2*0.165, -M_PI_2*0.33, -M_PI_2*0.66, -M_PI_2};
        return Contact::setUpFrictionEdges(9,numDirs,phi,eccen);
        */
        
        int numDirs[5] = {1,5,8,5,1};
        double phi[5] = {M_PI_2, M_PI_2*0.50, 0.0, -M_PI_2*0.50, -M_PI_2};
        return Contact::setUpFrictionEllipsoid( 5, numDirs, phi, eccen );
}

void SoftContact::computeWrenches()
{
        bool approxPlaneMoments = false;

        if (!approxPlaneMoments) {
                Contact::computeWrenches();
                return;
        }

        //for now we hard-code 4 possible locations for the contact
        //on 4 points around the perimeter of the ellipse

        //we also approximate the ellipse with a circle of radius sqrt(majorAxis * minorAxis) 
        //because for now we don't really compute the direction of the major axis

        //when we do, we will have to transform the tangents by:
        // - the transform that aligns it with the major radius of the paraboloid
        // - the transform that further moves that to the major axis of the contact ellipse

        if (wrench) delete [] wrench;
        numFCWrenches = 4 * numFrictionEdges;
        wrench = new Wrench[numFCWrenches];

        vec3 tangentX = frame.affine().row(0);
        vec3 tangentY = frame.affine().row(1);
        
        vec3 radius;
        vec3 baseRadius = loc - ((GraspableBody*)body1)->getCoG();

        float a,b;
        a = b = 1000 * sqrt(majorAxis * minorAxis); //also convert to milimeters
        a = b = 1000 * std::max(majorAxis, minorAxis);

        DBGA("Soft contact size: " << a);

        for (int i=0;i<numFrictionEdges;i++) {
                radius = baseRadius + ( a * tangentX );
                wrenchFromFrictionEdge( &frictionEdges[6*i], radius, &wrench[4*i+0] );

                radius = baseRadius - ( a * tangentX );
                wrenchFromFrictionEdge( &frictionEdges[6*i], radius, &wrench[4*i+1] );

                radius = baseRadius + ( b * tangentY );
                wrenchFromFrictionEdge( &frictionEdges[6*i], radius, &wrench[4*i+2] );

                radius = baseRadius - ( b * tangentY );
                wrenchFromFrictionEdge( &frictionEdges[6*i], radius, &wrench[4*i+3] );
        }
        DBGA("Soft wrenches computed");
}

void SoftContact::FitPoints( )
{
        double *coeffs = new double [3];
        FitParaboloid( bodyNghbd, numPts, coeffs );

        a = coeffs[0];
        b = coeffs[1];
        c = coeffs[2];

        RotateParaboloid( coeffs, &r1, &r2, &fitRot, &fitRotAngle );
        DBGP(getBody1()->getName().latin1() << ": " << "a=" << a << " b=" << b << " c=" <<c);
        DBGP("r1=" << r1 << " r2=" <<r2);
}

int SoftContact::CalcRelPhi( )
{
        vec3 temp, t;
        vec3 R11, R12;          //directions of relative curvatures in world frame

        temp.set( 1, 0 , 0);
        t = fitRot * temp;
        R11 = t * frame;
        R11 = R11 * body1->getTran();

        temp.set( 1, 0 , 0);
        t = ((SoftContact *)getMate())->fitRot * temp;
        R12 = t * ((SoftContact *)getMate())->frame;
        R12 = R12 * body2->getTran();

        relPhi = acos( (R11%R12)/(R11.len()*R12.len()) );
        ((SoftContact *)getMate())->relPhi = relPhi ;

        //fprintf( stderr, "Rel Phi   %f", relPhi );
        return 0;       
}

int SoftContact::CalcRprimes()
{
        if( !(getMate()) ) {
                DBGA("Contact doesn't have mate, not calculating curvature...");
                return 1;
        }

        SoftContact *m = (SoftContact *)getMate();

        CalcRelPhi();

        DBGP("Body 1" << getBody1()->getName().latin1() << " Body 2 " << m->getBody1()->getName().latin1());

        //the less than zero curvature is for flat objects
        //1/r goes to zero for flat objects because the curvature
        //is infinite
        double x,y,w,z;
        if( r1 < 0.0 )
                w = 0.0;
        else
                w = 1/r1;

        if( r2 < 0.0 )
                x = 0.0;
        else
                x = 1/r2;       

        if( m->r1 < 0.0 )
                y = 0.0;
        else
                y = 1/m->r1;

        if( m->r2 < 0.0 )
                z = 0.0;
        else
                z = 1/m->r2;

        DBGP("x: " << x << " y: " << y << " w: " << w << " z: " << z);

        double ApB = 0.5*( w + x + y + z );

        DBGP("Apb: " << ApB);

        double AmB = 0.5*sqrt( (w - x)*(w - x) + (y - z)*(y - z) +
                                        2*cos(2*relPhi)*(w - x)*(y - z) );

        DBGP("Amb: " << AmB << " relPhi: " << relPhi);

        if( AmB > ApB ) {
                printf( "Invalid relative curvature, ending calculation...\n" );
                return 1;
        }

        //by definition r1prime >= r2prime
        if (ApB + AmB != 0)
                r1prime = 1/( ApB + AmB );
        else
                r1prime = -1.0;

        if( ApB == AmB)
                r2prime = -1.0;
        else
                r2prime = 1/(ApB - AmB );

        m->r1prime = r1prime;
        m->r2prime = r2prime;

        //fprintf(stderr,"original: %f %f and %f %f\n",r1,r2,m->r1, m->r2);
        //fprintf(stderr,"Relative radii: %f %f equiv: %f\n",r1prime, r2prime, sqrt(r1prime*r2prime) );
        return 0;
}

double SoftContact::CalcContact_Mattress( double nForce )
{
        if (r1prime < 0) {
                DBGA("Degenerate soft contact");
                r1prime = 20;
        }
        if (r2prime < 0) {
                DBGA("Degenerate soft contact");
                r2prime = 20;
        }
                
        //hardwired height of mattress = 3.0 mm
        //r primes are in mm
        double h = 0.003;
        double delta = sqrt( nForce*h/(MAX(body1->getYoungs(), body2->getYoungs())
                                                                        * M_PI * sqrt(r1prime*0.001*r2prime*0.001)) ); //meters

        majorAxis = sqrt( 2*delta*r1prime*0.001 ); //meters
        minorAxis = sqrt( 2*delta*r2prime*0.001 ); 
        DBGP("Axes: " << majorAxis << " " << minorAxis);
        //axes are given in meters!!!!
        //rprimes and the radii of curvature are calculated in mm

        SoftContact *m = (SoftContact *)getMate();
        m->majorAxis = majorAxis;
        m->minorAxis = minorAxis;

        //returns max available torque proportional to given normal force
        //max torque=*K/h*mu*4*pi*(ab)^1.5/15 where a and b are 
        //the axis of the contact ellipse
        //this just returns the max torque divided by the normal force times mu
        //which is 8sqrt(ab)/15
        //the pressure distrib is K*delta/h/2*pi*ab

        //std::cerr<<"Ma ma "<< majorAxis << minorAxis << "\n";

        return 8*sqrt( majorAxis*minorAxis )/15;        //in meters
}

SoSeparator* SoftContact::getVisualIndicator()
{
        double height,radius;
        SoSeparator *cne;
        SoTransform *tran;
        SoCoordinate3 *coords;
        cne = new SoSeparator;

        int sampling_n = 10, sampling_m = 10;
        //double sampleSize_n = 2, sampleSize_m = 1;
        //double sampleSize_n = (4.0 * majorAxis * 1000) / (2.0 * sampling_n);
        //double sampleSize_m = (4.0 * minorAxis * 1000) / (2.0 * sampling_m);

        double sampleSize_n = 0.7;
        double sampleSize_m = 0.7;


        DBGP("Major " << majorAxis << " minor " << minorAxis);
        
        //points runs left to right and then up to down in the grid
        SbVec3f *points = new SbVec3f[(2*sampling_n + 1)*(2*sampling_m + 1) ];

        coneMat = new SoMaterial;
        coneMat->diffuseColor = SbColor(0.8f,0.0f,0.0f);
        coneMat->ambientColor = SbColor(0.2f,0.0f,0.0f);
        coneMat->emissiveColor = SbColor(0.4f,0.0f,0.0f);
        coneMat->transparency = 0.8f;

        SoMaterial* zaxisMat = new SoMaterial;  
        zaxisMat->diffuseColor = SbColor(0,0,0);
        zaxisMat->ambientColor = SbColor(0,0,0);

        radius = Body::CONE_HEIGHT / 5;
        height = Body::CONE_HEIGHT;

        tran = new SoTransform;  
        getContactFrame().toSoTransform(tran);

        SoCylinder *zaxisCyl = new SoCylinder;
        zaxisCyl->radius = 0.05f;
        zaxisCyl->height = 0.2 * height;
  
        SoTransform *zaxisTran = new SoTransform;
        zaxisTran->translation.setValue(0,0,0.2 * height/2.0);
        zaxisTran->rotation.setValue(SbVec3f(1,0,0),(float)M_PI/2.0f);
  
        SoSeparator *zaxisSep = new SoSeparator;
        zaxisSep->addChild(zaxisTran);
        zaxisSep->addChild(zaxisMat);
        zaxisSep->addChild(zaxisCyl);

        int n_squares = 2 * sampling_n * 2 *sampling_m;
        int32_t *cIndex = new int32_t[5*n_squares];

        int count = 0;
        int count_sq = 0;
        for( int i = -sampling_n; i <= sampling_n; i++ )
        {
                for( int j = -sampling_m; j <= sampling_m; j++ )
                {
                        double x = i*sampleSize_n;
                        double y = j*sampleSize_m;
                        //double z = a*x*x + b*y*y + c*x*y;
                        double z = 0;
                        if (r1 > 0) z+= x*x*1.0/(2*r1);
                        if (r2 > 0) z+= y*y*1.0/(2*r2);
                        points[ count ].setValue( x, y, z );
                        if (x>0 && y == 0){
                                points[count].setValue(x,y,z+0.4);
                        }

                        if ( i > -sampling_n && j > -sampling_m)
                        {
                                cIndex[5*count_sq + 0] = count - (2*sampling_m + 1);
                                cIndex[5*count_sq + 1] = count - (2*sampling_m + 1) - 1;
                                cIndex[5*count_sq + 2] = count - 1;
                                cIndex[5*count_sq + 3] = count;
                                cIndex[5*count_sq + 4] = -1;
                                count_sq++;
                        }
                        count++;
                }
        }
        if (count != (2*sampling_n + 1)*(2*sampling_m + 1) || count_sq != n_squares)
                exit(0);

        coords = new SoCoordinate3;
        coords->point.setValues( 0, count, points );

        SoIndexedFaceSet *ifs = new SoIndexedFaceSet;
        ifs->coordIndex.setValues(0, 5*n_squares, cIndex);

        //neighborhood is in frame of contact, so add contact transform first
        cne->addChild(tran);
/*
        SoMaterial* sphereMat = new SoMaterial;  
        sphereMat->diffuseColor = SbColor(1,1,0);
        sphereMat->ambientColor = SbColor(1,1,0);
        cne->addChild(sphereMat);
        for (int i=0; i<numPts; i++)
        {
                SoSphere* sphere = new SoSphere;
                sphere->radius = 0.5;
                SoTransform* sphereTran = new SoTransform;
                sphereTran->translation.setValue( bodyNghbd[i].x(), bodyNghbd[i].y(), bodyNghbd[i].z() );
                SoSeparator* sep = new SoSeparator;
                sep->addChild(sphereTran);
                sep->addChild(sphere);
                cne->addChild(sep);
        }
*/

        cne->addChild(zaxisSep);

        //this angle gets us from the contact frame to the frame in which the paraboloid is defined by just
        //two parameters, r1 and r2
        SoTransform* axisAlignTran = new SoTransform;  
        axisAlignTran->rotation.setValue(SbVec3f(0,0,1), -fitRotAngle);
        cne->addChild(axisAlignTran);

        cne->addChild(coneMat);
        cne->addChild(coords);
        cne->addChild(ifs);

        //add an arrow in the direction of the major axis of the contact ellipse
        //this is the angle between the major axis of the two paraboloids. it is used to compute the
        //magnitude of the major axis of the contact ellipse
        //but for now we don't compute the direction of the contact ellipse!
        SoTransform* phiAlignTran = new SoTransform;  
        phiAlignTran->rotation.setValue(SbVec3f(0,0,1), relPhi);
        cne->addChild(phiAlignTran);

        SoCylinder *xaxisCyl = new SoCylinder;
        xaxisCyl->radius = 0.05f;
        xaxisCyl->height = height / 2.0;
  
        SoTransform *xaxisTran = new SoTransform;
        xaxisTran->translation.setValue(height/4.0, 0.0, 0.0);
        xaxisTran->rotation.setValue(SbVec3f(0,0,1),-(float)M_PI/2.0f);
  
        SoSeparator *xaxisSep = new SoSeparator;
        xaxisSep->addChild(xaxisTran);
        xaxisSep->addChild(zaxisMat);
        xaxisSep->addChild(xaxisCyl);

        //cne->addChild(xaxisSep);
        return cne;
}

// Virtual Contact

void
VirtualContact::init()
{
        mFingerNum = -2;
        mLinkNum = 0;
        mZaxisMat = NULL;
        mWorldInd = NULL;
        mate = this;
        body1 = NULL;
        body2 = NULL;
}

VirtualContact::VirtualContact() : Contact()
{
        init();
}

VirtualContact::VirtualContact(const VirtualContact *original) : Contact()
{
        init();
        mFingerNum = original->mFingerNum;
        mLinkNum = original->mLinkNum;
        //copy friction edges
        //this should really be done by a Contact copy constructor
        numFrictionEdges = original->numFrictionEdges;
        memcpy( frictionEdges, original->frictionEdges, 6 * numFrictionEdges * sizeof(double) );
        //wrenches are not copied; this contact should build them itself
        loc = original->loc;
        frame = original->frame;
        normal = original->normal;
        cof = original->cof;    
        body1 = original->body1;
        body2 = original->body2;
}

VirtualContact::VirtualContact(int f, int l, Contact *original) : Contact()
{
        init();
        mFingerNum = f;
        mLinkNum = l;

        numFrictionEdges = original->numFrictionEdges;
        memcpy( frictionEdges, original->frictionEdges, 6 * numFrictionEdges * sizeof(double) );
        cof = original->cof;

        loc = original->loc;
        frame = original->frame;
        //we now rotate the frame so that the normal points outwards, as the contact on the object would
        Quaternion q(3.14159, vec3(1,0,0));
        transf newRot(q, vec3(0,0,0) );
        frame = newRot * frame;
        normal = -1 * original->normal;
        body1 = original->body1;
        body2 = original->body2;
}

VirtualContact::~VirtualContact()
{
        mate = NULL;
}

position
VirtualContact::getWorldLocation()
{
        return loc * body1->getTran();
}

vec3
VirtualContact::getWorldNormal()
{
        return normal * body1->getTran();
}

int
VirtualContact::setUpFrictionEdges(bool dynamicsOn)
{
        (void*)&dynamicsOn;
        return 1;
}

void
VirtualContact::computeWrenches(bool useObjectData, bool simplify)
{
        int i;
        //double alpha;
        vec3 radius, forceVec,torqueVec,tangentX,tangentY;

        //we need everything to be wrt world coordinates
        position worldLoc;
        vec3 worldNormal;

        if (!useObjectData) {   
                worldLoc = getWorldLocation();
                worldNormal = getWorldNormal();
                transf worldFrame = frame * body1->getTran();
                tangentX = worldFrame.affine().row(0);
                tangentY = worldFrame.affine().row(1);
        } else {
//              LOOK AT VIRTUAL CONTACT ON THE HAND
                worldLoc = getWorldLocation();
                worldNormal = getWorldNormal();
                tangentX = vec3(0,1,0) * worldNormal;
                tangentX = normalise(tangentX);
                tangentY = worldNormal * tangentX;
        }

        //mCenter needs to be already set to object cog if needed as such
        radius = worldLoc - mCenter;
        if (wrench) delete [] wrench;

        if (simplify) {
                numFCWrenches = 1; //this is hack-ish, should be fixed
                wrench = new Wrench[1];
                wrench[0].force = worldNormal;
                wrench[0].torque = (radius*worldNormal) / mMaxRadius;
                return;
        }

        numFCWrenches = numFrictionEdges;
        wrench = new Wrench[numFrictionEdges];

        //SHOULD SET UP COEFFICIENT BASED ON MATERIALS!
        for (i=0;i<numFCWrenches;i++) {
                forceVec = worldNormal + (tangentX*cof*frictionEdges[6*i])+ (tangentY*cof*frictionEdges[6*i+1]);
                //max friction is normal_force_magnitude (which is 1) * coeff_of_friction 
                //possible friction for this wrench is (friction_edge * max_friction) in the X and Y direction of the contact

                wrench[i].force = forceVec;

                wrench[i].torque = ( (radius*forceVec) + worldNormal*cof*frictionEdges[6*i+5] )/mMaxRadius;
                //max torque is contact_radius * normal_force_magnitude (which is 1) * coeff_of_friction
                //possible torque for this wrench is (friction_edge * max_torque) in the direction of the contact normal
                //the contact_radius coefficient is already taken into account in the friction_edge
        }
}

void
VirtualContact::scaleWrenches(double factor)
{
        int i;
        for (i=0; i<numFCWrenches; i++) {
                wrench[i].force = factor * wrench[i].force;
                wrench[i].torque = factor * wrench[i].torque;
        }
}

void
VirtualContact::getWorldIndicator(bool useObjectData)
{
        vec3 forceVec;
        position worldLoc;

        if (!useObjectData) {
                worldLoc = getWorldLocation();
        } else {
                vec3 objDist;
                getObjectDistanceAndNormal(body2, &objDist, NULL);
                worldLoc = getWorldLocation() + objDist;
        }

        SoTransform* tran = new SoTransform;  
        SbMatrix tr;
        tr.setTranslate( worldLoc.toSbVec3f() );
        tran->setMatrix( tr );
  
        SbVec3f *points = (SbVec3f*)calloc(numFCWrenches+1, sizeof(SbVec3f) );
        int32_t *cIndex = (int32_t*)calloc(4*numFCWrenches, sizeof(int32_t) );

        points[0].setValue(0,0,0);

        for (int i=0;i<numFCWrenches;i++) {
                //if ( wrench[i].torque.len() != 0 ) continue;
                forceVec = wrench[i].force;
                forceVec = Body::CONE_HEIGHT * forceVec;
                points[i+1].setValue( forceVec.x(), forceVec.y(), forceVec.z() );
                cIndex[4*i] = 0;
                cIndex[4*i+1] = i + 2;
                if ( i == numFCWrenches-1) cIndex[4*i+1] = 1;
                cIndex[4*i+2] = i + 1;
                cIndex[4*i+3] = -1;
        }

        SoCoordinate3* coords = new SoCoordinate3;
        SoIndexedFaceSet* ifs = new SoIndexedFaceSet;
        coords->point.setValues(0,numFCWrenches+1,points);
        ifs->coordIndex.setValues(0,4*numFCWrenches,cIndex);
        free(points);
        free(cIndex);

        SoMaterial *coneMat = new SoMaterial;  
    coneMat->diffuseColor = SbColor(0.0f,0.0f,0.8f);
    coneMat->ambientColor = SbColor(0.0f,0.0f,0.2f);
    coneMat->emissiveColor = SbColor(0.0f,0.0f,0.4f);

        if (mWorldInd) {
                body1->getWorld()->getIVRoot()->removeChild(mWorldInd);
        }

        mWorldInd = new SoSeparator;
        mWorldInd->addChild(tran);
        mWorldInd->addChild(coneMat);
        mWorldInd->addChild(coords);
        mWorldInd->addChild(ifs);
        body1->getWorld()->getIVRoot()->addChild(mWorldInd);

        /*
        SoSeparator* cSep = new SoSeparator;
        tr.setTranslate( mCenter.toSbVec3f() );
        tran = new SoTransform;
        tran->setMatrix( tr );
        cSep->addChild(tran);
        SoSphere* cSphere = new SoSphere();
        cSphere->radius = 5;
        cSep->addChild(cSphere);
        body1->getWorld()->getIVRoot()->addChild(cSep);
        */
}

SoSeparator* 
VirtualContact::getVisualIndicator()
{
        SoTransform *tran;

        //this one is a member variable so we can change color if we want to "mark" the contact
        if (!mZaxisMat) {
                mZaxisMat = new SoMaterial;
                mZaxisMat->ref();
        }
        mZaxisMat->diffuseColor = SbColor(0.8f,0,0);
        mZaxisMat->ambientColor = SbColor(0.8f,0,0);

        tran = new SoTransform;  
        getContactFrame().toSoTransform(tran);
  
        SoCylinder *zaxisCyl = new SoCylinder;
        zaxisCyl->radius = 0.2f;
        zaxisCyl->height = Body::CONE_HEIGHT;

        SoSphere *zaxisSphere = new SoSphere;
        zaxisSphere->radius = 3.0f;
  
        SoTransform *zaxisTran = new SoTransform;
        zaxisTran->translation.setValue(0,0,Body::CONE_HEIGHT/2.0);
//      zaxisTran->translation.setValue(0,0,2.0);
        zaxisTran->rotation.setValue(SbVec3f(1,0,0),(float)M_PI/2.0f);
  
        SoSeparator *zaxisSep = new SoSeparator;
        zaxisSep->addChild(zaxisTran);
        zaxisSep->addChild(mZaxisMat);
        zaxisSep->addChild(zaxisCyl);
//      zaxisSep->addChild(zaxisSphere);

        SoSeparator* cne = new SoSeparator;
        cne->addChild(tran);
        cne->addChild(zaxisSep);
        return cne;
}

void
VirtualContact::mark(bool m)
{
        if (!mZaxisMat) return;
        if (m) {
                mZaxisMat->diffuseColor = SbColor(0,0,0.8f);
                mZaxisMat->ambientColor = SbColor(0,0,0.8f);
        } else {
                mZaxisMat->diffuseColor = SbColor(0.8f,0,0);
                mZaxisMat->ambientColor = SbColor(0.8f,0,0);
        }
}

void
VirtualContact::writeToFile(FILE *fp)
{
        //finger and link number
        fprintf(fp,"%d %d\n",mFingerNum, mLinkNum);

        //numFrictionEdges
        fprintf(fp,"%d\n",numFrictionEdges);

        //frictionEdges
        for (int i=0; i<numFrictionEdges; i++) {
                for (int j=0; j<6; j++)
                        fprintf(fp,"%f ",frictionEdges[6*i+j]);
                fprintf(fp,"\n");
        }

        //loc
        fprintf(fp,"%f %f %f\n",loc.x(), loc.y(), loc.z());

        //frame
        Quaternion q = frame.rotation();
        vec3 t = frame.translation();
        fprintf(fp,"%f %f %f %f\n",q.w,q.x,q.y,q.z);
        fprintf(fp,"%f %f %f\n",t.x(), t.y(), t.z());

        //normal
        fprintf(fp,"%f %f %f\n",normal.x(), normal.y(), normal.z());

        //cof
        fprintf(fp,"%f\n",cof);
}

void
VirtualContact::readFromFile(FILE *fp)
{
        float v,x,y,z;

        //finger and link number
        fscanf(fp,"%d %d",&mFingerNum, &mLinkNum);

        //numFrictionEdges
        fscanf(fp,"%d",&numFrictionEdges);

        //frictionEdges
        for (int i=0; i<numFrictionEdges; i++) {
                for (int j=0; j<6; j++) {
                        fscanf(fp,"%f",&v);
                        frictionEdges[6*i+j] = v;
                }
        }

        //loc
        fscanf(fp,"%f %f %f",&x, &y, &z);
        loc = position(x,y,z);

        //frame
        Quaternion q;
        vec3 t;
        fscanf(fp,"%f %f %f %f",&v,&x,&y,&z);
        q.set(v,x,y,z);
        fscanf(fp,"%f %f %f",&x, &y, &z);
        t.set(x,y,z);
        frame.set(q,t);

        //normal
        fscanf(fp,"%f %f %f",&x, &y, &z);
        normal.set(x,y,z);

        //cof
        fscanf(fp,"%f",&v);
        cof = v;
}

void
VirtualContact::getObjectDistanceAndNormal(Body *body, vec3 *objDistance, vec3 *objNormal)
{
        position loc = getWorldLocation();
        *objDistance = body->getWorld()->pointDistanceToBody(loc, body, objNormal);
}


VirtualContactOnObject::VirtualContactOnObject()
{
        wrench = NULL;
        body2 = NULL;
        mate = this;
        prevBetas = NULL;
}

VirtualContactOnObject::~VirtualContactOnObject()
{
}

void
VirtualContactOnObject::readFromFile(FILE *fp)
{
        float w,x,y,z;

        //numFCVectors
        fscanf(fp,"%d",&numFrictionEdges);

        //frictionEdges
        for (int i=0; i<numFrictionEdges; i++) {
                for (int j=0; j<6; j++) {
                        fscanf(fp,"%f",&w);
                        frictionEdges[6*i+j] = w;
                }
        }
        fprintf(stderr,"\n<frictionEdges scanned successfully>"); // for test

        // (w,x,y,z) is already a quaternion, if you want to do frame rotate v rad along a vector (x,y,z),
        //you can use q(v,vec(x,y,z))
        Quaternion q;
        vec3 t;
        fscanf(fp,"%f %f %f %f",&w,&x,&y,&z);
        q.set(w,x,y,z);
        fscanf(fp,"%f %f %f",&x, &y, &z);
        t.set(x,y,z);
        loc = position(x,y,z);
        frame.set(q,t);

        //normal
        fscanf(fp,"%f %f %f",&x, &y, &z);
        normal.set(x,y,z);

        //cof
        fscanf(fp,"%f",&w);
        cof = w;
}

#ifdef ARIZONA_PROJECT_ENABLED
void
VirtualContactOnObject::readFromRawData(ArizonaRawExp* are, QString file, int index, bool flipNormal)
{
        
        float v;
        FILE *fp = fopen(file.latin1(), "r");
        if (!fp) {
                fprintf(stderr,"Could not open filename %s\n",file.latin1());
                return;
        }

        //numFCVectors
        fscanf(fp,"%d",&numFrictionEdges);

        //frictionEdges
        for (int i=0; i<numFrictionEdges; i++) {
                for (int j=0; j<6; j++) {
                        fscanf(fp,"%f",&v);
                        frictionEdges[6*i+j] = v;
                }
        }

        Quaternion q;
        vec3 t;
        q = are->getQuaternion(index);
        t = are->getContact(index);
        loc = position(t.x(),t.y(),t.z());
        frame.set(q,t);

        //normal
        normal = are->getNormal(index);
        if(flipNormal){
                std::cout << "flipped normal" << std::endl;
                normal = - normal;
        }

        //cof, need further consideration
        cof = 0.5;

        fclose(fp);
        
}
#endif

void
VirtualContactOnObject::writeToFile(FILE *fp){
        //numFrictionEdges
        fprintf(fp,"%d\n",numFrictionEdges);

        //frictionEdges
        for (int i=0; i<numFrictionEdges; i++) {
                for (int j=0; j<6; j++)
                        fprintf(fp,"%f ",frictionEdges[6*i+j]);
                fprintf(fp,"\n");
        }

        //frame
        Quaternion q = frame.rotation();
        vec3 t = frame.translation();
        fprintf(fp,"%f %f %f %f\n",q.w,q.x,q.y,q.z);
        fprintf(fp,"%f %f %f\n",t.x(), t.y(), t.z());

        //normal
        fprintf(fp,"%f %f %f\n",normal.x(), normal.y(), normal.z());

        //cof
        fprintf(fp,"%f\n",cof);
}

---

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