[contact generation] add method generateEndEffectorBezier

idl/hpp/corbaserver/rbprm/rbprmbuilder.idl

@@ -551,6 +551,17 @@ module hpp
                                     in unsigned short numOptimizations,
                                     in Names_t trackedEffectors) raises (Error);
 
+
+
+    /// 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
+    /// \param state1 index of first state.
+    /// \param state2 index of end state.
+    /// \param rootPositions com positions to track
+    short generateEndEffectorBezier(in double state1,in double state2,
+                        in  unsigned short comTraj) raises (Error);
+
     /// Project a given state into a given COM position
     /// between two indicated states. The states do not need to be consecutive, but increasing in Id.
     /// Will fail if the index of the state does not exist.

src/hpp/corbaserver/rbprm/rbprmfullbody.py

@@ -665,6 +665,17 @@ class FullBody (object):
      # \return id of the root path computed
      def effectorRRTFromPath(self, state1, refPathId, path_start, path_to, comPos1, comPos2, comPos3, numOptim = 10, trackedEffectors = []):
           return self.client.rbprm.rbprm.effectorRRTFromPath(state1, refPathId, path_start, path_to, comPos1, comPos2, comPos3, numOptim, trackedEffectors)
+
+
+    # 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
+    # \param state1 index of first state.
+    # \param state2 index of end state.
+    # \param rootPositions com positions to track
+     def generateEndEffectorBezier(self, state1, state2, comPos):
+          return self.client.rbprm.rbprm.generateEndEffectorBezier(state1, state2, comPos)
+
           
      ## Project a given state into a given COM position
      # between two indicated states. The states do not need to be consecutive, but increasing in Id.

src/rbprmbuilder.impl.cc

@@ -2410,6 +2410,33 @@ assert(s2 == s1 +1);
         }
     }
 
+    CORBA::Short RbprmBuilder::generateEndEffectorBezier(double state1, double state2,
+    unsigned short cT)throw (hpp::Error){
+        try
+        {
+            hppDout(notice,"Begin generateEndEffectorBezier");
+            std::size_t s1((std::size_t)state1), s2((std::size_t)state2);
+            hppDout(notice,"index first state = "<<s1<<" ; index second state : "<<s2);
+            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));
+            }
+            const core::PathVectors_t& paths = problemSolver()->paths();
+            if(paths.size() -1 < cT)
+            {
+                throw std::runtime_error("in generateEndEffectorBezier, at least one com trajectory is not present in problem solver");
+            }
+            State& state1=lastStatesComputed_[s1], state2=lastStatesComputed_[s2];
+            hppDout(notice,"start generateEndEffectorBezier");
+            interpolation::generateEndEffectorBezier(fullBody(),problemSolver(),paths[cT],state1,state2);
+            return problemSolver()->paths().size()-1;
+        }
+        catch(std::runtime_error& e)
+        {
+            throw Error(e.what());
+        }
+    }
+
     hpp::floatSeq* RbprmBuilder::projectToCom(double state, const hpp::floatSeq& targetCom, unsigned short max_num_sample) throw (hpp::Error)
     {
         try

src/rbprmbuilder.impl.hh

@@ -313,6 +313,10 @@ namespace hpp {
                                            unsigned short comTraj3,
                                            unsigned short numOptimizations,
                                            const hpp::Names_t& trackedEffectors) throw (hpp::Error);
+
+        virtual CORBA::Short generateEndEffectorBezier(double state1, double state2,
+        unsigned short cT) throw (hpp::Error);
+
         virtual hpp::floatSeq* projectToCom(double state, const hpp::floatSeq& targetCom, unsigned short max_num_sample) throw (hpp::Error);
         virtual CORBA::Short createState(const hpp::floatSeq& configuration, const hpp::Names_t& contactLimbs) throw (hpp::Error);
         virtual hpp::floatSeq* getConfigAtState(unsigned short stateId) throw (hpp::Error);