[effector] add API for generateEffectorBezierArray

idl/hpp/corbaserver/rbprm/rbprmbuilder.idl

@@ -554,6 +554,16 @@ module hpp
                         in  unsigned short comTraj,
                         in unsigned short numOptimizations) raises (Error);
 
+    /// Provided a path has already been computed and interpolated, generate an array of bezier curves,
+    /// with varying weightRRT (see effector-rrt.cc::fitBezier)
+    /// \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
+    floatSeqSeq generateEffectorBezierArray(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

src/hpp/corbaserver/rbprm/rbprmfullbody.py

@@ -685,6 +685,17 @@ class FullBody (object):
      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 an array of bezier curves,
+    ## with varying weightRRT (see effector-rrt.cc::fitBezier)
+    # (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
+    # \return array of pathIds : first index is the trajectorie, second index is the curve inside this trajectory
+    # (there should be 3 curves per trajectories : takeoff / mid / landing)
+     def generateEffectorBezierArray(self,state1,state2,comPos,numOptim=10):
+         return self.client.rbprm.rbprm.generateEffectorBezierArray(state1,state2,comPos,numOptim)
 
 
 

src/rbprmbuilder.impl.cc

@@ -2578,6 +2578,80 @@ assert(s2 == s1 +1);
         return rrtOnePhase(&interpolation::comRRT,state1,state2,comTraj,numOptimizations);
     }
 
+    hpp::floatSeqSeq* RbprmBuilder::generateEffectorBezierArray(double state1,double state2,
+                                       unsigned short comTraj,
+                                       unsigned short numOptimizations) throw (hpp::Error)
+    {
+        hppDout(notice,"########## begin GenerateEffectorBezierArray 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)) ;
+        hppDout(notice,"seed generateEffectorBezierArray = "<<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_)<<")]");
+        bool success_comrrt = false;
+        core::PathPtr_t fullBodyComPath;
+        while( !success_comrrt){
+            try{
+                hppDout(notice,"In generateEffectorBezierArray : begin com-rrt.");
+                fullBodyComPath = interpolation::comRRT(fullBody(), problemSolver(), paths[comTraj], s1, s2, numOptimizations, true);
+                hppDout(notice,"In generateEffectorBezierArray : end com-rrt.");
+                success_comrrt = true;
+                PathVectorPtr_t pv = PathVector::create(fullBodyComPath->outputSize(),fullBodyComPath->outputDerivativeSize());
+                pv->appendPath(fullBodyComPath);
+                size_t id = problemSolver()->addPath(pv);
+                hppDout(notice,"Add com-rrt path at index : "<<id);
+            }catch(std::runtime_error e){
+                hppDout(notice,"In generateEffectorBezierArray : comRRT failed. ");
+                hppDout(notice,"Error = "<<e.what());
+             }
+        }
+        std::string effectorVar = s2.contactCreations(s1).front();
+        JointPtr_t effector =  fullBody()->device_->getJointByName(fullBody()->GetLimbs().at(effectorVar)->effector_->name());
+        std::vector<PathVectorPtr_t> listBeziers =  interpolation::fitBeziersToPath(fullBody(),effector,paths[comTraj]->length(),fullBodyComPath,s1,s2);
+
+        hpp::floatSeqSeq *res;
+        res = new hpp::floatSeqSeq ();
+        res->length ((_CORBA_ULong)listBeziers.size());
+
+        // for each pathVector : add each path and store the ID in an floatSeq
+        size_t id_traj = 0;
+        for(std::vector<PathVectorPtr_t>::const_iterator it_pv = listBeziers.begin() ; it_pv != listBeziers.end() ; ++it_pv){
+            std::vector<CORBA::Short> pathIds;
+            for(size_t id_path = 0 ; id_path < (*it_pv)->numberPaths() ; ++id_path){
+                PathPtr_t path = (*it_pv)->pathAtRank(id_path);
+                PathVectorPtr_t pv = PathVector::create(path->outputSize(),path->outputDerivativeSize());
+                pv->appendPath(path);
+                pathIds.push_back(problemSolver()->addPath(pv));
+            }
+            // convert pathIds to floatSeq :
+            hpp::floatSeq* dofArray = new hpp::floatSeq();
+            dofArray->length(pathIds.size());
+            for(std::size_t i=0; i< pathIds.size(); ++i)
+            {
+              (*dofArray)[(_CORBA_ULong)i] = pathIds[i];
+            }
+            (*res) [(_CORBA_ULong)id_traj] = *dofArray;
+            id_traj++;
+        }
+
+        return res;
+    }
+
+
+
     CORBA::Short RbprmBuilder::generateEndEffectorBezier(double state1, double state2,
     unsigned short cT)throw (hpp::Error){
         try

src/rbprmbuilder.impl.hh

@@ -323,6 +323,9 @@ namespace hpp {
         virtual hpp::floatSeq* comRRTOnePhase(double state1,double state2,
                                            unsigned short comTraj,
                                            unsigned short numOptimizations) throw (hpp::Error);
+        virtual hpp::floatSeqSeq* generateEffectorBezierArray(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);