add API for rrtOnePhase methods

idl/hpp/corbaserver/rbprm/rbprmbuilder.idl

@@ -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

src/hpp/corbaserver/rbprm/rbprmfullbody.py

@@ -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

src/rbprmbuilder.impl.cc

@@ -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

src/rbprmbuilder.impl.hh

@@ -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);