added api to plot sample data

idl/hpp/corbaserver/rbprm/rbprmbuilder.idl

@@ -128,6 +128,18 @@ module hpp
     /// and the selected sample
     floatSeq getSamplePosition(in string sampleName, in unsigned short sampleId) raises (Error);
 
+    /// Get the end effector position of a given limb configuration
+    /// \param limbName name of the limb from which to retrieve a sample
+    /// \return number of samples generated for the limb
+    unsigned short getNumSamples(in string limbName) raises (Error);
+
+    /// Get the sample value for a given analysis
+    /// \param limbName name of the limb from which to retrieve a sample
+    /// \param valueName name of the analytic measure desired
+    /// \param sampleId id of the considered sample
+    /// \return the value computed for the given sample and analytics
+    double getSampleValue(in string limbName, in string valueName, in unsigned short sampleId) raises (Error);
+
     /// Generate all possible contact in a given configuration
     /// \param dofArray initial configuration of the robot
     /// \param direction desired direction of motion for the robot
@@ -227,13 +239,15 @@ module hpp
     short isConfigBalanced(in floatSeq config, in Names_t contacts,  in double robustnessTreshold) raises (Error);
 
     /// run and store an analysis on all limb databases
-    /// \param analysis name of the analysis existing
+    /// \param analysis name of the analysis existing if analysis ="all",
+    /// all tests are run.
     /// \param isstatic 1 is becomes new static value of database, 0 otherwise
     void runSampleAnalysis(in string analysis, in double isstatic) raises (Error);
 
     /// run and store an analysis on a limb database
     /// \param limbname name of the limb to perform the analysis to
-    /// \param analysis name of the analysis existing
+    /// \param analysis name of the analysis existing if analysis ="all",
+    /// all tests are run.
     /// \param isstatic 1 is becomes new static value of database, 0 otherwise
     void runLimbSampleAnalysis(in string limbname, in string analysis, in double isstatic) raises (Error);
 

src/CMakeLists.txt

