[transition test] add a method to retrieve the Bezier curve in python from the path index

idl/hpp/corbaserver/rbprm/rbprmbuilder.idl

@@ -717,6 +717,12 @@ module hpp
     ///  Throw an error if there is no trajectory computed for the given id/name
     floatSeqSeqSeq getEffectorTrajectoryWaypoints(in unsigned short pathId,in string effectorName) raises (Error);
 
+    /// Return the waypoints of the bezier curve at given index
+    /// \param pathId the path index
+    /// \return a list of lenght degree+ 2 : the first item is the time of the curve, the others are the waypoints
+    floatSeqSeq getPathAsBezier(in unsigned short pathId)raises (Error);
+
+
     boolean areKinematicsConstraintsVerified(in floatSeq point)raises (Error);
 
     boolean areKinematicsConstraintsVerifiedForState(in unsigned short stateFrom,in floatSeq point)raises (Error);

src/hpp/corbaserver/rbprm/rbprmfullbody.py

@@ -1080,6 +1080,15 @@ class FullBody (object):
              res +=[curve]
          return res
 
+    ## Return the bezier curve corresponding to a given path index
+     def getPathAsBezier(self,pathId):
+         l = self.client.rbprm.rbprm.getPathAsBezier(pathId)
+         t = l[0][0]
+         wps = matrix(l[1:]).transpose()
+         curve = bezier(wps,t)
+         return curve
+
+
       ## return the contacts variation between two states
       # \param stateIdFrom : index of the first state
       # \param stateIdTo : index of the second state

src/rbprmbuilder.impl.cc

@@ -1589,6 +1589,46 @@ namespace hpp {
         }
     }
 
+    hpp::floatSeqSeq* RbprmBuilder::getPathAsBezier(unsigned short pathId)throw (hpp::Error){
+        try{
+            if(!fullBodyLoaded_)
+                throw std::runtime_error ("No Fullbody loaded");
+            if(problemSolver()->paths().size() <= pathId)
+                throw std::runtime_error ("No path at index "+ boost::lexical_cast<std::string>(pathId));
+            PathVectorPtr_t pathVector = problemSolver()->paths()[pathId];
+            PathPtr_t path = pathVector->pathAtRank(0);
+            // try to cast path as BezierPath :
+            BezierPathPtr_t bezierPath = boost::dynamic_pointer_cast<BezierPath>(path);
+            if(!bezierPath)
+                throw std::runtime_error ("Not a bezier path at index "+ boost::lexical_cast<std::string>(pathId));
+
+            const bezier_t::t_point_t waypoints = bezierPath->getWaypoints();
+
+            // build the floatSeqSeq : first value is the time, the others are the waypoints
+            hpp::floatSeqSeq *res;
+            res = new hpp::floatSeqSeq ();
+            _CORBA_ULong size = (_CORBA_ULong)waypoints.size()+1;
+            res->length (size); // +1 because the first value is the length (time)
+            { // add the time at the first index :
+                double* dofArray = hpp::floatSeq::allocbuf(1);
+                hpp::floatSeq floats (1, 1, dofArray, true);
+                dofArray[0] = bezierPath->length();
+                (*res) [(_CORBA_ULong)0] = floats; // Always assume the curve start at 0. There isn't any ways to create it otherwise in python
+            }
+            // now add the waypoints :
+            std::size_t i=1;
+            for(bezier_t::t_point_t::const_iterator wit = waypoints.begin(); wit != waypoints.end(); ++wit,++i)
+            {
+                (*res) [(_CORBA_ULong)i] = vectorToFloatseq(*wit);
+            }
+            return res;
+        }
+        catch(std::runtime_error& e)
+        {
+            std::cout << "ERROR " << e.what() << std::endl;
+            throw Error(e.what());
+        }
+    }
 
 
 

src/rbprmbuilder.impl.hh

@@ -347,6 +347,8 @@ namespace hpp {
         virtual hpp::floatSeq* computeTargetTransform(const char* limbName, const hpp::floatSeq& configuration, const hpp::floatSeq& p, const hpp::floatSeq& n) throw (hpp::Error);
         virtual Names_t* getEffectorsTrajectoriesNames(unsigned short pathId)throw (hpp::Error);
         virtual hpp::floatSeqSeqSeq* getEffectorTrajectoryWaypoints(unsigned short pathId,const char* effectorName)throw (hpp::Error);
+        virtual hpp::floatSeqSeq* getPathAsBezier(unsigned short pathId)throw (hpp::Error);
+
 
         virtual bool areKinematicsConstraintsVerified(const hpp::floatSeq &point)throw (hpp::Error);
         virtual bool areKinematicsConstraintsVerifiedForState(unsigned short stateId,const hpp::floatSeq &point)throw (hpp::Error);