diff --git a/src/subtasks/generate_grasp_pose.cpp b/src/subtasks/generate_grasp_pose.cpp index d1c74db8..67b3ff6a 100644 --- a/src/subtasks/generate_grasp_pose.cpp +++ b/src/subtasks/generate_grasp_pose.cpp @@ -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; } }