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
/// \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);
/// 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.
/// 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.
......
......@@ -234,6 +234,17 @@ class FullBody (object):
def interpolateConfigs(self, 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
# generate a contact sequence over a list of configurations
#
......
......@@ -20,7 +20,8 @@
#include "rbprmbuilder.impl.hh"
#include "hpp/rbprm/rbprm-device.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/sampling/sample-db.hh"
#include "hpp/model/urdf/util.hh"
......@@ -52,6 +53,7 @@ namespace hpp {
try
{
hpp::model::DevicePtr_t romDevice = model::Device::create (robotName);
std::cout << romDevice->currentConfiguration() << std::endl;
romDevices_.insert(std::make_pair(robotName, romDevice));
hpp::model::urdf::loadRobotModel (romDevice,
std::string (rootJointType),
......@@ -558,7 +560,7 @@ namespace hpp {
{
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);
lastStatesComputed_ = interpolator->Interpolate(problemSolver_->collisionObstacles(),configurations,robustnessTreshold);
hpp::floatSeqSeq *res;
......@@ -609,7 +611,7 @@ namespace hpp {
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);
hpp::floatSeqSeq *res;
......@@ -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)
{
std::stringstream ss;
......
......@@ -125,6 +125,7 @@ namespace hpp {
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 interpolateBetweenStates(double state1, double state2) 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);
......
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