Skip to content
Snippets Groups Projects
Commit 33a52da1 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

functional sample database

parent 9dc877aa
No related branches found
No related tags found
No related merge requests found
...@@ -166,10 +166,7 @@ install(FILES ...@@ -166,10 +166,7 @@ install(FILES
data/meshes/hyq/hyq_lfleg_rom.stl data/meshes/hyq/hyq_lfleg_rom.stl
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/meshes/hyq DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/meshes/hyq
) )
install(DIRECTORY data/hyq_description
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/../
)
SETUP_PROJECT_FINALIZE() SETUP_PROJECT_FINALIZE()
...@@ -160,6 +160,13 @@ module hpp ...@@ -160,6 +160,13 @@ module hpp
in double x, in double y, in unsigned short samples, in string heuristicName, in double x, in double y, in unsigned short samples, in string heuristicName,
in double resolution, in string contactType) raises (Error); 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 /// Set the start state of a contact generation problem
/// environment, ordered by their efficiency /// environment, ordered by their efficiency
/// \param dofArray start configuration of the robot /// \param dofArray start configuration of the robot
...@@ -193,6 +200,12 @@ module hpp ...@@ -193,6 +200,12 @@ module hpp
/// \param filename name of the file used to save the contacts. /// \param filename name of the file used to save the contacts.
void saveComputedStates(in string filename) raises (Error); 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 /// returns the size and transforms of all boxes of the octree for a limb
/// \param limbname name of the considered limb /// \param limbname name of the considered limb
/// \param dofArray considered configuration of the robot /// \param dofArray considered configuration of the robot
......
...@@ -94,6 +94,15 @@ class FullBody (object): ...@@ -94,6 +94,15 @@ class FullBody (object):
def addLimb(self, limbId, name, effectorname, offset, normal, x, y, samples): 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) 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 ## Add a limb to the model
# #
# \param id: user defined id for the limb. Must be unique. # \param id: user defined id for the limb. Must be unique.
...@@ -168,6 +177,13 @@ class FullBody (object): ...@@ -168,6 +177,13 @@ class FullBody (object):
def saveComputedStates(self, filename): def saveComputedStates(self, filename):
return self.client.rbprm.rbprm.saveComputedStates(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 ## Discretizes a path return by a motion planner into a discrete
# sequence of balanced, contact configurations and returns # sequence of balanced, contact configurations and returns
# the sequence as an array of configurations # the sequence as an array of configurations
...@@ -199,7 +215,6 @@ class FullBody (object): ...@@ -199,7 +215,6 @@ class FullBody (object):
## Create octree nodes representation for a given limb ## Create octree nodes representation for a given limb
# #
# \param stepSize discretization step
# \param gui gepetoo viewer instance discretization step # \param gui gepetoo viewer instance discretization step
# \param winId window to draw to step # \param winId window to draw to step
# \param limbName name of the limb considered # \param limbName name of the limb considered
......
...@@ -22,6 +22,7 @@ ...@@ -22,6 +22,7 @@
#include "hpp/rbprm/rbprm-validation.hh" #include "hpp/rbprm/rbprm-validation.hh"
#include "hpp/rbprm/rbprm-path-interpolation.hh" #include "hpp/rbprm/rbprm-path-interpolation.hh"
#include "hpp/rbprm/stability/stability.hh" #include "hpp/rbprm/stability/stability.hh"
#include "hpp/rbprm/sampling/sample-db.hh"
#include "hpp/model/urdf/util.hh" #include "hpp/model/urdf/util.hh"
#include <fstream> #include <fstream>
...@@ -331,7 +332,7 @@ namespace hpp { ...@@ -331,7 +332,7 @@ namespace hpp {
for(model::ObjectVector_t::const_iterator oit = problemSolver_->collisionObstacles().begin(); for(model::ObjectVector_t::const_iterator oit = problemSolver_->collisionObstacles().begin();
oit != problemSolver_->collisionObstacles().end(); ++oit, ++i) 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(); for(std::vector<sampling::T_OctreeReport>::const_iterator cit = reports.begin();
cit != reports.end(); ++cit) cit != reports.end(); ++cit)
...@@ -378,6 +379,22 @@ namespace hpp { ...@@ -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) 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(); core::Configuration_t old = fullBody->device_->currentConfiguration();
...@@ -557,6 +574,23 @@ namespace hpp { ...@@ -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) hpp::floatSeqSeq* RbprmBuilder::GetOctreeBoxes(const char* limbName, const hpp::floatSeq& configuration) throw (hpp::Error)
{ {
try try
......
...@@ -112,12 +112,14 @@ namespace hpp { ...@@ -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, 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); 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 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 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* 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* interpolateConfigs(const hpp::floatSeqSeq& configs, double robustnessTreshold) throw (hpp::Error);
virtual void saveComputedStates(const char* filepath) 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::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 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); virtual CORBA::Short isConfigBalanced(const hpp::floatSeq& config, const hpp::Names_t& contactLimbs, double robustnessTreshold) throw (hpp::Error);
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment