[transition test] isDynamicallyReachable now add the resulting path (if any) and return the Id

idl/hpp/corbaserver/rbprm/rbprmbuilder.idl

@@ -699,7 +699,7 @@ module hpp
 
     boolean isReachableFromState(in unsigned short stateFrom, in unsigned short stateTo)raises (Error);
 
-    boolean isDynamicallyReachableFromState(in unsigned short stateFrom, in unsigned short stateTo)raises (Error);
+    short isDynamicallyReachableFromState(in unsigned short stateFrom, in unsigned short stateTo)raises (Error);
 
 
   }; // interface Robot

script/dynamic/flatGround_hrp2_interpSTATIC_testTransition.py

@@ -164,19 +164,25 @@ print "number of configs :", len(configsFull)
 
 
 
-from 
-
-
 
 
 player = fullBodyPlayerHrp2.Player(fullBody,pp,tp,configsFull,draw=False,use_window=1,optim_effector=True,use_velocity=False,pathId = pId)
 
-
+from display_tools import *
 
 
 #player.displayContactPlan(1.)
 
 r(fullBody.getConfigAtState(3))
+pid = fullBody.isDynamicallyReachableFromState(2,3)
+pp.displayPath(pid,r.color.blue)
+displayBezierConstraints(r)
+
+createSphere("s",r)
+moveSphere("s",r,x)
+
+
+
 q1 = fullBody.getConfigAtState(3)
 q1[-3:]=[2,0,0]
 r(q1)

script/tools/constraint_to_dae.py

@@ -4,6 +4,8 @@ DIR = "/home/pfernbac/Documents/com_ineq_test/"
 STAB_NAME = "stability"
 CONS_NAME = "constraints"
 KIN_NAME = "kinematics"
+BEZIER_NAME = "bezier_wp"
+
 
 def generate_off_file(name):
     os.remove(DIR+name+"_.off") if os.path.isfile(DIR+name+"_.off") else None
@@ -175,6 +177,8 @@ global i_const
 i_const = 0
 global i_two_step
 i_two_step=0
+global i_bezier
+i_bezier=0
     
 
 def displayStabilityConstraints(r,quasiStatic=False):
@@ -243,11 +247,23 @@ def removeAllConstraints(r):
     global i_kin
     global i_const
     global i_two_step
+    global i_bezier
+
     r.client.gui.removeFromGroup("constraint_twoStep_c"+str(i_two_step-1),r.sceneName)
     r.client.gui.removeFromGroup("constraint_twoStep_b"+str(i_two_step-1),r.sceneName)
     r.client.gui.removeFromGroup("all_constraint_"+str(i_const-1),r.sceneName)
     r.client.gui.removeFromGroup("kin_constraint_"+str(i_kin-1),r.sceneName)
     r.client.gui.removeFromGroup("stab_constraint_"+str(i_stab-1),r.sceneName)
+    r.client.gui.removeFromGroup("bezier_contraint_"+str(i_bezier-1),r.sceneName)
     
 
 
+def displayBezierConstraints(r):
+    global i_bezier
+    generate_off_file(BEZIER_NAME)
+    convert_off_dae(BEZIER_NAME)
+    insert_color_material(BEZIER_NAME,"green",[0,1,0],0.3)
+    
+    r.client.gui.addMesh("bezier_constraint_"+str(i_bezier),DIR+BEZIER_NAME+".dae")
+    r.client.gui.addToGroup("bezier_constraint_"+str(i_bezier),r.sceneName)        
+    i_bezier +=1        
\ No newline at end of file

src/rbprmbuilder.impl.cc

@@ -3042,7 +3042,7 @@ assert(s2 == s1 +1);
 
 
 
-    bool RbprmBuilder::isDynamicallyReachableFromState(unsigned short stateFrom,unsigned short stateTo)throw (hpp::Error){
+    CORBA::Short RbprmBuilder::isDynamicallyReachableFromState(unsigned short stateFrom,unsigned short stateTo)throw (hpp::Error){
         if(!fullBodyLoaded_){
           throw std::runtime_error ("fullBody not loaded");
         }
@@ -3050,7 +3050,12 @@ assert(s2 == s1 +1);
             throw std::runtime_error ("Unexisting state ID");
         }
         reachability::Result res = reachability::isReachableDynamic(fullBody(),lastStatesComputed_[stateFrom],lastStatesComputed_[stateTo]);
-        return (res.success());
+        if (res.success()){
+            core::PathVectorPtr_t pathVector = core::PathVector::create(res.path_->outputSize(),res.path_->outputDerivativeSize());
+            pathVector->appendPath(res.path_);
+            return problemSolver()->addPath(pathVector);
+        }else
+            return 0;
     }
 
 

src/rbprmbuilder.impl.hh

@@ -341,7 +341,7 @@ namespace hpp {
         virtual bool areKinematicsConstraintsVerified(const hpp::floatSeq &point)throw (hpp::Error);
         virtual bool areKinematicsConstraintsVerifiedForState(unsigned short stateId,const hpp::floatSeq &point)throw (hpp::Error);
         virtual bool isReachableFromState(unsigned short stateFrom,unsigned short stateTo)throw (hpp::Error);
-        virtual bool isDynamicallyReachableFromState(unsigned short stateFrom,unsigned short stateTo)throw (hpp::Error);
+        virtual CORBA::Short isDynamicallyReachableFromState(unsigned short stateFrom,unsigned short stateTo)throw (hpp::Error);
 
 
         void selectFullBody (const char* name) throw (hpp::Error)