Commit 33a52da1 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

functional sample database

parent 9dc877aa
......@@ -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()
......@@ -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
......
......@@ -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
......
......@@ -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
......
......@@ -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);
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment