rebase

src/hpp/corbaserver/rbprm/rbprmfullbody.py

@@ -242,7 +242,9 @@ class FullBody (object):
     # \param pathId Id of the path to compute from
     # \param robustnessTreshold minimum value of the static equilibrium robustness criterion required to accept the configuration (0 by default).
     def computeContactPoints(self, stateId):
-		return self.client.rbprm.rbprm.computeContactPoints(stateId)
+		rawdata = self.client.rbprm.rbprm.computeContactPoints(stateId)
+		return [[b[i:i+6] for i in range(0, len(b), 6)] for b in rawdata]
+
 		
 	## Given start and goal states
 	#  generate a contact sequence over a list of configurations

src/rbprmbuilder.impl.cc

@@ -661,20 +661,29 @@ namespace hpp {
         const State& firstState = lastStatesComputed_[cId], thirdState = lastStatesComputed_[cId+1];
         std::vector<std::vector<fcl::Vec3f> > allStates;
         allStates.push_back(computeRectangleContact(fullBody_, firstState));
-        std::vector<std::string> variations =thirdState.contactVariations(firstState);
-        if(variations.size() >1)
+        std::vector<std::string> breaks, creations;
+        thirdState.contactBreaks(firstState, breaks);
+        if(breaks.size() > 1)
         {
-            throw std::runtime_error ("too many state variation between states" + std::string(""+cId) +
+            throw std::runtime_error ("too many contact breaks between states" + std::string(""+cId) +
                                       "and " + std::string(""+(cId + 1)));
         }
-        if(!variations.empty())
+        if(breaks.size() == 1)
         {
-std::cout << "variation " << variations[0] << std::endl;
             State intermediary(firstState);
-            intermediary.RemoveContact(*variations.begin());
+            intermediary.RemoveContact(*breaks.begin());
             allStates.push_back(computeRectangleContact(fullBody_, intermediary));
         }
-        allStates.push_back(computeRectangleContact(fullBody_, thirdState));
+        thirdState.contactCreations(firstState, creations);
+        if(creations.size() == 1)
+        {
+            allStates.push_back(computeRectangleContact(fullBody_, thirdState));
+        }
+        if(creations.size() > 1)
+        {
+            throw std::runtime_error ("too many contact creations between states" + std::string(""+cId) +
+                                      "and " + std::string(""+(cId + 1)));
+        }
 
         hpp::floatSeqSeq *res;
         res = new hpp::floatSeqSeq ();