avoid double state update before collision check (#2)

This commit is contained in:
Robert Haschke 2017-09-28 11:42:44 -07:00 committed by Michael Görner
parent 2dfc2f395e
commit d02816ef67
2 changed files with 2 additions and 2 deletions

View File

@ -76,7 +76,7 @@ namespace {
const double* joint_positions){
state->setJointGroupPositions(jmg, joint_positions);
state->update();
return !scene->isStateColliding(*state, jmg->getName());
return !scene->isStateColliding(static_cast<const robot_state::RobotState&>(*state), jmg->getName());
}
}

View File

@ -100,7 +100,7 @@ namespace {
state->setJointGroupPositions(jmg, joint_positions);
state->update();
if( scene->isStateColliding(*state, jmg->getName()) ){
if( scene->isStateColliding(static_cast<const robot_state::RobotState&>(*state), jmg->getName()) ){
old_solutions->emplace_back();
state->copyJointGroupPositions(jmg, old_solutions->back());
return false;