diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp index 635a687f..0ed7dfb2 100644 --- a/core/src/stages/compute_ik.cpp +++ b/core/src/stages/compute_ik.cpp @@ -202,6 +202,9 @@ void ComputeIK::init(const moveit::core::RobotModelConstPtr& robot_model) void ComputeIK::onNewSolution(const SolutionBase &s) { + if (s.isFailure()) + return; + assert(s.start() && s.end()); assert(s.start()->scene() == s.end()->scene()); // wrapped child should be a generator planning_scene::PlanningScenePtr sandbox_scene = s.start()->scene()->diff();