From 2c680c6cd8eec21a59948fb59367e131e5cd1d86 Mon Sep 17 00:00:00 2001
From: Pierre Fernbach <pierre.fernbach@laas.fr>
Date: Fri, 2 Feb 2018 19:32:37 +0100
Subject: [PATCH] [transition test] isDynamicallyReachable now add the
resulting path (if any) and return the Id
---
idl/hpp/corbaserver/rbprm/rbprmbuilder.idl | 2 +-
...latGround_hrp2_interpSTATIC_testTransition.py | 14 ++++++++++----
script/tools/constraint_to_dae.py | 16 ++++++++++++++++
src/rbprmbuilder.impl.cc | 9 +++++++--
src/rbprmbuilder.impl.hh | 2 +-
5 files changed, 35 insertions(+), 8 deletions(-)
diff --git a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
index 61c7ad8..9e42988 100755
--- a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
+++ b/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
diff --git a/script/dynamic/flatGround_hrp2_interpSTATIC_testTransition.py b/script/dynamic/flatGround_hrp2_interpSTATIC_testTransition.py
index 62d7bae..e825bc9 100644
--- a/script/dynamic/flatGround_hrp2_interpSTATIC_testTransition.py
+++ b/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)
diff --git a/script/tools/constraint_to_dae.py b/script/tools/constraint_to_dae.py
index 9aaae47..471bdcc 100644
--- a/script/tools/constraint_to_dae.py
+++ b/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
diff --git a/src/rbprmbuilder.impl.cc b/src/rbprmbuilder.impl.cc
index dce1c4d..7c72f96 100755
--- a/src/rbprmbuilder.impl.cc
+++ b/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;
}
diff --git a/src/rbprmbuilder.impl.hh b/src/rbprmbuilder.impl.hh
index 3e5d750..55aaf02 100755
--- a/src/rbprmbuilder.impl.hh
+++ b/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)
--
GitLab