Commit 385f8331 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

Interpolate now return Pairs (time in path/state) rather than just the states...

Interpolate now return Pairs (time in path/state) rather than just the states + added API to call state interpolation with user defined root path
parent 6ad820a8
...@@ -222,12 +222,28 @@ module hpp ...@@ -222,12 +222,28 @@ module hpp
floatSeqSeq interpolateConfigs(in floatSeqSeq configs, in double robustnessTreshold) raises (Error); floatSeqSeq interpolateConfigs(in floatSeqSeq configs, in double robustnessTreshold) raises (Error);
/// Provided a path has already been computed and interpolated, generate a continuous path /// 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 /// between two indicated states. The states do not need to be consecutive, but increasing in Id.
/// \param state1 /// 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 state2 index of second state.
/// \param numOptimizations Number of iterations of the shortcut algorithm to apply between each states /// \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); 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. /// 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. /// 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. /// \param filename name of the file used to save the contacts.
......
...@@ -252,6 +252,18 @@ class FullBody (object): ...@@ -252,6 +252,18 @@ class FullBody (object):
def interpolateBetweenStates(self, state1, state2, numOptim = 10): def interpolateBetweenStates(self, state1, state2, numOptim = 10):
return self.client.rbprm.rbprm.interpolateBetweenStates(state1, state2, numOptim) 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 ## Given start and goal states
......
...@@ -549,6 +549,16 @@ namespace hpp { ...@@ -549,6 +549,16 @@ namespace hpp {
} }
} }
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) floatSeqSeq* RbprmBuilder::interpolateConfigs(const hpp::floatSeqSeq& configs, double robustnessTreshold) throw (hpp::Error)
{ {
try try
...@@ -563,7 +573,8 @@ namespace hpp { ...@@ -563,7 +573,8 @@ namespace hpp {
} }
hpp::rbprm::interpolation::RbPrmInterpolationPtr_t interpolator = rbprm::interpolation::RbPrmInterpolation::create(fullBody_,startState_,endState_); hpp::rbprm::interpolation::RbPrmInterpolationPtr_t interpolator = rbprm::interpolation::RbPrmInterpolation::create(fullBody_,startState_,endState_);
std::vector<model::Configuration_t> configurations = doubleDofArrayToConfig(fullBody_->device_, configs); std::vector<model::Configuration_t> configurations = doubleDofArrayToConfig(fullBody_->device_, configs);
lastStatesComputed_ = interpolator->Interpolate(problemSolver_->collisionObstacles(),configurations,robustnessTreshold); lastStatesComputedTime_ = interpolator->Interpolate(problemSolver_->collisionObstacles(),configurations,robustnessTreshold);
lastStatesComputed_ = TimeStatesToStates(lastStatesComputedTime_);
hpp::floatSeqSeq *res; hpp::floatSeqSeq *res;
res = new hpp::floatSeqSeq (); res = new hpp::floatSeqSeq ();
...@@ -613,8 +624,8 @@ namespace hpp { ...@@ -613,8 +624,8 @@ namespace hpp {
} }
hpp::rbprm::interpolation::RbPrmInterpolationPtr_t interpolator = hpp::rbprm::interpolation::RbPrmInterpolation::create(fullBody_,startState_,endState_,problemSolver_->paths()[pathId]); hpp::rbprm::interpolation::RbPrmInterpolationPtr_t interpolator = hpp::rbprm::interpolation::RbPrmInterpolation::create(fullBody_,startState_,endState_,problemSolver_->paths()[pathId]);
lastStatesComputed_ = interpolator->Interpolate(problemSolver_->collisionObstacles(),timestep,robustnessTreshold); lastStatesComputedTime_ = interpolator->Interpolate(problemSolver_->collisionObstacles(),timestep,robustnessTreshold);
lastStatesComputed_ = TimeStatesToStates(lastStatesComputedTime_);
hpp::floatSeqSeq *res; hpp::floatSeqSeq *res;
res = new hpp::floatSeqSeq (); res = new hpp::floatSeqSeq ();
...@@ -666,6 +677,31 @@ namespace hpp { ...@@ -666,6 +677,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) void RbprmBuilder::saveComputedStates(const char* outfilename) throw (hpp::Error)
{ {
std::stringstream ss; std::stringstream ss;
......
...@@ -128,6 +128,7 @@ namespace hpp { ...@@ -128,6 +128,7 @@ namespace hpp {
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 interpolateBetweenStates(double state1, double state2, unsigned short numOptimizations) 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 saveComputedStates(const char* filepath) throw (hpp::Error);
virtual void saveLimbDatabase(const char* limbname,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); virtual hpp::floatSeq* getOctreeBox(const char* limbName, double sampleId) throw (hpp::Error);
...@@ -153,6 +154,7 @@ namespace hpp { ...@@ -153,6 +154,7 @@ namespace hpp {
rbprm::State startState_; rbprm::State startState_;
rbprm::State endState_; rbprm::State endState_;
std::vector<rbprm::State> lastStatesComputed_; std::vector<rbprm::State> lastStatesComputed_;
rbprm::T_StateFrame lastStatesComputedTime_;
sampling::AnalysisFactory* analysisFactory_; sampling::AnalysisFactory* analysisFactory_;
}; // class RobotBuilder }; // class RobotBuilder
} // namespace impl } // 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