Commit 2c680c6c authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

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

parent 24cc85a8
...@@ -699,7 +699,7 @@ module hpp ...@@ -699,7 +699,7 @@ module hpp
boolean isReachableFromState(in unsigned short stateFrom, in unsigned short stateTo)raises (Error); 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 }; // interface Robot
......
...@@ -164,19 +164,25 @@ print "number of configs :", len(configsFull) ...@@ -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) 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.) #player.displayContactPlan(1.)
r(fullBody.getConfigAtState(3)) 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 = fullBody.getConfigAtState(3)
q1[-3:]=[2,0,0] q1[-3:]=[2,0,0]
r(q1) r(q1)
......
...@@ -4,6 +4,8 @@ DIR = "/home/pfernbac/Documents/com_ineq_test/" ...@@ -4,6 +4,8 @@ DIR = "/home/pfernbac/Documents/com_ineq_test/"
STAB_NAME = "stability" STAB_NAME = "stability"
CONS_NAME = "constraints" CONS_NAME = "constraints"
KIN_NAME = "kinematics" KIN_NAME = "kinematics"
BEZIER_NAME = "bezier_wp"
def generate_off_file(name): def generate_off_file(name):
os.remove(DIR+name+"_.off") if os.path.isfile(DIR+name+"_.off") else None os.remove(DIR+name+"_.off") if os.path.isfile(DIR+name+"_.off") else None
...@@ -175,6 +177,8 @@ global i_const ...@@ -175,6 +177,8 @@ global i_const
i_const = 0 i_const = 0
global i_two_step global i_two_step
i_two_step=0 i_two_step=0
global i_bezier
i_bezier=0
def displayStabilityConstraints(r,quasiStatic=False): def displayStabilityConstraints(r,quasiStatic=False):
...@@ -243,11 +247,23 @@ def removeAllConstraints(r): ...@@ -243,11 +247,23 @@ def removeAllConstraints(r):
global i_kin global i_kin
global i_const global i_const
global i_two_step 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_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("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("all_constraint_"+str(i_const-1),r.sceneName)
r.client.gui.removeFromGroup("kin_constraint_"+str(i_kin-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("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
...@@ -3042,7 +3042,7 @@ assert(s2 == s1 +1); ...@@ -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_){ if(!fullBodyLoaded_){
throw std::runtime_error ("fullBody not loaded"); throw std::runtime_error ("fullBody not loaded");
} }
...@@ -3050,7 +3050,12 @@ assert(s2 == s1 +1); ...@@ -3050,7 +3050,12 @@ assert(s2 == s1 +1);
throw std::runtime_error ("Unexisting state ID"); throw std::runtime_error ("Unexisting state ID");
} }
reachability::Result res = reachability::isReachableDynamic(fullBody(),lastStatesComputed_[stateFrom],lastStatesComputed_[stateTo]); 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;
} }
......
...@@ -341,7 +341,7 @@ namespace hpp { ...@@ -341,7 +341,7 @@ namespace hpp {
virtual bool areKinematicsConstraintsVerified(const hpp::floatSeq &point)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); 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 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) void selectFullBody (const char* name) throw (hpp::Error)
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment