From f24790162347fc2976bcef45db189d193e887c0b Mon Sep 17 00:00:00 2001 From: v4hn Date: Thu, 2 Mar 2017 17:24:21 +0100 Subject: [PATCH] generate grasp pose: store colliding states as infeasible solutions This way there is no need to do collision checking for this solution again --- src/subtasks/generate_grasp_pose.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) 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; } }