generate grasp pose: store colliding states as infeasible solutions

This way there is no need to do collision checking for this solution again
This commit is contained in:
v4hn 2017-03-02 17:24:21 +01:00
parent c3c6d899cb
commit f247901623

View File

@ -74,7 +74,12 @@ namespace {
state->setJointGroupPositions(jmg, joint_positions);
state->update();
return scene->isStateColliding(*state, jmg->getName());
if( scene->isStateColliding(*state, jmg->getName()) ){
old_solutions->emplace_back();
state->copyJointGroupPositions(jmg, old_solutions->back());
return false;
}
return true;
}
}