graspit: /opt/ros/diamondback/stacks/graspit_simulator/graspit/graspit_source/src/contactSetting.cpp Source File

00001 //######################################################################
00002 //
00003 // GraspIt!
00004 // Copyright (C) 2002-2009  Columbia University in the City of New York.
00005 // All rights reserved.
00006 //
00007 // GraspIt! is free software: you can redistribute it and/or modify
00008 // it under the terms of the GNU General Public License as published by
00009 // the Free Software Foundation, either version 3 of the License, or
00010 // (at your option) any later version.
00011 //
00012 // GraspIt! is distributed in the hope that it will be useful,
00013 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00014 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00015 // GNU General Public License for more details.
00016 //
00017 // You should have received a copy of the GNU General Public License
00018 // along with GraspIt!.  If not, see <http://www.gnu.org/licenses/>.
00019 //
00020 // Author(s):  Andrew T. Miller, Claire Lackner and Matei T. Ciocarlie
00021 //
00022 // $Id: contactSetting.cpp,v 1.3 2009/06/16 19:28:28 cmatei Exp $
00023 //
00024 //######################################################################
00025 
00026 #include "contactSetting.h"
00027 #include "contact.h"
00028 #include "matvec3D.h"
00029 #include "world.h"
00030 #include "body.h"
00031 #include "mytools.h"
00032 #include "FitParabola.h"
00033 
00034 //#define GRASPITDBG
00035 #include "debug.h"
00036 
00037 double 
00038 contactDistance(Body* body1, Body* body2, ContactData &cp)
00039 {
00040         position b1_pos(cp.b1_pos), b2_pos(cp.b2_pos);
00041         b1_pos = b1_pos * body1->getTran();
00042         b2_pos = b2_pos * body2->getTran();
00043         vec3 dist_vec = (b1_pos-b2_pos);
00044         return dist_vec.len();
00045 }
00046 
00047 /*      
00048         Attempt to check if the normals of the contact are well defined by looking at the distance between the bodies.
00049 */      
00050 bool 
00051 checkContactNormals(Body *b1, Body *b2, ContactData *c)
00052 {
00053         bool r = true;
00054         vec3 n1(c->b1_normal);
00055         vec3 n2(c->b2_normal);
00056         n1 = n1 * b1->getTran();
00057         n2 = n2 * b2->getTran();
00058 
00059         position p1(c->b1_pos);
00060         position p2(c->b2_pos);
00061         p1 = p1 * b1->getTran();
00062         p2 = p2 * b2->getTran();
00063 
00064         vec3 d = p2 - p1;
00065         if ( d % n1 > 0) {
00066                 r = false;
00067                 for (int i=0; i<3; i++) {
00068                         c->b1_normal[i] = -c->b1_normal[i];
00069                 }
00070         }
00071         if ( d % n2 < 0) {
00072                 r = false;
00073                 for (int i=0; i<3; i++) {
00074                         c->b2_normal[i] = - c->b2_normal[i];
00075                 }
00076         }
00077         return r;
00078 }
00079 
00080 bool neighborhoodsOverlap(const Neighborhood &n1, const Neighborhood &n2) {
00081         // both neighborhoods are assumed to be in the same frame 
00082         Neighborhood::const_iterator it1, it2;
00083         for (it1 = n1.begin(); it1!=n1.end(); it1++)
00084                 for (it2 = n2.begin(); it2 != n2.end(); it2++) {
00085                         if ( *it1 == *it2 )
00086                                 return true;
00087                 }
00088         return false;
00089 }
00090 
00091 void mergeNeighborhoods(Neighborhood &n1, Neighborhood &n2)
00092 {
00093         Neighborhood::iterator it1, it2;
00094         bool present;
00095         for (it2 = n2.begin(); it2 != n2.end(); it2++) {
00096                 present = false;
00097                 for (it1 = n1.begin(); it1 != n1.end(); it1++) {
00098                         if ( *it1 == *it2 ) {
00099                                 present = true;
00100                                 break;
00101                         }
00102                 }
00103                 if (!present) {
00104                         n1.push_back( *it2 );
00105                 }
00106         }
00107 }
00108 
00112 void findSoftNeighborhoods( Body *body1, Body *body2, ContactReport &contactSet )
00113 {
00114         ContactReport::iterator itr;
00115 
00116         for( itr = contactSet.begin(); itr != contactSet.end(); itr++ ) {
00117                 DBGP("Contact finding regions:");
00118                 //right now, findregion assumes point is in body frame
00119                 //the units for the threshold and the radius should be in mm, NOT cm
00120 
00121                 //The input radius is proportional to the fourth root of the youngs mod/depth of mattress
00122                 //(units for Youngs Mod and mattress depth are in Pa and meters)
00123                 //This gives a radius around 6 mm, for rubber with youngs = 1.5E6 and h = 3E-3which is reasonable
00124 
00125                 double rad = pow( 1/(MAX( body1->getYoungs(), body2->getYoungs() )), 0.333 ) * 1000.0 * 0.4;
00126                 //the 0.4 is a fudge factor for the time being
00127 
00128                 //hack to ensure that the fit is at least resonable
00129                 if( rad <= 3.0 && rad >= 10.0 ) rad = 5.0;
00130 
00131                 body1->getWorld()->FindRegion( body1, itr->b1_pos, itr->b1_normal, rad, &(itr->nghbd1) );
00132                 DBGP("Neighborhood on body1 has " << itr->nghbd1.size() << " points");
00133                 body2->getWorld()->FindRegion( body2, itr->b2_pos, itr->b2_normal, rad, &(itr->nghbd2) );
00134                 DBGP("Neighborhood on body2 has " << itr->nghbd2.size() << " points");
00135         }
00136 }
00137 
00142 void
00143 mergeSoftNeighborhoods(Body *body1, Body *body2, ContactReport &contactSet)
00144 {
00145         bool mergePerformed = true;
00146 
00147         ContactReport::iterator refContact, otherContact;
00148 
00149         while (mergePerformed) {
00150                 mergePerformed = false;
00151                 //check each contact agains each other contact
00152                 for (refContact=contactSet.begin(); refContact!=contactSet.end(); refContact++) {
00153                         for (otherContact = contactSet.begin(); otherContact != contactSet.end(); otherContact++) {
00154                                 if (otherContact == refContact) continue;
00155 
00156                                 //this arbitrarily checks only the neighborhoods on body1
00157                                 if ( neighborhoodsOverlap(refContact->nghbd1, otherContact->nghbd1) ) {
00158                                         DBGP("Overlap found");
00159                                         // this should be improved; it keeps the closest contact but merges neighborhoods
00160                                         // ideally it should keep the closest contact and RECALCULATE the neighborhoods
00161                                         if ( contactDistance(body1, body2, *refContact) < contactDistance(body1, body2, *otherContact) ) {
00162                                                 mergeNeighborhoods( refContact->nghbd1, otherContact->nghbd1 );
00163                                                 mergeNeighborhoods( refContact->nghbd2, otherContact->nghbd2 );
00164                                                 contactSet.erase( otherContact );
00165                                         } else {
00166                                                 mergeNeighborhoods( otherContact->nghbd1, refContact->nghbd1 );
00167                                                 mergeNeighborhoods( otherContact->nghbd2, refContact->nghbd2 );
00168                                                 contactSet.erase( refContact );
00169                                         }
00170                                         mergePerformed = true;
00171                                         break;
00172                                 } else {
00173                                         DBGP("Overlap not found");
00174                                 }
00175                         }
00176                         //if we have a merge, restart operation
00177                         if (mergePerformed)
00178                                 break;
00179                 }
00180         }
00181 }
00182 
00188 void
00189 addContacts(Body *body1, Body *body2, ContactReport &contactSet, bool softContactsOn )
00190 {
00191         ContactReport::iterator cp;
00192         Contact *c1,*c2;
00193         int i;
00194 
00195         if ( softContactsOn && ( body1->isElastic() || body2->isElastic() ) ) {
00196                 findSoftNeighborhoods( body1, body2, contactSet);
00197                 DBGP("Before merge: " << contactSet.size());
00198                 mergeSoftNeighborhoods( body1, body2, contactSet);
00199                 DBGP("After merge: " << contactSet.size());
00200         }
00201 
00202         for (i=0,cp=contactSet.begin();cp!=contactSet.end();cp++,i++) {
00203 
00204                 DBGP( body1->getName().latin1() << " - " << body2->getName().latin1() << " contact: " <<
00205                 cp->b1_pos << " " <<  cp->b1_normal );
00206 
00207                 //this is an attempt to check if the contact normals point in the right direction
00208                 //based on the distance between the bodies. It is meant to help with bad geometry with ill-defined
00209                 //normals. Can be removed completely - should never come up for good geometry
00210                 if (! checkContactNormals(body1, body2, &(*cp)) ) {
00211                         DBGP("Wrong normals detected!");
00212                 }
00213                 if ( softContactsOn && ( body1->isElastic() || body2->isElastic() ) ) {
00214                         c1 = new SoftContact( body1, body2, cp->b1_pos, cp->b1_normal, &(cp->nghbd1) );
00215                         c2 = new SoftContact( body2, body1, cp->b2_pos, cp->b2_normal, &(cp->nghbd2) );
00216                         c1->setMate(c2);
00217                         c2->setMate(c1);
00218 
00219                         ((SoftContact *)c1)->setUpFrictionEdges();
00220                         ((SoftContact *)c2)->setUpFrictionEdges();
00221                 } else {
00222                         c1 = new PointContact(body1,body2,cp->b1_pos,cp->b1_normal);
00223                         c2 = new PointContact(body2,body1,cp->b2_pos,cp->b2_normal);
00224                         c1->setMate(c2);
00225                         c2->setMate(c1);
00226                 }
00227 
00228                 body1->addContact(c1);
00229                 body2->addContact(c2);
00230 
00231                 //check if the new contacts inherit two contacts from previous time step
00232                 //if so, remove ancestors so nobody else inherits them
00233                 Contact *ancestor = body1->checkContactInheritance(c1);
00234                 if (ancestor) {
00235                         c1->inherit(ancestor);
00236                         if (!ancestor->getMate()) 
00237                                 fprintf(stderr,"No mate for inherited contact!!\n");
00238                         else
00239                                 c2->inherit(ancestor->getMate());
00240                         //careful: this also deletes the removed contact so remove the mate first
00241                         if (ancestor->getMate()) body2->removePrevContact( ancestor->getMate() );
00242                         body1->removePrevContact( ancestor );
00243                 } else {
00244                         ancestor = body2->checkContactInheritance(c2);
00245                         if (ancestor){
00246                                 if (!ancestor->getMate())
00247                                         fprintf(stderr,"No mate for inherited contact!!\n");
00248                                 else            
00249                                         c1->inherit(ancestor->getMate());
00250                                 c2->inherit(ancestor);
00251                                 if (ancestor->getMate()) body1->removePrevContact( ancestor->getMate() );
00252                                 body2->removePrevContact( ancestor );
00253                         } else {
00254 //                              fprintf(stderr,"New contact between %s and %s\n",body1->getName().latin1(), body2->getName().latin1() );
00255                         }
00256                 }
00257         }
00258 }
00259 
00267 void
00268 addVirtualContacts(Body *body1, int f, int l, Body *body2, ContactReport &contactSet, 
00269                                    bool softContactsOn )
00270 {
00271         if ( softContactsOn && body1->isElastic() )     {
00272                 findSoftNeighborhoods( body1, body2, contactSet );
00273                 mergeSoftNeighborhoods(body1, body2, contactSet);
00274         }
00275 
00276         ContactReport::iterator cp;
00277         for (cp=contactSet.begin(); cp!=contactSet.end(); cp++) {
00278                 Contact *c1;
00279                 if ( softContactsOn && body1->isElastic() )     {
00280                         c1 = new SoftContact( body1, body2, cp->b1_pos, cp->b1_normal, &(cp->nghbd1) );
00281                         c1->setMate(NULL);
00282                         ((SoftContact *)c1)->setUpFrictionEdges();
00283                 } else {
00284                         c1 = new PointContact(body1, body2, cp->b1_pos, cp->b1_normal);
00285                         c1->setMate(NULL);
00286                 }
00287                 
00288                 VirtualContact *vc = new VirtualContact(f, l, c1);
00289                 vc->setBody(body1);
00290                 body1->addVirtualContact(vc);
00291                 delete c1;
00292         }
00293 }