diff --git a/CMakeLists.txt b/CMakeLists.txt
index e4180625fa8080f7a099a60a444c09ebf5c585f0..97aac335fe348802f018bcbf572511ee06cd61cc 100755
--- a/CMakeLists.txt
+++ b/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()
diff --git a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
index b18b16de9080888b475096845d40cedf4e531297..1e41bd166ba7f97fe9e7f9bba3d716e2b713ac86 100755
--- a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
+++ b/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
diff --git a/src/hpp/corbaserver/rbprm/rbprmfullbody.py b/src/hpp/corbaserver/rbprm/rbprmfullbody.py
index bdb72096a4192d160075a3dfec20455175b93acc..b50ec3b2a1c730756d0e104a7d5a3252ba0a33a9 100755
--- a/src/hpp/corbaserver/rbprm/rbprmfullbody.py
+++ b/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
diff --git a/src/rbprmbuilder.impl.cc b/src/rbprmbuilder.impl.cc
index 376b5c11b2266e923beb4b9d9315b67b9ec1607e..545e8f224fbcbf578aa12e38e4aa6fc45adc6122 100755
--- a/src/rbprmbuilder.impl.cc
+++ b/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
diff --git a/src/rbprmbuilder.impl.hh b/src/rbprmbuilder.impl.hh
index 5faf68ff6ea3b90d10c919ea46d230d6cddcec9f..87777145170bd658b1401af82888c67086cf6344 100755
--- a/src/rbprmbuilder.impl.hh
+++ b/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);