interpolate now takes extra optional param to filter states

idl/hpp/corbaserver/rbprm/rbprmbuilder.idl

@@ -234,7 +234,9 @@ module hpp
     /// to this function. If these conditions are not met an error is raised.
     /// \param timestep normalized step for generation along the path (ie the path has a length of 1).
     /// \param path path computed.
-    floatSeqSeq interpolate(in double timestep, in double path, in double robustnessTreshold) raises (Error);
+    /// \param robustnessTreshold minimum value of the static equilibrium robustness criterion required to accept the configuration (0 by default).
+    /// \param filterStates If different than 0, the resulting state list will be filtered to remove unnecessary states
+    floatSeqSeq interpolate(in double timestep, in double path, in double robustnessTreshold, in unsigned short filterStates) 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
@@ -242,7 +244,8 @@ module hpp
     /// \param timestep normalized step for generation along the path (ie the path has a length of 1).
     /// \param path path computed.
     /// \param robustnessTreshold minimum value of the static equilibrium robustness criterion required to accept the configuration (0 by default).
-    floatSeqSeq interpolateConfigs(in floatSeqSeq configs, in double robustnessTreshold) raises (Error);
+    /// \param filterStates If different than 0, the resulting state list will be filtered to remove unnecessary states
+    floatSeqSeq interpolateConfigs(in floatSeqSeq configs, in double robustnessTreshold, in unsigned short filterStates) raises (Error);
 
 
     /// returns the CWC of the robot at a given state

src/CMakeLists.txt

@@ -110,6 +110,7 @@ INSTALL(
         ${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/tools//generateROMs.py
         ${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/tools/plot_analytics.py
         ${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/tools/cwc_trajectory.py
+        ${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/tools/cwc_trajectory_helper.py
         ${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/tools/path_to_trajectory.py
         ${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/tools/obj_to_constraints.py
   DESTINATION ${PYTHON_SITELIB}/hpp/corbaserver/rbprm/tools

src/hpp/corbaserver/rbprm/rbprmfullbody.py

@@ -254,8 +254,13 @@ class FullBody (object):
     # \param stepSize discretization step
     # \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 interpolate(self, stepsize, pathId = 1, robustnessTreshold = 0):
-		return self.client.rbprm.rbprm.interpolate(stepsize, pathId, robustnessTreshold)
+    # \param filterStates If different than 0, the resulting state list will be filtered to remove unnecessary states
+    def interpolate(self, stepsize, pathId = 1, robustnessTreshold = 0, filterStates = False):
+		if(filterStates):
+			filt = 1
+		else:
+			filt = 0
+		return self.client.rbprm.rbprm.interpolate(stepsize, pathId, robustnessTreshold, filt)
 	
 	## 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.
@@ -272,8 +277,14 @@ class FullBody (object):
 	#
     # \param stepSize discretization step
     # \param pathId Id of the path to compute from
-    def interpolateConfigs(self, configs):
-		return self.client.rbprm.rbprm.interpolateConfigs(configs)
+    # \param robustnessTreshold minimum value of the static equilibrium robustness criterion required to accept the configuration (0 by default).
+    # \param filterStates If different than 0, the resulting state list will be filtered to remove unnecessary states
+    def interpolateConfigs(self, configs, robustnessTreshold = 0, filterStates = False):
+		if(filterStates):
+			filt = 1
+		else:
+			filt = 0
+		return self.client.rbprm.rbprm.interpolateConfigs(configs, robustnessTreshold, filt)
 		
 	##
     # 

src/hpp/corbaserver/rbprm/tools/cwc_trajectory_helper.py

@@ -217,7 +217,8 @@ def saveAllData(fullBody, r, name):
 	saveToPinocchio(name)
 
 def play_traj(fullBody,pp,frame_rate):
-	return play_trajectory(fullBody,pp,frame_rate)
+	global trajec
+	return play_trajectory(fullBody,pp,trajec,frame_rate)
 
 #~ fullBody.exportAll(r, trajec, 'darpa_hyq_t_var_04f_andrea');
 #~ saveToPinocchio('darpa_hyq_t_var_04f_andrea')

src/rbprmbuilder.impl.cc

@@ -783,7 +783,7 @@ namespace hpp {
         return res;
     }
 
-    floatSeqSeq* RbprmBuilder::interpolateConfigs(const hpp::floatSeqSeq& configs, double robustnessTreshold) throw (hpp::Error)
+    floatSeqSeq* RbprmBuilder::interpolateConfigs(const hpp::floatSeqSeq& configs, double robustnessTreshold, unsigned short filterStates) throw (hpp::Error)
     {
         try
         {
@@ -803,7 +803,7 @@ namespace hpp {
                 throw hpp::Error ("No affordances found. Unable to interpolate.");
             }
             hpp::rbprm::interpolation::RbPrmInterpolationPtr_t interpolator = rbprm::interpolation::RbPrmInterpolation::create(fullBody_,startState_,endState_);
-            lastStatesComputedTime_ = interpolator->Interpolate(affMap, bindShooter_.affFilter_,configurations,robustnessTreshold);
+            lastStatesComputedTime_ = interpolator->Interpolate(affMap, bindShooter_.affFilter_,configurations,robustnessTreshold, filterStates != 0);
             lastStatesComputed_ = TimeStatesToStates(lastStatesComputedTime_);
             hpp::floatSeqSeq *res;
             res = new hpp::floatSeqSeq ();
@@ -1047,7 +1047,7 @@ namespace hpp {
         return res;
     }
 
-    floatSeqSeq* RbprmBuilder::interpolate(double timestep, double path, double robustnessTreshold) throw (hpp::Error)
+    floatSeqSeq* RbprmBuilder::interpolate(double timestep, double path, double robustnessTreshold, unsigned short filterStates) throw (hpp::Error)
     {
         try
         {
@@ -1075,7 +1075,7 @@ namespace hpp {
         hpp::rbprm::interpolation::RbPrmInterpolationPtr_t interpolator = 
 					rbprm::interpolation::RbPrmInterpolation::create(fullBody_,startState_,endState_,problemSolver_->paths()[pathId]);
         lastStatesComputedTime_ = interpolator->Interpolate(affMap, bindShooter_.affFilter_,
-					timestep,robustnessTreshold);
+                    timestep,robustnessTreshold, filterStates != 0);
 		lastStatesComputed_ = TimeStatesToStates(lastStatesComputedTime_);
 
         hpp::floatSeqSeq *res;

src/rbprmbuilder.impl.hh

@@ -144,8 +144,8 @@ namespace hpp {
         virtual void setStartState(const hpp::floatSeq& configuration, const hpp::Names_t& contactLimbs) throw (hpp::Error);
         virtual void setEndState(const hpp::floatSeq& configuration, const hpp::Names_t& contactLimbs) throw (hpp::Error);
         virtual hpp::floatSeqSeq* computeContactPoints(unsigned short cId) throw (hpp::Error);
-        virtual hpp::floatSeqSeq* interpolate(double timestep, double path, double robustnessTreshold) throw (hpp::Error);
-        virtual hpp::floatSeqSeq* interpolateConfigs(const hpp::floatSeqSeq& configs, double robustnessTreshold) throw (hpp::Error);
+        virtual hpp::floatSeqSeq* interpolate(double timestep, double path, double robustnessTreshold, unsigned short filterStates) throw (hpp::Error);
+        virtual hpp::floatSeqSeq* interpolateConfigs(const hpp::floatSeqSeq& configs, double robustnessTreshold, unsigned short filterStates) 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 generateRootPath(const hpp::floatSeqSeq& rootPositions,