Commit e71e80ed authored by Steve Tonneau's avatar Steve Tonneau
Browse files

first working version, poor handling of extra dof in cloned device though

parent 1827454d
...@@ -218,6 +218,12 @@ module hpp ...@@ -218,6 +218,12 @@ module hpp
/// \param robustnessTreshold minimum value of the static equilibrium robustness criterion required to accept the configuration (0 by default). /// \param robustnessTreshold minimum value of the static equilibrium robustness criterion required to accept the configuration (0 by default).
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
/// between two indicated states. Will fail if the index of the states do not exist
/// \param state1
/// \param state2 index of second state.
void interpolateBetweenStates(in double state1, in double state2) 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.
......
...@@ -234,6 +234,17 @@ class FullBody (object): ...@@ -234,6 +234,17 @@ class FullBody (object):
def interpolateConfigs(self, configs): def interpolateConfigs(self, configs):
return self.client.rbprm.rbprm.interpolateConfigs(configs) return self.client.rbprm.rbprm.interpolateConfigs(configs)
## Given start and goal states
# 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 index of first state.
# \param index of second state.
def interpolateBetweenStates(self, state1, state2):
return self.client.rbprm.rbprm.interpolateBetweenStates(state1, state2)
## Given start and goal states ## Given start and goal states
# generate a contact sequence over a list of configurations # generate a contact sequence over a list of configurations
# #
......
...@@ -20,7 +20,8 @@ ...@@ -20,7 +20,8 @@
#include "rbprmbuilder.impl.hh" #include "rbprmbuilder.impl.hh"
#include "hpp/rbprm/rbprm-device.hh" #include "hpp/rbprm/rbprm-device.hh"
#include "hpp/rbprm/rbprm-validation.hh" #include "hpp/rbprm/rbprm-validation.hh"
#include "hpp/rbprm/rbprm-path-interpolation.hh" #include "hpp/rbprm/interpolation/rbprm-path-interpolation.hh"
#include "hpp/rbprm/interpolation/limb-rrt-helper.hh"
#include "hpp/rbprm/stability/stability.hh" #include "hpp/rbprm/stability/stability.hh"
#include "hpp/rbprm/sampling/sample-db.hh" #include "hpp/rbprm/sampling/sample-db.hh"
#include "hpp/model/urdf/util.hh" #include "hpp/model/urdf/util.hh"
...@@ -52,6 +53,7 @@ namespace hpp { ...@@ -52,6 +53,7 @@ namespace hpp {
try try
{ {
hpp::model::DevicePtr_t romDevice = model::Device::create (robotName); hpp::model::DevicePtr_t romDevice = model::Device::create (robotName);
std::cout << romDevice->currentConfiguration() << std::endl;
romDevices_.insert(std::make_pair(robotName, romDevice)); romDevices_.insert(std::make_pair(robotName, romDevice));
hpp::model::urdf::loadRobotModel (romDevice, hpp::model::urdf::loadRobotModel (romDevice,
std::string (rootJointType), std::string (rootJointType),
...@@ -558,7 +560,7 @@ namespace hpp { ...@@ -558,7 +560,7 @@ namespace hpp {
{ {
throw std::runtime_error ("End state not initialized, can not interpolate "); throw std::runtime_error ("End state not initialized, can not interpolate ");
} }
hpp::rbprm::RbPrmInterpolationPtr_t interpolator = rbprm::RbPrmInterpolation::create(fullBody_,startState_,endState_); hpp::rbprm::interpolation::RbPrmInterpolationPtr_t interpolator = hpp::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); lastStatesComputed_ = interpolator->Interpolate(problemSolver_->collisionObstacles(),configurations,robustnessTreshold);
hpp::floatSeqSeq *res; hpp::floatSeqSeq *res;
...@@ -609,7 +611,7 @@ namespace hpp { ...@@ -609,7 +611,7 @@ namespace hpp {
throw std::runtime_error ("No path computed, cannot interpolate "); throw std::runtime_error ("No path computed, cannot interpolate ");
} }
hpp::rbprm::RbPrmInterpolationPtr_t interpolator = rbprm::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); lastStatesComputed_ = interpolator->Interpolate(problemSolver_->collisionObstacles(),timestep,robustnessTreshold);
hpp::floatSeqSeq *res; hpp::floatSeqSeq *res;
...@@ -641,6 +643,27 @@ namespace hpp { ...@@ -641,6 +643,27 @@ namespace hpp {
} }
} }
void RbprmBuilder::interpolateBetweenStates(double state1, double state2) throw (hpp::Error)
{
try
{
std::size_t s1(state1), s2(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));
}
//create helper
interpolation::LimbRRTHelper helper(fullBody_, problemSolver_->problem());
core::PathVectorPtr_t path = interpolation::interpolateStates(helper,lastStatesComputed_[s1],lastStatesComputed_[s2]);
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;
......
...@@ -125,6 +125,7 @@ namespace hpp { ...@@ -125,6 +125,7 @@ namespace hpp {
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 interpolateBetweenStates(double state1, double state2) 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);
......
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