[Feature] States not necessarily deleted when interpolation is called

idl/hpp/corbaserver/rbprm/rbprmbuilder.idl

@@ -371,7 +371,7 @@ module hpp
     /// \param filterStates If different than 0, the resulting state list will be filtered to remove unnecessary states
     /// \param testReachability : if true, check each contact transition with our reachability criterion
     /// \param quasiStatic : if True, use our reachability criterion with the quasiStatic constraint
-    floatSeqSeq interpolate(in double timestep, in double path, in double robustnessTreshold, in unsigned short filterStates, in boolean testReachability, in boolean quasiStatic) raises (Error);
+    floatSeqSeq interpolate(in double timestep, in double path, in double robustnessTreshold, in unsigned short filterStates, in boolean testReachability, in boolean quasiStatic, in boolean erasePreviousStates) raises (Error);
 
     /// Provided a path has already been computed, interpolates it and generates the statically stable
     /// constact configurations along it. setStartState and setEndState must have been called prior
@@ -382,7 +382,7 @@ module hpp
     /// \param filterStates If different than 0, the resulting state list will be filtered to remove unnecessary states
     /// \param testReachability : if true, check each contact transition with our reachability criterion
     /// \param quasiStatic : if True, use our reachability criterion with the quasiStatic constraint
-    floatSeqSeq interpolateConfigs(in floatSeqSeq configs, in double robustnessTreshold, in unsigned short filterStates,in boolean testReachability, in boolean quasiStatic) raises (Error);
+    floatSeqSeq interpolateConfigs(in floatSeqSeq configs, in double robustnessTreshold, in unsigned short filterStates,in boolean testReachability, in boolean quasiStatic, in boolean erasePreviousStates) raises (Error);
 
 
     /// returns the CWC of the robot at a given state
@@ -670,6 +670,10 @@ module hpp
     /// \return whether the limb is in contact at this state
     short computeIntermediary(in unsigned short state1, in unsigned short state2) raises (Error);
 
+    /// Compute the number of computed states
+    /// \return the number of computed states
+    short getNumStates() raises (Error);
+
     /// Saves the last computed states by the function interpolate in a filename.
     /// Raises an error if interpolate has not been called, or the file could not be opened.
     /// \param filename name of the file used to save the contacts.

src/hpp/corbaserver/rbprm/rbprmfullbody.py

@@ -257,6 +257,11 @@ class FullBody (Robot):
      def getNumSamples(self, limbName):
           return self.clientRbprm.rbprm.getNumSamples(limbName)
           
+     ## Get the number of existing states in the client 
+     #
+     def getNumStates(self, limbName):
+          return self.clientRbprm.rbprm.getNumStates()
+          
      ## Get the number of octreeNodes
      #
      # \param limbName name of the limb from which to retrieve a sample
@@ -348,12 +353,13 @@ class FullBody (Robot):
      # \param filterStates If different than 0, the resulting state list will be filtered to remove unnecessary states
      # \param testReachability : if true, check each contact transition with our reachability criterion
      # \param quasiStatic : if True, use our reachability criterion with the quasiStatic constraint
-     def interpolate(self, stepsize, pathId = 1, robustnessTreshold = 0, filterStates = False,testReachability = True, quasiStatic = False):
+     # \param erasePreviousStates : if True the list of previously computed states is erased
+     def interpolate(self, stepsize, pathId = 1, robustnessTreshold = 0, filterStates = False,testReachability = True, quasiStatic = False, erasePreviousStates = True):
           if(filterStates):
                 filt = 1
           else:
                 filt = 0
-          return self.clientRbprm.rbprm.interpolate(stepsize, pathId, robustnessTreshold, filt,testReachability, quasiStatic)
+          return self.clientRbprm.rbprm.interpolate(stepsize, pathId, robustnessTreshold, filt,testReachability, quasiStatic, erasePreviousStates)
      
      ## Provided a discrete contact sequence has already been computed, computes
      # all the contact positions and normals for a given state, the next one, and the intermediate between them.

src/rbprmbuilder.impl.cc

@@ -1544,7 +1544,7 @@ namespace hpp {
         return res;
     }
 
