Fix simple planner enforcing limits on cartesian waypoints that do no…

…t have a seed
tesseract-robotics · Feb 4, 2024 · c568056 · c568056
1 parent 69de947
commit c568056
Showing 1 changed file with 7 additions and 3 deletions.
diff --git a/tesseract_motion_planners/simple/src/simple_motion_planner.cpp b/tesseract_motion_planners/simple/src/simple_motion_planner.cpp
@@ -115,9 +115,13 @@ PlannerResponse SimpleMotionPlanner::solve(const PlannerRequest& request) const
     }
     else if (mi.getWaypoint().isCartesianWaypoint())
     {
-      Eigen::VectorXd& jp = mi.getWaypoint().as<CartesianWaypointPoly>().getSeed().position;
-      assert(tesseract_common::satisfiesPositionLimits<double>(jp, joint_limits));
-      tesseract_common::enforcePositionLimits<double>(jp, joint_limits);
+      auto& cwp = mi.getWaypoint().as<CartesianWaypointPoly>();
+      if (cwp.hasSeed())
+      {
+        Eigen::VectorXd& jp = cwp.getSeed().position;
+        assert(tesseract_common::satisfiesPositionLimits<double>(jp, joint_limits));
+        tesseract_common::enforcePositionLimits<double>(jp, joint_limits);
+      }
     }
     else
       throw std::runtime_error("Unsupported waypoint type.");