Commit b4faf78f authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

Fix error handling (don't crash server when collision is found during comRRT)

And clean debug message
parent 4cc0e10f
......@@ -259,8 +259,7 @@ def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, d
def __cVarPerPhase(var, dt, t, final_state, addValue):
varVals = addValue + [v.tolist() for v in final_state[var]]
print "t = "
print t
print "cVarPerPhase : t = ", t
varPerPhase = [[varVals[(int)(round(t_id/dt)) ] for t_id in np.arange(t[index],t[index+1]-_EPS,dt)] for index, _ in enumerate(t[:-1]) ]
#print "varperPhase ="
#print varPerPhase
......@@ -286,7 +285,6 @@ def __optim__threading_ok(fullBody, states, state_id, computeCones = False, mu =
else:
res = gen_trajectory(fullBody, states, state_id, computeCones, mu, dt, phase_dt, reduce_ineq, verbose, limbsCOMConstraints, profile, use_window,use_velocity, pathId)
alpha = res[1]['alpha']
print "alpha in optim__trheading = ",alpha
t = [ti * alpha for ti in res[1]["t_init_phases"]]
dt = res[1]["dt"] * alpha
final_state = res[0]
......
......@@ -1586,10 +1586,11 @@ assert(s2 == s1 +1);
&& problemSolver_->problem()->configValidations()->validate(s2Bis.configuration_, rport)))
{
std::cout << "could not project without collision at state " << s1 << std::endl;
//throw std::runtime_error ("could not project without collision at state " + s1 );
rport->print(std::cout);
//throw std::runtime_error ("could not project without collision at state " + s1 );
}
{
try{
hppDout(notice,"begin comRRT states 1 and 1bis");
core::PathPtr_t p1 = interpolation::comRRT(fullBody_,problemSolver_->problem(), paths[cT1],
state1,s1Bis, numOptimizations,true);
......@@ -1602,9 +1603,13 @@ assert(s2 == s1 +1);
resPath->appendPath(reducedPath);
pathsIds.push_back(AddPath(p1,problemSolver_));
}
catch(std::runtime_error& e)
{
throw Error(e.what());
}
{
try{
hppDout(notice,"begin comRRT between statebis 1 and 2");
core::PathPtr_t p2 =(*functor)(fullBody_,problemSolver_->problem(), paths[cT2],
s1Bis,s2Bis, numOptimizations,true);
......@@ -1617,9 +1622,13 @@ assert(s2 == s1 +1);
PathPtr_t reducedPath = core::SubchainPath::create(p2,intervals);
resPath->appendPath(reducedPath);
}
catch(std::runtime_error& e)
{
throw Error(e.what());
}
//if(s2Bis.configuration_ != state2.configuration_)
{
try{
hppDout(notice,"begin comRRT states 2bis and 2");
core::PathPtr_t p3= interpolation::comRRT(fullBody_,problemSolver_->problem(), paths[cT3],
s2Bis,state2, numOptimizations,true);
......@@ -1632,6 +1641,10 @@ assert(s2 == s1 +1);
resPath->appendPath(reducedPath);
pathsIds.push_back(AddPath(p3,problemSolver_));
}
catch(std::runtime_error& e)
{
throw Error(e.what());
}
pathsIds.push_back(AddPath(resPath,problemSolver_));
hpp::floatSeq* dofArray = new hpp::floatSeq();
......
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