functional sample database

CMakeLists.txt

@@ -166,10 +166,7 @@ install(FILES
 	data/meshes/hyq/hyq_lfleg_rom.stl
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/meshes/hyq
   )
-  
-install(DIRECTORY data/hyq_description
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/../
-  )
+
   
 SETUP_PROJECT_FINALIZE()
 

idl/hpp/corbaserver/rbprm/rbprmbuilder.idl

@@ -160,6 +160,13 @@ module hpp
                  in double x, in double y, in unsigned short samples, in string heuristicName,
                  in double resolution, in string contactType) raises (Error);
 
+    /// Specifies a subchain of the robot as a limb, which can be used to create contacts.
+    /// A limb must consist in a simple kinematic chain, where every node has only one child
+    /// \param databasepath filepath to the database
+    /// \param id user given name of the new limb
+    /// \param heuristicName heuristic used to bias sample selection
+    void addLimbDatabase(in string databasepath, in string id, in string heuristicName) raises (Error);
+
     /// Set the start state of a contact generation problem
     /// environment, ordered by their efficiency
     /// \param dofArray start configuration of the robot
@@ -193,6 +200,12 @@ module hpp
     /// \param filename name of the file used to save the contacts.
     void saveComputedStates(in string filename) raises (Error);
 
+    /// Saves a sample database into a file
+    /// Raises an ifthe file could not be opened.
+    /// \param limbname name of the limb used to save the samples.
+    /// \param filename name of the file used to save the samples.
+    void saveLimbDatabase(in string limbname, in string filename) raises (Error);
+
     /// returns the size and transforms of all boxes of the octree for a limb
     /// \param limbname name of the considered limb
     /// \param dofArray considered configuration of the robot

src/hpp/corbaserver/rbprm/rbprmfullbody.py

@@ -94,6 +94,15 @@ class FullBody (object):
     def addLimb(self, limbId, name, effectorname, offset, normal, x, y, samples):
 		self.client.rbprm.rbprm.addLimb(limbId, name, effectorname, offset, normal, x, y, samples, "EFORT", 0.03)
 
+	## Add a limb to the model
+	#
+	# \param databasepath: path to the database describing the robot
+	# \param limbId: user defined id for the limb. Must be unique.
+    #  The id is used if several contact points are defined for the same limb (ex: the knee and the foot)
+    # \param heuristicName: name of the selected heuristic for configuration evaluation
+    def addLimbDatabase(self, databasepath, limbId, heuristicName):
+		self.client.rbprm.rbprm.addLimbDatabase(databasepath, limbId, heuristicName)		
+
 	## Add a limb to the model
 	#
 	# \param id: user defined id for the limb. Must be unique.
@@ -168,6 +177,13 @@ class FullBody (object):
     def saveComputedStates(self, filename):
 		return self.client.rbprm.rbprm.saveComputedStates(filename)
 	
+	## Saves a limb database
+	#
+    # \param limb name of the limb for which the DB must be saved
+    # \param The file where the configuration must be saved
+    def saveLimbDatabase(self, limbName, filename):
+		return self.client.rbprm.rbprm.saveLimbDatabase(limbName, filename)
+	
 	## Discretizes a path return by a motion planner into a discrete
 	# sequence of balanced, contact configurations and returns
 	# the sequence as an array of configurations
@@ -199,7 +215,6 @@ class FullBody (object):
 		
 	## Create octree nodes representation for a given limb
 	#
-    # \param stepSize discretization step
     # \param gui gepetoo viewer instance discretization step
     # \param winId window to draw to step
     # \param limbName name of the limb considered

src/rbprmbuilder.impl.cc

@@ -22,6 +22,7 @@
 #include "hpp/rbprm/rbprm-validation.hh"
 #include "hpp/rbprm/rbprm-path-interpolation.hh"
 #include "hpp/rbprm/stability/stability.hh"
+#include "hpp/rbprm/sampling/sample-db.hh"
 #include "hpp/model/urdf/util.hh"
 #include <fstream>
 
@@ -331,7 +332,7 @@ namespace hpp {
             for(model::ObjectVector_t::const_iterator oit = problemSolver_->collisionObstacles().begin();
                 oit != problemSolver_->collisionObstacles().end(); ++oit, ++i)
             {
-                sampling::GetCandidates(limb->sampleContainer_, transform, transform, *oit, dir, reports[i]);
+                sampling::GetCandidates(limb->sampleContainer_, transform, *oit, dir, reports[i]);
             }
             for(std::vector<sampling::T_OctreeReport>::const_iterator cit = reports.begin();
                 cit != reports.end(); ++cit)
@@ -378,6 +379,22 @@ namespace hpp {
         }
     }
 
+
+    void RbprmBuilder::addLimbDatabase(const char* databasePath, const char* id, const char* heuristicName) throw (hpp::Error)
+    {
+        if(!fullBodyLoaded_)
+            throw Error ("No full body robot was loaded");
+        try
+        {
+            std::string fileName(databasePath);
+            fullBody_->AddLimb(fileName, std::string(id), problemSolver_->collisionObstacles(), heuristicName);
+        }
+        catch(std::runtime_error& e)
+        {
+            throw Error(e.what());
+        }
+    }
+
     void SetPositionAndNormal(rbprm::State& state, hpp::rbprm::RbPrmFullBodyPtr_t fullBody, const hpp::floatSeq& configuration, const hpp::Names_t& contactLimbs)
     {
         core::Configuration_t old = fullBody->device_->currentConfiguration();
@@ -557,6 +574,23 @@ namespace hpp {
         }
     }
 
+    void RbprmBuilder::saveLimbDatabase(const char* limbname,const char* filepath) throw (hpp::Error)
+    {
+        try
+        {
+        std::string limbName(limbname);
+        std::ofstream fout;
+        fout.open(filepath, std::fstream::out | std::fstream::app);
+        rbprm::saveLimbInfoAndDatabase(fullBody_->GetLimbs().at(limbName),fout);
+        //sampling::saveLimbDatabase(fullBody_->GetLimbs().at(limbName)->sampleContainer_,fout);
+        fout.close();
+        }
+        catch(std::runtime_error& e)
+        {
+            throw Error(e.what());
+        }
+    }
+
     hpp::floatSeqSeq* RbprmBuilder::GetOctreeBoxes(const char* limbName, const hpp::floatSeq& configuration) throw (hpp::Error)
     {
         try

src/rbprmbuilder.impl.hh

@@ -112,12 +112,14 @@ namespace hpp {
 
         virtual void addLimb(const char* id, const char* limb, const char* effector, const hpp::floatSeq& offset, const hpp::floatSeq& normal, double x, double y,
                              unsigned short samples, const char *heuristicName, double resolution, const char *contactType) throw (hpp::Error);
+        virtual void addLimbDatabase(const char* databasePath, const char* id, const char* heuristicName) throw (hpp::Error);
 
         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* interpolate(double timestep, double path, double robustnessTreshold) throw (hpp::Error);
         virtual hpp::floatSeqSeq* interpolateConfigs(const hpp::floatSeqSeq& configs, double robustnessTreshold) throw (hpp::Error);
         virtual void saveComputedStates(const char* filepath) throw (hpp::Error);
+        virtual void saveLimbDatabase(const char* limbname,const char* filepath) 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) throw (hpp::Error);