limbRRT, comRRT and generateRootPath return id of the path created

idl/hpp/corbaserver/rbprm/rbprmbuilder.idl

@@ -248,7 +248,8 @@ module hpp
     /// \param positions array of positions
     /// \param q1 quaternion of 1st rotation
     /// \param q2 quaternion of 2nd rotation
-    void generateRootPath(in floatSeqSeq rootPositions, in floatSeq q1, in floatSeq q2) raises (Error);
+    /// \return id of the root path computed
+    short generateRootPath(in floatSeqSeq rootPositions, in floatSeq q1, in floatSeq q2) raises (Error);
 
 
     /// Provided a path has already been computed and interpolated, generate a continuous path
@@ -260,7 +261,8 @@ module hpp
     /// \param state1 index of first state.
     /// \param state2 index of second state.
     /// \param numOptimizations Number of iterations of the shortcut algorithm to apply between each states
-    void limbRRT(in double state1, in double state2, in unsigned short numOptimizations) raises (Error);
+    /// \return id of the root path computed
+    short limbRRT(in double state1, in double state2, 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.
@@ -272,7 +274,8 @@ module hpp
     /// \param state2 index of second state.
     /// \param path index of the path considered for the generation
     /// \param numOptimizations Number of iterations of the shortcut algorithm to apply between each states
-    void limbRRTFromRootPath(in double state1, in double state2,  in  unsigned short path, in unsigned short numOptimizations) raises (Error);
+    /// \return id of the root path computed
+    short limbRRTFromRootPath(in double state1, in double state2,  in  unsigned short path, 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.
@@ -284,7 +287,8 @@ module hpp
     /// \param state2 index of second state.
     /// \param comPath index of the path considered of the com path
     /// \param numOptimizations Number of iterations of the shortcut algorithm to apply between each states
-    void comRRT(in double state1, in double state2,  in  unsigned short comPath, in unsigned short numOptimizations) raises (Error);
+    /// \return id of the root path computed
+    short comRRT(in double state1, in double state2,  in  unsigned short comPath, in unsigned short numOptimizations) raises (Error);
 
     /// Saves the last computed states by the function interpolate in a filename.
     /// Raises an error if interpolate has not been called, or the file could not be opened.

src/hpp/corbaserver/rbprm/rbprmfullbody.py

@@ -295,6 +295,7 @@ class FullBody (object):
     #  \param positions array of positions
     #  \param configState1 configuration of 1st rotation
     #  \param configState2 configuration of 2nd rotation
+    #  \return id of the resulting path
     def generateRootPathStates(self, positions, configState1, configState2):
 		return self.client.rbprm.rbprm.generateRootPath(positions, configState1[3:7], configState2[3:7])
 		
@@ -305,6 +306,7 @@ class FullBody (object):
     # \param index of first state.
     # \param index of second state.
     # \param numOptim Number of iterations of the shortcut algorithm to apply between each states
+    #  \return id of the resulting path
     def limbRRT(self, state1, state2, numOptim = 10):
 		return self.client.rbprm.rbprm.limbRRT(state1, state2, numOptim)
 		
@@ -318,6 +320,7 @@ class FullBody (object):
 	# \param state2 index of second state.                                                                          
 	# \param path index of the path considered for the generation                                                   
 	# \param numOptimizations Number of iterations of the shortcut algorithm to apply between each states 
+    #  \return id of the resulting path
     def limbRRTFromRootPath(self, state1, state2, path, numOptim = 10):
 		return self.client.rbprm.rbprm.limbRRTFromRootPath(state1, state2, path, numOptim)		
 		
@@ -332,6 +335,7 @@ class FullBody (object):
 	# \param state2 index of second state.                                                                          
 	# \param path index of the com path considered for the generation                                                   
 	# \param numOptimizations Number of iterations of the shortcut algorithm to apply between each states 
+    #  \return id of the resulting path
     def comRRT(self, state1, state2, path, numOptim = 10):
 		return self.client.rbprm.rbprm.comRRT(state1, state2, path, numOptim)		
 		

src/hpp/corbaserver/rbprm/tools/cwc_trajectory.py

@@ -59,3 +59,10 @@ def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, re
 
 	plt.show()
 	return var_final, params
+	
+def solve_com_RRT(fullBody, states, state_id, computeCones = False, mu = 1, reduce_ineq = True, num_optims = 0):
+	res = gen_trajectory(fullBody, configs, state_id, computeCones, 0.5, False)
+	c0 = configs[i][0:3]
+	pos = [c0] + [c.tolist() for c in res[0]['c']]
+	pid = fullBody.generateRootPathStates(pos, configs[i], configs[i+1])
+	fullBody.comRRT(i,i+1,pid,num_optims)

src/rbprmbuilder.impl.cc

@@ -751,14 +751,14 @@ namespace hpp {
         return res;
     }
 
-    void RbprmBuilder::generateRootPath(const hpp::floatSeqSeq& rootPositions,
+    CORBA::Short RbprmBuilder::generateRootPath(const hpp::floatSeqSeq& rootPositions,
                           const hpp::floatSeq& q1Seq, const hpp::floatSeq& q2Seq) throw (hpp::Error)
     {
         try
         {
             model::Configuration_t q1 = dofArrayToConfig(4, q1Seq), q2 = dofArrayToConfig(4, q2Seq);
             T_Configuration positions = doubleDofArrayToConfig(3, rootPositions);
-            problemSolver_->addPath(addRotations(positions,q1,q2,fullBody_->device_->currentConfiguration(),
+            return problemSolver_->addPath(addRotations(positions,q1,q2,fullBody_->device_->currentConfiguration(),
                                                  fullBody_->device_,problemSolver_->problem()));
         }
         catch(std::runtime_error& e)
@@ -918,14 +918,14 @@ namespace hpp {
         }
     }
 
-    void AddPath(core::PathPtr_t path, core::ProblemSolverPtr_t ps)
+    CORBA::Short AddPath(core::PathPtr_t path, core::ProblemSolverPtr_t ps)
     {
         core::PathVectorPtr_t resPath = core::PathVector::create(path->outputSize(), path->outputDerivativeSize());
         resPath->appendPath(path);
-        ps->addPath(resPath);
+        return ps->addPath(resPath);
     }
 
-    void RbprmBuilder::limbRRT(double state1, double state2, unsigned short numOptimizations) throw (hpp::Error)
+    CORBA::Short RbprmBuilder::limbRRT(double state1, double state2, unsigned short numOptimizations) throw (hpp::Error)
     {
         try
         {
@@ -938,7 +938,7 @@ namespace hpp {
 //            /interpolation::TimeConstraintHelper helper(fullBody_, problemSolver_->problem());
             core::PathPtr_t path = interpolation::limbRRT(fullBody_,problemSolver_->problem(),
                                                                           lastStatesComputed_.begin()+s1,lastStatesComputed_.begin()+s2, numOptimizations);
-            AddPath(path,problemSolver_);
+            return AddPath(path,problemSolver_);
         }
         catch(std::runtime_error& e)
         {
@@ -946,7 +946,7 @@ namespace hpp {
         }
     }
 
-    void RbprmBuilder::limbRRTFromRootPath(double state1, double state2, unsigned short path, unsigned short numOptimizations) throw (hpp::Error)
+    CORBA::Short RbprmBuilder::limbRRTFromRootPath(double state1, double state2, unsigned short path, unsigned short numOptimizations) throw (hpp::Error)
     {
         try
         {
@@ -962,7 +962,7 @@ namespace hpp {
             }
             core::PathPtr_t path = interpolation::limbRRTFromPath(fullBody_,problemSolver_->problem(), problemSolver_->paths()[pathId],
                                                                           lastStatesComputedTime_.begin()+s1,lastStatesComputedTime_.begin()+s2, numOptimizations);
-            AddPath(path,problemSolver_);
+            return AddPath(path,problemSolver_);
         }
         catch(std::runtime_error& e)
         {
@@ -970,7 +970,7 @@ namespace hpp {
         }
     }
 
-    void RbprmBuilder::comRRT(double state1, double state2, unsigned short path, unsigned short numOptimizations) throw (hpp::Error)
+    CORBA::Short RbprmBuilder::comRRT(double state1, double state2, unsigned short path, unsigned short numOptimizations) throw (hpp::Error)
     {
         try
         {
@@ -988,7 +988,7 @@ assert(s2 == s1 +1);
             }
             core::PathPtr_t path = interpolation::comRRT(fullBody_,problemSolver_->problem(), problemSolver_->paths()[pathId],
                                                                           *(lastStatesComputed_.begin()+s1),*(lastStatesComputed_.begin()+s2), numOptimizations);
-            AddPath(path,problemSolver_);
+            return AddPath(path,problemSolver_);
         }
         catch(std::runtime_error& e)
         {

src/rbprmbuilder.impl.hh

@@ -144,11 +144,11 @@ namespace hpp {
         virtual hpp::floatSeqSeq* interpolateConfigs(const hpp::floatSeqSeq& configs, double robustnessTreshold) throw (hpp::Error);
         virtual hpp::floatSeqSeq* getContactCone(unsigned short stateId) throw (hpp::Error);
         virtual hpp::floatSeqSeq* getContactIntermediateCone(unsigned short stateId) throw (hpp::Error);
-        virtual void generateRootPath(const hpp::floatSeqSeq& rootPositions,
+        virtual CORBA::Short generateRootPath(const hpp::floatSeqSeq& rootPositions,
                                       const hpp::floatSeq& q1, const hpp::floatSeq& q2) throw (hpp::Error);
-        virtual void limbRRT(double state1, double state2, unsigned short numOptimizations) throw (hpp::Error);
-        virtual void limbRRTFromRootPath(double state1, double state2, unsigned short path, unsigned short numOptimizations) throw (hpp::Error);
-        virtual void comRRT(double state1, double state2, unsigned short path, unsigned short numOptimizations) throw (hpp::Error);
+        virtual CORBA::Short limbRRT(double state1, double state2, unsigned short numOptimizations) throw (hpp::Error);
+        virtual CORBA::Short limbRRTFromRootPath(double state1, double state2, unsigned short path, unsigned short numOptimizations) throw (hpp::Error);
+        virtual CORBA::Short comRRT(double state1, double state2, unsigned short path, unsigned short numOptimizations) throw (hpp::Error);
         virtual void saveComputedStates(const char* filepath) throw (hpp::Error);
         virtual void saveLimbDatabase(const char* limbname,const char* filepath) throw (hpp::Error);
         virtual hpp::floatSeq* getOctreeBox(const char* limbName, double sampleId) throw (hpp::Error);