@@ -120,10 +120,16 @@ INSTALL(
 	${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/client.py
 	${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/rbprmbuilder.py
         ${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/rbprmfullbody.py
-	${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/problem_solver.py
+        ${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/problem_solver.py
   DESTINATION ${PYTHON_SITELIB}/hpp/corbaserver/rbprm
   )
-
+INSTALL(
+  FILES
+        ${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/tools/__init__.py
+        ${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/tools//generateROMs.py
+        ${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/tools/plot_analytics.py
+  DESTINATION ${PYTHON_SITELIB}/hpp/corbaserver/rbprm/tools
+  )
 # Stand alone corba server
 ADD_EXECUTABLE (hpp-rbprm-server hpp-rbprm-corba.cc)
 TARGET_LINK_LIBRARIES (hpp-rbprm-server ${LIBRARY_NAME} hpp-rbprm)

src/hpp/corbaserver/rbprm/rbprmfullbody.py

@@ -154,7 +154,21 @@ class FullBody (object):
     # \param direction a 3d vector specifying the desired direction of motion
     def getContactSamplesIds(self, name, configuration, direction):
 		return self.client.rbprm.rbprm.getContactSamplesIds(name, configuration, direction)
-	
+		
+	## Get the number of samples generated for a limb
+	#
+    # \param limbName name of the limb from which to retrieve a sample
+    def getNumSamples(self, limbName):
+		return self.client.rbprm.rbprm.getNumSamples(limbName)
+		
+	## Get the sample value for a given analysis
+	#
+    # \param limbName name of the limb from which to retrieve a sample
+    # \param valueName name of the analytic measure desired
+    # \param sampleId id of the considered sample
+    def getSampleValue(self, limbName, valueName, sampleId):
+		return self.client.rbprm.rbprm.getSampleValue(limbName, valueName, sampleId)
+		
 	## Initialize the first configuration of the path discretization 
 	# with a balanced configuration for the interpolation problem;
 	#
@@ -162,7 +176,7 @@ class FullBody (object):
     # \param contacts the array of limbs in contact
     def setStartState(self, configuration, contacts):
 		return self.client.rbprm.rbprm.setStartState(configuration, contacts)
-		
+			
 	## Initialize the last configuration of the path discretization 
 	# with a balanced configuration for the interpolation problem;
 	#

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

+

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

+import numpy as np
+from mpl_toolkits.mplot3d import Axes3D
+import matplotlib.pyplot as plt
+from matplotlib import cm
+#~ 
+#~ def randrange(n, vmin, vmax):
+    #~ return (vmax - vmin)*np.random.rand(n) + vmin
+#~ 
+#~ fig = plt.figure()
+#~ ax = fig.add_subplot(111, projection='3d')
+#~ n = 100
+#~ for c, m, zl, zh in [('r', 'o', -50, -25), ('b', '^', -30, -5)]:
+    #~ xs = randrange(n, 23, 32)
+    #~ ys = randrange(n, 0, 100)
+    #~ zs = randrange(n, zl, zh)
+    #~ ax.scatter(xs, ys, zs, c=c, marker=m)
+#~ 
+#~ ax.set_xlabel('X Label')
+#~ ax.set_ylabel('Y Label')
+#~ ax.set_zlabel('Z Label')
+#~ 
+#~ plt.show()
+
+## Display a 3d plot of the values computed for a limb database
+#
+# \param robot FullBody object
+# \param valueName name of the plotted analytics
+# \param limb name of the considered limb
+def plotValues(robot, valueName, limb):
+	xs = []
+	ys = []
+	zs = []
+	vals = []
+	fig = plt.figure()
+	ax = fig.add_subplot(111, projection='3d')
+	numSamples = robot.getNumSamples(limb)	
+	for i in range(0,numSamples):
+		pos = robot.getSamplePosition(limb, i)
+		xs.append(pos[0])
+		ys.append(pos[1])
+		zs.append(pos[2])
+		g = robot.getSampleValue(limb, valueName, i)
+		print g
+		vals.append([1-g,g,0])
+		#~ ax.scatter(pos[0], pos[1], pos[2], c=robot.getSampleValue(limb, valueName, i))
+	ax.scatter(xs, ys, zs, c=vals)
+	ax.set_xlabel('X Label')
+	ax.set_ylabel('Y Label')
+	ax.set_zlabel('Z Label')
+	plt.show()

src/rbprmbuilder.impl.cc

@@ -37,6 +37,7 @@ namespace hpp {
     , romLoaded_(false)
     , fullBodyLoaded_(false)
     , bindShooter_()
+    , analysisFactory_(0)
     {
         // NOTHING
     }
@@ -129,7 +130,9 @@ namespace hpp {
             hppDout (error, exc.what ());
             throw hpp::Error (exc.what ());
         }
-        fullBodyLoaded_ = true;
+        fullBodyLoaded_ = true;        
+        if(!analysisFactory_)
+            analysisFactory_ = new sampling::AnalysisFactory(fullBody_);
     }
 
     hpp::floatSeq* RbprmBuilder::getSampleConfig(const char* limb, unsigned short sampleId) throw (hpp::Error)
@@ -188,6 +191,43 @@ namespace hpp {
         return dofArray;
     }
 
+
+    CORBA::UShort RbprmBuilder::getNumSamples(const char* limb) throw (hpp::Error)
+    {
+        const T_Limb& limbs = fullBody_->GetLimbs();
+        T_Limb::const_iterator lit = limbs.find(std::string(limb));
+        if(lit == limbs.end())
+        {
+            std::string err("No limb " + std::string(limb) + "was defined for robot" + fullBody_->device_->name());
+            throw Error (err.c_str());
+        }
+        return lit->second->sampleContainer_.samples_.size();
+    }
+
+    double RbprmBuilder::getSampleValue(const char* limb, const char* valueName, unsigned short sampleId) throw (hpp::Error)
+    {
+        const T_Limb& limbs = fullBody_->GetLimbs();
+        T_Limb::const_iterator lit = limbs.find(std::string(limb));
+        if(lit == limbs.end())
+        {
+            std::string err("No limb " + std::string(limb) + "was defined for robot" + fullBody_->device_->name());
+            throw Error (err.c_str());
+        }
+        const sampling::SampleDB& database = lit->second->sampleContainer_;
+        if (database.samples_.size() <= sampleId)
+        {
+            std::string err("unexisting sample id " + sampleId);
+            throw Error (err.c_str());
+        }
+        sampling::T_Values::const_iterator cit = database.values_.find(std::string(valueName));
+        if(cit == database.values_.end())
+        {
+            std::string err("value not existing in database " + std::string(valueName));
+            throw Error (err.c_str());
+        }
+        return cit->second[sampleId];
+    }
+
     model::Configuration_t dofArrayToConfig (const model::DevicePtr_t& robot,
       const hpp::floatSeq& dofArray)
     {
@@ -700,17 +740,34 @@ namespace hpp {
     {
         try
         {
+        if(!fullBodyLoaded_)
+            throw Error ("No full body robot was loaded");
         std::string eval(analysis);
-        sampling::T_evaluate::const_iterator analysisit = analysisFactory_.evaluate_.find(std::string(eval));
-        if(analysisit == analysisFactory_.evaluate_.end())
+        if (eval == "all")
         {
-            std::string err("No analysis named  " + eval + "was defined for analyzing database sample");
-            throw Error (err.c_str());
+            for(sampling::T_evaluate::const_iterator analysisit = analysisFactory_->evaluate_.begin();
+                analysisit != analysisFactory_->evaluate_.end(); ++ analysisit)
+            {
+                for(T_Limb::const_iterator cit = fullBody_->GetLimbs().begin(); cit !=fullBody_->GetLimbs().end();++cit)
+                {
+                    sampling::SampleDB & sampleDB =const_cast<sampling::SampleDB &> (cit->second->sampleContainer_);
+                    sampling::addValue(sampleDB, analysisit->first, analysisit->second, isstatic > 0.5, isstatic > 0.5);
+                }
+            }
         }
-        for(T_Limb::const_iterator cit = fullBody_->GetLimbs().begin(); cit !=fullBody_->GetLimbs().end();++cit)
+        else
         {
-            sampling::SampleDB & sampleDB =const_cast<sampling::SampleDB &> (cit->second->sampleContainer_);
-            sampling::addValue(sampleDB, analysisit->first, analysisit->second, isstatic > 0.5, isstatic > 0.5);
+            sampling::T_evaluate::const_iterator analysisit = analysisFactory_->evaluate_.find(std::string(eval));
+            if(analysisit == analysisFactory_->evaluate_.end())
+            {
+                std::string err("No analysis named  " + eval + "was defined for analyzing database sample");
+                throw Error (err.c_str());
+            }
+            for(T_Limb::const_iterator cit = fullBody_->GetLimbs().begin(); cit !=fullBody_->GetLimbs().end();++cit)
+            {
+                sampling::SampleDB & sampleDB =const_cast<sampling::SampleDB &> (cit->second->sampleContainer_);
+                sampling::addValue(sampleDB, analysisit->first, analysisit->second, isstatic > 0.5, isstatic > 0.5);
+            }
         }
         }
         catch(std::runtime_error& e)
@@ -723,21 +780,35 @@ namespace hpp {
     {
         try
         {
-        std::string eval(analysis);
-        sampling::T_evaluate::const_iterator analysisit = analysisFactory_.evaluate_.find(std::string(eval));
-        if(analysisit == analysisFactory_.evaluate_.end())
-        {
-            std::string err("No analysis named  " + eval + "was defined for analyzing database sample");
-            throw Error (err.c_str());
-        }
+        if(!fullBodyLoaded_)
+            throw Error ("No full body robot was loaded");
         T_Limb::const_iterator lit = fullBody_->GetLimbs().find(std::string(limbname));
         if(lit == fullBody_->GetLimbs().end())
         {
             std::string err("No limb " + std::string(limbname) + "was defined for robot" + fullBody_->device_->name());
             throw Error (err.c_str());
         }
-        sampling::SampleDB & sampleDB =const_cast<sampling::SampleDB &> (lit->second->sampleContainer_);
-        sampling::addValue(sampleDB, analysisit->first, analysisit->second, isstatic > 0.5, isstatic > 0.5);
+        std::string eval(analysis);
+        if (eval == "all")
+        {
+            for(sampling::T_evaluate::const_iterator analysisit = analysisFactory_->evaluate_.begin();
+                analysisit != analysisFactory_->evaluate_.end(); ++ analysisit)
+            {
+                sampling::SampleDB & sampleDB =const_cast<sampling::SampleDB &> (lit->second->sampleContainer_);
+                sampling::addValue(sampleDB, analysisit->first, analysisit->second, isstatic > 0.5, isstatic > 0.5);
+            }
+        }
+        else
+        {
+            sampling::T_evaluate::const_iterator analysisit = analysisFactory_->evaluate_.find(std::string(eval));
+            if(analysisit == analysisFactory_->evaluate_.end())
+            {
+                std::string err("No analysis named  " + eval + "was defined for analyzing database sample");
+                throw Error (err.c_str());
+            }
+            sampling::SampleDB & sampleDB =const_cast<sampling::SampleDB &> (lit->second->sampleContainer_);
+            sampling::addValue(sampleDB, analysisit->first, analysisit->second, isstatic > 0.5, isstatic > 0.5);
+            }
         }
         catch(std::runtime_error& e)
         {

src/rbprmbuilder.impl.hh

@@ -103,6 +103,8 @@ namespace hpp {
 
         virtual hpp::floatSeq* getSampleConfig(const char* limb, unsigned short sampleId) throw (hpp::Error);
         virtual hpp::floatSeq* getSamplePosition(const char* limb, unsigned short sampleId) throw (hpp::Error);
+        virtual CORBA::UShort getNumSamples(const char* limb) throw (hpp::Error);
+        virtual double getSampleValue(const char* limb, const char* valueName, unsigned short sampleId) throw (hpp::Error);
 
         virtual hpp::floatSeq* generateContacts(const hpp::floatSeq& configuration,
                                                 const hpp::floatSeq& direction) throw (hpp::Error);
@@ -143,7 +145,7 @@ namespace hpp {
         rbprm::State startState_;
         rbprm::State endState_;
         std::vector<rbprm::State> lastStatesComputed_;
-        sampling::AnalysisFactory analysisFactory_;
+        sampling::AnalysisFactory* analysisFactory_;
       }; // class RobotBuilder
     } // namespace impl
   } // namespace manipulation