Skip to content
GitLab
Commit 556d4996 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

merging commit 385f8331 

Interpolate now return Pairs (time in path/state)
parent 8e90b376
Hide whitespace changes
Inline Side-by-side
idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
View file @ 556d4996
......@@ -220,12 +220,28 @@ module hpp
floatSeqSeq interpolateConfigs(in floatSeqSeq configs, in double robustnessTreshold) raises (Error);
/// Provided a path has already been computed and interpolated, generate a continuous path
/// between two indicated states. Will fail if the index of the states do not exist
/// \param state1
/// between two indicated states. The states do not need to be consecutive, but increasing in Id.
/// Will fail if the index of the states do not exist
/// The path of the root and limbs not considered by the contact transitions between
/// two states are computed using the current active steering method, and considered to be valid
/// in the sense of the active PathValidation.
/// \param state1 index of first state.
/// \param state2 index of second state.
/// \param numOptimizations Number of iterations of the shortcut algorithm to apply between each states
void interpolateBetweenStates(in double state1, in double state2, in unsigned short numOptimizations) raises (Error);
/// Provided a path has already been computed and interpolated, generate a continuous path
/// between two indicated states. The states do not need to be consecutive, but increasing in Id.
/// Will fail if the index of the states do not exist
/// The path of the root and limbs not considered by the contact transitions between
/// two states is assumed to be already computed, and registered in the solver under the id specified by the user.
/// It must be valid in the sense of the active PathValidation.
/// \param state1 index of first state.
/// \param state2 index of second state.
/// \param path index of the path considered for the generation
/// \param numOptimizations Number of iterations of the shortcut algorithm to apply between each states
void interpolateBetweenStatesFromPath(in double state1, in double state2, in unsigned short path, in unsigned short numOptimizations) raises (Error);
/// Saves the last computed states by the function interpolate in a filename.
/// Raises an error if interpolate has not been called, or the file could not be opened.
/// \param filename name of the file used to save the contacts.
......
src/hpp/corbaserver/rbprm/rbprmfullbody.py
View file @ 556d4996
......@@ -252,6 +252,18 @@ class FullBody (object):
def interpolateBetweenStates(self, state1, state2, numOptim = 10):
return self.client.rbprm.rbprm.interpolateBetweenStates(state1, state2, numOptim)
## Provided a path has already been computed and interpolated, generate a continuous path
# between two indicated states. The states do not need to be consecutive, but increasing in Id.
# Will fail if the index of the states do not exist
# The path of the root and limbs not considered by the contact transitions between
# two states is assumed to be already computed, and registered in the solver under the id specified by the user.
# It must be valid in the sense of the active PathValidation.
# \param state1 index of first state.
# \param state2 index of second state.
# \param path index of the path considered for the generation
# \param numOptimizations Number of iterations of the shortcut algorithm to apply between each states
def interpolateBetweenStatesFromPath(self, state1, state2, path, numOptim = 10):
return self.client.rbprm.rbprm.interpolateBetweenStatesFromPath(state1, state2, path, numOptim)
## Given start and goal states
......
src/rbprmbuilder.impl.cc
View file @ 556d4996
......@@ -553,8 +553,17 @@ namespace hpp {
}
}
floatSeqSeq* RbprmBuilder::interpolateConfigs(const hpp::floatSeqSeq& configs,
double robustnessTreshold) throw (hpp::Error)
std::vector<State> TimeStatesToStates(const T_StateFrame& ref)
{
std::vector<State> res;
for(CIT_StateFrame cit = ref.begin(); cit != ref.end(); ++cit)
{
res.push_back(cit->second);
}
return res;
}
floatSeqSeq* RbprmBuilder::interpolateConfigs(const hpp::floatSeqSeq& configs, double robustnessTreshold) throw (hpp::Error)
{
try
{
......@@ -566,25 +575,23 @@ namespace hpp {
{
throw std::runtime_error ("End state not initialized, can not interpolate ");
}
const affMap_t &affMap = problemSolver_->map
<std::vector<boost::shared_ptr<model::CollisionObject> > > ();
if (affMap.empty ()) {
throw hpp::Error ("No affordances found. Unable to interpolate.");
}
hpp::rbprm::interpolation::RbPrmInterpolationPtr_t interpolator =
rbprm::interpolation::RbPrmInterpolation::create(fullBody_,startState_,endState_);
std::vector<model::Configuration_t> configurations = doubleDofArrayToConfig(fullBody_->device_, configs);
lastStatesComputed_ = interpolator->Interpolate(affMap, bindShooter_.affFilter_,
configurations,robustnessTreshold);
const affMap_t &affMap = problemSolver_->map
<std::vector<boost::shared_ptr<model::CollisionObject> > > ();
if (affMap.empty ())
{
throw hpp::Error ("No affordances found. Unable to interpolate.");
}
hpp::rbprm::interpolation::RbPrmInterpolationPtr_t interpolator = rbprm::interpolation::RbPrmInterpolation::create(fullBody_,startState_,endState_);
lastStatesComputedTime_ = interpolator->Interpolate(affMap, bindShooter_.affFilter_,configurations,robustnessTreshold);
lastStatesComputed_ = TimeStatesToStates(lastStatesComputedTime_);
hpp::floatSeqSeq *res;
res = new hpp::floatSeqSeq ();
res->length ((_CORBA_ULong)lastStatesComputed_.size ());
std::size_t i=0;
std::size_t id = 0;
for(std::vector<State>::const_iterator cit = lastStatesComputed_.begin();
cit != lastStatesComputed_.end(); ++cit, ++id)
for(std::vector<State>::const_iterator cit = lastStatesComputed_.begin(); cit != lastStatesComputed_.end(); ++cit, ++id)
{
std::cout << "ID " << id;
cit->print();
......@@ -620,20 +627,23 @@ namespace hpp {
{
throw std::runtime_error ("End state not initialized, cannot interpolate ");
}
if(problemSolver_->paths().size() <= pathId)
{
throw std::runtime_error ("No path computed, cannot interpolate ");
}
const affMap_t &affMap = problemSolver_->map
const affMap_t &affMap = problemSolver_->map
<std::vector<boost::shared_ptr<model::CollisionObject> > > ();
if (affMap.empty ()) {
throw hpp::Error ("No affordances found. Unable to interpolate.");
}
if (affMap.empty ())
{
throw hpp::Error ("No affordances found. Unable to interpolate.");
}
hpp::rbprm::interpolation::RbPrmInterpolationPtr_t interpolator =
rbprm::interpolation::RbPrmInterpolation::create(fullBody_,startState_,endState_,problemSolver_->paths()[pathId]);
lastStatesComputed_ = interpolator->Interpolate(affMap, bindShooter_.affFilter_,
lastStatesComputedTime_ = interpolator->Interpolate(affMap, bindShooter_.affFilter_,
timestep,robustnessTreshold);
lastStatesComputed_ = TimeStatesToStates(lastStatesComputedTime_);
hpp::floatSeqSeq *res;
res = new hpp::floatSeqSeq ();
......@@ -675,6 +685,7 @@ namespace hpp {
throw std::runtime_error ("did not find a states at indicated indices: " + std::string(""+s1) + ", " + std::string(""+s2));
}
//create helper
// /interpolation::LimbRRTHelper helper(fullBody_, problemSolver_->problem());
core::PathVectorPtr_t path = interpolation::interpolateStates(fullBody_,problemSolver_->problem(),
lastStatesComputed_.begin()+s1,lastStatesComputed_.begin()+s2, numOptimizations);
problemSolver_->addPath(path);
......@@ -686,6 +697,31 @@ namespace hpp {
}
}
void RbprmBuilder::interpolateBetweenStatesFromPath(double state1, double state2, unsigned short path, unsigned short numOptimizations) throw (hpp::Error)
{
try
{
std::size_t s1((std::size_t)state1), s2((std::size_t)state2);
if(lastStatesComputed_.size () < s1 || lastStatesComputed_.size () < s2 )
{
throw std::runtime_error ("did not find a states at indicated indices: " + std::string(""+s1) + ", " + std::string(""+s2));
}
unsigned int pathId = (unsigned int)(path);
if(problemSolver_->paths().size() <= pathId)
{
throw std::runtime_error ("No path computed, cannot interpolate ");
}
core::PathVectorPtr_t path = interpolation::interpolateStates(fullBody_,problemSolver_->problem(), problemSolver_->paths()[pathId],
lastStatesComputedTime_.begin()+s1,lastStatesComputedTime_.begin()+s2, numOptimizations);
problemSolver_->addPath(path);
problemSolver_->robot()->setDimensionExtraConfigSpace(problemSolver_->robot()->extraConfigSpace().dimension()+1);
}
catch(std::runtime_error& e)
{
throw Error(e.what());
}
}
void RbprmBuilder::saveComputedStates(const char* outfilename) throw (hpp::Error)
{
std::stringstream ss;
......
src/rbprmbuilder.impl.hh
View file @ 556d4996
......@@ -142,6 +142,7 @@ namespace hpp {
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 interpolateBetweenStates(double state1, double state2, unsigned short numOptimizations) throw (hpp::Error);
virtual void interpolateBetweenStatesFromPath(double state1, double state2, unsigned short path, unsigned short numOptimizations) 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::floatSeq* getOctreeBox(const char* limbName, double sampleId) throw (hpp::Error);
......@@ -167,6 +168,7 @@ namespace hpp {
rbprm::State startState_;
rbprm::State endState_;
std::vector<rbprm::State> lastStatesComputed_;
rbprm::T_StateFrame lastStatesComputedTime_;
sampling::AnalysisFactory* analysisFactory_;
}; // class RobotBuilder
} // namespace impl
......
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