diff --git a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
index 5e030399f8a50c00d12d16dac4aee060e3140127..8c3d0ed6219f060283abb391c181416cca6c0b3e 100755
--- a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
+++ b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
@@ -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.
diff --git a/src/hpp/corbaserver/rbprm/rbprmfullbody.py b/src/hpp/corbaserver/rbprm/rbprmfullbody.py
index a8525347d6db7bd5cd313dcfd514b7b6a253f9d1..58b0b13ac623bbfde644fe9f15e07729b2de0307 100755
--- a/src/hpp/corbaserver/rbprm/rbprmfullbody.py
+++ b/src/hpp/corbaserver/rbprm/rbprmfullbody.py
@@ -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
diff --git a/src/rbprmbuilder.impl.cc b/src/rbprmbuilder.impl.cc
index 0b551e5b9d483c1bd4799c16805d8d1e1c823256..e015bb9d12601c21fec4b42279d9cefa705e6781 100755
--- a/src/rbprmbuilder.impl.cc
+++ b/src/rbprmbuilder.impl.cc
@@ -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;
diff --git a/src/rbprmbuilder.impl.hh b/src/rbprmbuilder.impl.hh
index 1c82b7ea970f89b320b87fd8bb2c0166b6c179c3..62580fe7bd3f1adf0a1f1834abec285894d53ba3 100755
--- a/src/rbprmbuilder.impl.hh
+++ b/src/rbprmbuilder.impl.hh
@@ -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