Merge remote-tracking branch 'roamlab/master'

include/graspit/body.h

@@ -92,6 +92,12 @@ class Body : public WorldElement {
     //! The Young's Modulus of the material, it describes its elasticity
     double youngMod;
 
+    //! The normal compliance of the link contact material for grasp compliance analysis
+    double normalCompl;
+
+    //! The shear compliance of the link contact material for grasp compliance analysis
+    double shearCompl;
+
     //! The file that geometry was loaded from, if any
     QString mGeometryFilename;
 
@@ -274,6 +280,12 @@ class Body : public WorldElement {
     //! Returns the Young's modulus for this body
     double getYoungs() { return youngMod; }
 
+    //! Returns the normal compliance of the link contact material
+    double getNormalCompl() { return normalCompl; }
+
+    //! Returns the shear compliance of the link contact material
+    double getShearCompl() { return shearCompl; }
+
     /*! Returns a pointer to the root of the Inventor geometry that was loaded.*/
     SoSeparator *getIVGeomRoot() const {return IVGeomRoot;}
 

src/body.cpp

@@ -112,6 +112,8 @@ Body::Body(World *w, const char *name) : WorldElement(w, name)
   //defaults:
   mIsElastic = false;
   youngMod = -1;
+  normalCompl = -1;
+  shearCompl = -1;
 
   material = -1;
   numContacts = 0;
@@ -140,6 +142,8 @@ Body::Body(const Body &b) : WorldElement(b)
   mIsElastic = b.mIsElastic;
   material = b.material;
   youngMod = b.youngMod;
+  normalCompl = b.normalCompl;
+  shearCompl = b.shearCompl;
   showFC = b.showFC;
   mUsesFlock = b.mUsesFlock;
   mRenderGeometry = true;
@@ -183,6 +187,8 @@ Body::cloneFrom(const Body *original)
 {
   mIsElastic = original->mIsElastic;
   youngMod = original->youngMod;
+  normalCompl = original->normalCompl;
+  shearCompl = original->shearCompl;
   material = original->material;
 
   //add virtual contacts
@@ -251,6 +257,30 @@ Body::loadFromXml(const TiXmlElement *root, QString rootPath)
     mIsElastic = true;
   }
 
+  //Contact compliance of link material
+  element = findXmlElement(root, "normalCompliance");
+  if (element) {
+    valueStr = element->GetText();
+    normalCompl = valueStr.toDouble();
+    if (normalCompl <= 0) {
+      QTWARNING("invalid normal compliance in body file");
+      return FAILURE;
+    }
+  }
+  element = findXmlElement(root, "shearCompliance");
+  if (element) {
+    valueStr = element->GetText();
+    shearCompl = valueStr.toDouble();
+    if (normalCompl <= 0) {
+      QTWARNING("invalid shear compliance in body file");
+      return FAILURE;
+    }
+  }
+  if (!((normalCompl > 0) == (shearCompl > 0))) {
+    QTWARNING("invalid compliance in body file. Must set both normal and shear compliance (or neither)");
+    return FAILURE;
+  }
+
   //Use Flock of Birds
   element = findXmlElement(root, "useFlockOfBirds");
   if (element) {
@@ -341,6 +371,12 @@ Body::saveToXml(QTextStream &xml) {
   if (youngMod > 0) {
     xml << "\t\t\t<youngs>" << youngMod << "</youngs>" << endl;
   }
+  if (normalCompl > 0) {
+    xml << "\t\t\t<normalCompliance>" << normalCompl << "</normalCompliance>" << endl;
+  }
+  if (shearCompl > 0) {
+    xml << "\t\t\t<shearCompliance>" << shearCompl << "</shearCompliance>" << endl;
+  }
   if (mUsesFlock) {
     xml << "\t\t\t<useFlockOfBirds>" << mBirdNumber << "</useFlockOfBirds>" << endl;
   }

src/grasp.cpp

@@ -1414,8 +1414,25 @@ Grasp::stiffnessMatrix(const std::list<Joint*> &joints,
   Matrix H(contactModelMatrix(numContacts, states));
 
   // Finger tip compliance
-  Matrix Cs(Matrix::EYE(6*numContacts, 6*numContacts));
-  for (int i=0; i<numContacts; i++) Cs.elem(6*i+2, 6*i+2) = 0.2;  
+  Matrix Cs(Matrix::ZEROES<Matrix>(6*numContacts, 6*numContacts));
+  std::list<Contact*>::const_iterator it = contacts.begin();
+  for (int i=0; it!=contacts.end(); i++, it++) {
+    double normalCompl = (*it)->getBody1()->getNormalCompl();
+    double shearCompl = (*it)->getBody1()->getShearCompl();
+    if (normalCompl > 0) {
+      Cs.elem(6*i+2, 6*i+2) = normalCompl;
+    } else {
+      Cs.elem(6*i+2, 6*i+2) = 1.0;
+    }
+    if (shearCompl > 0) {
+      Cs.elem(6*i, 6*i) = shearCompl;
+      Cs.elem(6*i+1, 6*i+1) = shearCompl;
+    } else {
+      Cs.elem(6*i, 6*i) = 1.0;
+      Cs.elem(6*i+1, 6*i+1) = 1.0;
+    }
+  }
+
   // Joint compliance
   Matrix Cq(Matrix::ZEROES<Matrix>(numJoints, numJoints));
   Matrix J(contactJacobian(joints, contacts));

src/robot.cpp

@@ -234,6 +234,42 @@ Robot::loadFromXml(const TiXmlElement *root, QString rootPath)
 
   //optional information
 
+  //disable collisions
+  elementList = findAllXmlElements(root, "disableCollision");
+  for (p = elementList.begin(); p != elementList.end(); p++) {
+    int b1_link, b1_chain, b2_link, b2_chain;
+    if ((*p)->QueryIntAttribute("body1_link", &b1_link)!=TIXML_SUCCESS ||
+        (*p)->QueryIntAttribute("body1_chain", &b1_chain)!=TIXML_SUCCESS ||
+        (*p)->QueryIntAttribute("body2_link", &b2_link)!=TIXML_SUCCESS ||
+        (*p)->QueryIntAttribute("body2_chain", &b2_chain)!=TIXML_SUCCESS) {
+      DBGA("Failed to read collision disabling pair");
+      continue;
+    }
+    Body *body1, *body2;
+    if (b1_chain < 0) body1 = base;
+    else {
+      if (b1_chain < getNumChains() && b1_link >=0 &&
+          b1_link < getChain(b1_chain)->getNumLinks())
+        body1 = getChain(b1_chain)->getLink(b1_link);
+      else {
+        DBGA("Incorrect pair for collision disabling");
+        continue;
+      }
+    }
+    if (b2_chain < 0) body2 = base;
+    else {
+      if (b2_chain < getNumChains() && b2_link >=0 &&
+          b2_link < getChain(b2_chain)->getNumLinks())
+        body2 = getChain(b2_chain)->getLink(b2_link);
+      else {
+        DBGA("Incorrect pair for collision disabling");
+        continue;
+      }
+    }
+    myWorld->toggleCollisions(false, body1, body2);
+  }  
+
+
   //load approach direction
   approachTran = transf::IDENTITY;
   element = findXmlElement(root, "approachDirection");