-    floatSeqSeq* RbprmBuilder::interpolateConfigs(const hpp::floatSeqSeq& configs, double robustnessTreshold, unsigned short filterStates, bool testReachability, bool quasiStatic) throw (hpp::Error)
+    floatSeqSeq* RbprmBuilder::interpolateConfigs(const hpp::floatSeqSeq& configs, double robustnessTreshold, unsigned short filterStates, bool testReachability, bool quasiStatic, bool erasePreviousStates) throw (hpp::Error)
     {
         try
         {
@@ -1563,15 +1563,16 @@ namespace hpp {
                 throw hpp::Error ("No affordances found. Unable to interpolate.");
             }
             hpp::rbprm::interpolation::RbPrmInterpolationPtr_t interpolator = rbprm::interpolation::RbPrmInterpolation::create(fullBody(),startState_,endState_,core::PathVectorConstPtr_t(),testReachability,quasiStatic);
-            lastStatesComputedTime_ = interpolator->Interpolate(affMap, bindShooter_.affFilter_,configurations,robustnessTreshold, filterStates != 0);
-            lastStatesComputed_ = TimeStatesToStates(lastStatesComputedTime_);
+            rbprm::T_StateFrame newTimeStates =
+                    interpolator->Interpolate(affMap, bindShooter_.affFilter_,configurations,robustnessTreshold, filterStates != 0);
+            std::vector<rbprm::State> newStates =TimeStatesToStates(newTimeStates);
             hpp::floatSeqSeq *res;
             res = new hpp::floatSeqSeq ();
 
-            res->length ((_CORBA_ULong)lastStatesComputed_.size ());
+            res->length ((_CORBA_ULong)newStates.size ());
             std::size_t i=0;
             std::size_t id = 0;
-            for(std::vector<State>::const_iterator cit = lastStatesComputed_.begin(); cit != lastStatesComputed_.end(); ++cit, ++id)
+            for(std::vector<State>::const_iterator cit = newStates.begin(); cit != newStates.end(); ++cit, ++id)
             {
                 std::cout << "ID " << id;
                 cit->print();
@@ -1586,6 +1587,16 @@ namespace hpp {
                 (*res) [(_CORBA_ULong)i] = floats;
                 ++i;
             }
+            if(erasePreviousStates)
+            {
+                lastStatesComputedTime_ = newTimeStates;
+                lastStatesComputed_ = newStates;
+            }
+            else
+            {
+                lastStatesComputed_.insert(lastStatesComputed_.begin(), newStates.begin(), newStates.end());
+                lastStatesComputedTime_.insert(lastStatesComputedTime_.begin(), newTimeStates.begin(), newTimeStates.end());
+            }
             return res;
         }
         catch(std::runtime_error& e)
@@ -2233,7 +2244,7 @@ namespace hpp {
     }
 
 
-    floatSeqSeq* RbprmBuilder::interpolate(double timestep, double path, double robustnessTreshold, unsigned short filterStates, bool testReachability, bool quasiStatic) throw (hpp::Error)
+    floatSeqSeq* RbprmBuilder::interpolate(double timestep, double path, double robustnessTreshold, unsigned short filterStates, bool testReachability, bool quasiStatic, bool erasePreviousStates) throw (hpp::Error)
     {
         hppDout(notice,"### Begin interpolate");
         try
@@ -2260,18 +2271,19 @@ namespace hpp {
 
         hpp::rbprm::interpolation::RbPrmInterpolationPtr_t interpolator =
                     rbprm::interpolation::RbPrmInterpolation::create(fullBody(),startState_,endState_,problemSolver()->paths()[pathId],testReachability,quasiStatic);
-        lastStatesComputedTime_ = interpolator->Interpolate(affMap, bindShooter_.affFilter_,
+
+        rbprm::T_StateFrame newTimeStates = interpolator->Interpolate(affMap, bindShooter_.affFilter_,
                     timestep,robustnessTreshold, filterStates != 0);
-		lastStatesComputed_ = TimeStatesToStates(lastStatesComputedTime_);
+        std::vector<rbprm::State> newStates = TimeStatesToStates(newTimeStates);
 
         hpp::floatSeqSeq *res;
         res = new hpp::floatSeqSeq ();
 
-        res->length ((_CORBA_ULong)lastStatesComputed_.size ());
+        res->length ((_CORBA_ULong)newStates.size ());
         std::size_t i=0;
         std::size_t id = 0;
-        for(std::vector<State>::const_iterator cit = lastStatesComputed_.begin();
-					cit != lastStatesComputed_.end(); ++cit, ++id)
+        for(std::vector<State>::const_iterator cit = newStates.begin();
+                    cit != newStates.end(); ++cit, ++id)
         {
             /*std::cout << "ID " << id;
             cit->print();*/
@@ -2286,6 +2298,16 @@ namespace hpp {
             (*res) [(_CORBA_ULong)i] = floats;
             ++i;
         }
+        if(erasePreviousStates)
+        {
+            lastStatesComputedTime_ = newTimeStates;
+            lastStatesComputed_ = newStates;
+        }
+        else
+        {
+            lastStatesComputed_.insert(lastStatesComputed_.begin(), newStates.begin(), newStates.end());
+            lastStatesComputedTime_.insert(lastStatesComputedTime_.begin(), newTimeStates.begin(), newTimeStates.end());
+        }
         return res;
         }
         catch(std::runtime_error& e)
@@ -3051,6 +3073,12 @@ namespace hpp {
     }
 
 
+
+    CORBA::Short  RbprmBuilder::getNumStates() throw (hpp::Error)
+    {
+        return lastStatesComputed_.size();
+    }
+
     CORBA::Short  RbprmBuilder::computeIntermediary(unsigned short stateFrom, unsigned short stateTo) throw (hpp::Error)
     try
     {

src/rbprmbuilder.impl.hh

@@ -261,8 +261,8 @@ namespace hpp {
         virtual hpp::floatSeqSeq* computeContactPointsForLimb(unsigned short cId, const char* limbName) throw (hpp::Error);
         virtual hpp::floatSeqSeq* computeContactPointsAtStateForLimb(unsigned short cId, unsigned short isIntermediate, const char* limbName) throw (hpp::Error);
         virtual hpp::floatSeqSeq* computeCenterOfContactAtStateForLimb(unsigned short cId, unsigned short isIntermediate, const char *limbName) throw (hpp::Error);
-        virtual hpp::floatSeqSeq* interpolate(double timestep, double path, double robustnessTreshold, unsigned short filterStates, bool testReachability, bool quasiStatic) throw (hpp::Error);
-        virtual hpp::floatSeqSeq* interpolateConfigs(const hpp::floatSeqSeq& configs, double robustnessTreshold, unsigned short filterStates, bool testReachability, bool quasiStatic) throw (hpp::Error);
+        virtual hpp::floatSeqSeq* interpolate(double timestep, double path, double robustnessTreshold, unsigned short filterStates, bool testReachability, bool quasiStatic, bool erasePreviousStates) throw (hpp::Error);
+        virtual hpp::floatSeqSeq* interpolateConfigs(const hpp::floatSeqSeq& configs, double robustnessTreshold, unsigned short filterStates, bool testReachability, bool quasiStatic, bool erasePreviousStates) throw (hpp::Error);
         virtual hpp::floatSeqSeq* getContactCone(unsigned short stateId, double friction) throw (hpp::Error);
         virtual hpp::floatSeqSeq* getContactIntermediateCone(unsigned short stateId, double friction) throw (hpp::Error);
         virtual CORBA::Short generateComTraj(const hpp::floatSeqSeq& positions, const hpp::floatSeqSeq& velocities,
@@ -346,6 +346,7 @@ namespace hpp {
         virtual CORBA::Short  isLimbInContact(const char* limbName, unsigned short state) throw (hpp::Error);
         virtual CORBA::Short  isLimbInContactIntermediary(const char* limbName, unsigned short state) throw (hpp::Error);
         virtual CORBA::Short  computeIntermediary(unsigned short state1, unsigned short state2) throw (hpp::Error);
+        virtual CORBA::Short  getNumStates() throw (hpp::Error);
         virtual hpp::floatSeqSeq* getOctreeBoxes(const char* limbName, const hpp::floatSeq& configuration) throw (hpp::Error);
         virtual hpp::floatSeq* getOctreeTransform(const char* limbName, const hpp::floatSeq& configuration) throw (hpp::Error);
         virtual CORBA::Short isConfigBalanced(const hpp::floatSeq& config, const hpp::Names_t& contactLimbs, double robustnessTreshold,const hpp::floatSeq& CoM) throw (hpp::Error);