ComputeIK: fix typo

This commit is contained in:
Robert Haschke 2019-02-20 11:15:05 +01:00
parent 2da9e0d722
commit 1047ef08a4

View File

@ -313,11 +313,11 @@ void ComputeIK::compute()
marker.color.a *= 0.5; marker.color.a *= 0.5;
failure_markers.push_back(marker); failure_markers.push_back(marker);
}; };
const auto& link_to_visualize = moveit::core::RobotModel::getRigidlyConnectedParentLinkModel(link) const auto& links_to_visualize = moveit::core::RobotModel::getRigidlyConnectedParentLinkModel(link)
->getParentJointModel()->getDescendantLinkModels(); ->getParentJointModel()->getDescendantLinkModels();
if (colliding) { if (colliding) {
SubTrajectory solution; SubTrajectory solution;
generateCollisionMarkers(sandbox_state, appender, link_to_visualize); generateCollisionMarkers(sandbox_state, appender, links_to_visualize);
std::copy(failure_markers.begin(), failure_markers.end(), std::back_inserter(solution.markers())); std::copy(failure_markers.begin(), failure_markers.end(), std::back_inserter(solution.markers()));
solution.markAsFailure(); solution.markAsFailure();
// TODO: visualize collisions // TODO: visualize collisions
@ -325,7 +325,7 @@ void ComputeIK::compute()
spawn(InterfaceState(sandbox_scene), std::move(solution)); spawn(InterfaceState(sandbox_scene), std::move(solution));
return; return;
} else } else
generateVisualMarkers(sandbox_state, appender, link_to_visualize); generateVisualMarkers(sandbox_state, appender, links_to_visualize);
// determine joint values of robot pose to compare IK solution with for costs // determine joint values of robot pose to compare IK solution with for costs