Commit 37cb84a6 authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

add API for rrtOnePhase methods

parent 4eb1a9f7
......@@ -542,6 +542,34 @@ module hpp
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 need to be consecutive with no contact variation between them
/// (the free limbs can move, but there should be no contact creation/break)
/// \param state1 index of the first state
/// \param state2 index of the second state
/// \param rootPositions1 com positions to track
/// \param numOptimizations Number of iterations of the shortcut algorithm to apply between each states
/// \return id of the root path computed
floatSeq effectorRRTOnePhase(in double state1, in double state2,
in unsigned short comTraj,
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 need to be consecutive with no contact variation between them
/// (the free limbs can move, but there should be no contact creation/break)
/// \param state1 index of the first state
/// \param state2 index of the second state
/// \param rootPositions1 com positions to track
/// \param numOptimizations Number of iterations of the shortcut algorithm to apply between each states
/// \return id of the root path computed
floatSeq comRRTOnePhase(in double state1, in double state2,
in unsigned short comTraj,
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
......
......@@ -675,6 +675,30 @@ class FullBody (object):
return self.client.rbprm.rbprm.effectorRRTFromPath(state1, refPathId, path_start, path_to, comPos1, comPos2, comPos3, numOptim, trackedEffectors)
## Provided a path has already been computed and interpolated, generate a continuous path
# between two indicated states. The states need to be consecutive with no contact variation between them
# (the free limbs can move, but there should be no contact creation/break)
# \param state1 index of the first state
# \param state2 index of the second state
# \param comPos com position to track
# \param numOptimizations Number of iterations of the shortcut algorithm to apply between each states
def effectorRRTOnePhase(self,state1,state2,comPos,numOptim=10):
return self.client.rbprm.rbprm.effectorRRTOnePhase(state1,state2,comPos,numOptim)
## Provided a path has already been computed and interpolated, generate a continuous path
# between two indicated states. The states need to be consecutive with no contact variation between them
# (the free limbs can move, but there should be no contact creation/break)
# \param state1 index of the first state
# \param state2 index of the second state
# \param comPos com position to track
# \param numOptimizations Number of iterations of the shortcut algorithm to apply between each states
def comRRTOnePhase(self,state1,state2,comPos,numOptim=10):
return self.client.rbprm.rbprm.comRRTOnePhase(state1,state2,comPos,numOptim)
# compute and add a trajectory for the end effector between the 2 states
# represented as a bezier curve.
# Do not check the kinematic feasability of this trajectory
......
......@@ -2523,6 +2523,69 @@ assert(s2 == s1 +1);
}
}
hpp::floatSeq* RbprmBuilder::rrtOnePhase(t_rrt functor,double state1,double state2,
unsigned short comTraj,
unsigned short numOptimizations) throw (hpp::Error)
{
hppDout(notice,"########## begin rrtOnePhase for state "<<state1<<" and "<<state2<<" ###########");
if(lastStatesComputed_.size () < state1 || lastStatesComputed_.size () < state2 )
{
throw std::runtime_error ("did not find a states at indicated indices");
}
const core::PathVectors_t& paths = problemSolver()->paths();
if(paths.size() -1 < comTraj)
{
throw std::runtime_error("in effectorRRTOnePhase, com trajectory is not present in problem solver");
}
unsigned int seed = (unsigned int) (time(NULL)) ;
std::cout<<"seed effectorRRT = "<<seed<<std::endl;
hppDout(notice,"seed effectorRRT = "<<seed);
srand (seed);
State s1 = lastStatesComputed_[size_t(state1)];
State s2 = lastStatesComputed_[size_t(state2)];
hppDout(notice,"state1 = r(["<<model::displayConfig(s1.configuration_)<<")]");
hppDout(notice,"state2 = r(["<<model::displayConfig(s2.configuration_)<<")]");
core::PathVectorPtr_t resPath = core::PathVector::create(fullBody()->device_->configSize(), fullBody()->device_->numberDof());
std::vector<CORBA::Short> pathsIds;
core::PathPtr_t p1 = (*functor)(fullBody(),problemSolver(), paths[comTraj],
s1,s2, numOptimizations,true);
hppDout(notice,"effectorRRT done.");
// reduce path to remove extradof
core::SizeInterval_t interval(0, p1->initial().rows()-1);
core::SizeIntervals_t intervals;
intervals.push_back(interval);
PathPtr_t reducedPath = core::SubchainPath::create(p1,intervals);
resPath->appendPath(reducedPath);
pathsIds.push_back(problemSolver()->addPath(resPath));
hpp::floatSeq* dofArray = new hpp::floatSeq();
dofArray->length(pathsIds.size());
for(std::size_t i=0; i< pathsIds.size(); ++i)
{
(*dofArray)[(_CORBA_ULong)i] = pathsIds[i];
}
return dofArray;
}
hpp::floatSeq* RbprmBuilder::effectorRRTOnePhase(double state1,double state2,
unsigned short comTraj,
unsigned short numOptimizations) throw (hpp::Error)
{
return rrtOnePhase(&interpolation::effectorRRT,state1,state2,comTraj,numOptimizations);
}
hpp::floatSeq* RbprmBuilder::comRRTOnePhase(double state1,double state2,
unsigned short comTraj,
unsigned short numOptimizations) throw (hpp::Error)
{
return rrtOnePhase(&interpolation::comRRT,state1,state2,comTraj,numOptimizations);
}
CORBA::Short RbprmBuilder::generateEndEffectorBezier(double state1, double state2,
unsigned short cT)throw (hpp::Error){
try
......
......@@ -314,6 +314,15 @@ namespace hpp {
unsigned short comTraj3,
unsigned short numOptimizations,
const hpp::Names_t& trackedEffectors) throw (hpp::Error);
virtual hpp::floatSeq* rrtOnePhase(t_rrt functor,double state1,double state2,
unsigned short comTraj,
unsigned short numOptimizations) throw (hpp::Error);
virtual hpp::floatSeq* effectorRRTOnePhase(double state1,double state2,
unsigned short comTraj,
unsigned short numOptimizations) throw (hpp::Error);
virtual hpp::floatSeq* comRRTOnePhase(double state1,double state2,
unsigned short comTraj,
unsigned short numOptimizations) throw (hpp::Error);
virtual CORBA::Short generateEndEffectorBezier(double state1, double state2,
unsigned short cT) 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