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
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
......
......@@ -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)
......
......@@ -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
......@@ -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;
}
......
......@@ -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)
......
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