handle colinear points errors

Author: AimanHaidar

moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_free_generator.hpp

@@ -68,13 +68,32 @@ class PathFreeGenerator
    */
 
   static double computeBlendRadius(const std::vector<KDL::Frame>& waypoints_, double smoothness);
+  static void checkConsecutiveColinearWaypoints(const KDL::Frame& p1, const KDL::Frame& p2, const KDL::Frame& p3);
 
 private:
   PathFreeGenerator(){};  // no instantiation of this helper class!
 
   static constexpr double MAX_SEGMENT_LENGTH{ 0.2e-3 };
   static constexpr double MIN_SMOOTHNESS{ 0.01 };
   static constexpr double MAX_SMOOTHNESS{ 0.99 };
+  static constexpr double MAX_COLINEAR_NORM{ 1e-9 };
+};
+
+class ErrorMotionPlanningColinearConsicutiveWaypoints : public KDL::Error_MotionPlanning
+{
+public:
+  const char* Description() const override
+  {
+    return "Three collinear consecutive waypoints."
+           " A Free Path cannot be created.";
+  }
+  int GetType() const override
+  {
+    return ERROR_CODE_COLINEAR_CONSECUTIVE_WAYPOINTS;
+  }  // LCOV_EXCL_LINE
+
+private:
+  static constexpr int ERROR_CODE_COLINEAR_CONSECUTIVE_WAYPOINTS{ 3104 };
 };
 
 }  // namespace pilz_industrial_motion_planner

moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_free.hpp

@@ -51,6 +51,8 @@ CREATE_MOVEIT_ERROR_CODE_EXCEPTION(LinTrajectoryConversionFailure, moveit_msgs::
 CREATE_MOVEIT_ERROR_CODE_EXCEPTION(JointNumberMismatch, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS);
 CREATE_MOVEIT_ERROR_CODE_EXCEPTION(LinInverseForGoalIncalculable, moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION);
 CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoWaypointsSpecified, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN);
+CREATE_MOVEIT_ERROR_CODE_EXCEPTION(ConsicutiveColinearWaypoints,
+                                   moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN);
 
 /**
  * @brief This class implements a free trajectory generator in Cartesian

moveit_planners/pilz_industrial_motion_planner/src/path_free_generator.cpp

@@ -94,7 +94,7 @@ double PathFreeGenerator::computeBlendRadius(const std::vector<KDL::Frame>& wayp
   {
     double dist1 = pose_distance(waypoints_[i], waypoints_[i - 1]);
     double dist2 = pose_distance(waypoints_[i + 1], waypoints_[i]);
-
+    checkConsecutiveColinearWaypoints(waypoints_[i - 1], waypoints_[i], waypoints_[i + 1]);
     if (dist1 < MAX_SEGMENT_LENGTH || dist2 < MAX_SEGMENT_LENGTH)
     {
       continue;
@@ -115,5 +115,17 @@ double PathFreeGenerator::computeBlendRadius(const std::vector<KDL::Frame>& wayp
 
   return max_radius;
 }
+void PathFreeGenerator::checkConsecutiveColinearWaypoints(const KDL::Frame& p1, const KDL::Frame& p2,
+                                                          const KDL::Frame& p3)
+{
+  KDL::Vector v1 = p2.p - p1.p;
+  KDL::Vector v2 = p3.p - p2.p;
+
+  KDL::Vector cross_product = v1 * v2;
+  if (cross_product.Norm() < MAX_COLINEAR_NORM)
+  {
+    throw ErrorMotionPlanningColinearConsicutiveWaypoints();
+  }
+}
 
 }  // namespace pilz_industrial_motion_planner

moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_free.cpp

@@ -133,7 +133,18 @@ void TrajectoryGeneratorFree::plan(const planning_scene::PlanningSceneConstPtr&
   // set pilz cartesian limits for each item
   setMaxCartesianSpeed(req);
   // create Cartesian FREE path
-  std::unique_ptr<KDL::Path> path(setPathFree(plan_info.start_pose, plan_info.waypoints, req.smoothness_level));
+  std::unique_ptr<KDL::Path> path;
+  try
+  {
+    path = setPathFree(plan_info.start_pose, plan_info.waypoints, req.smoothness_level);
+  }
+  catch (const KDL::Error_MotionPlanning& e)
+  {
+    RCLCPP_ERROR(getLogger(), "Motion planning error: %s", e.Description());
+    std::ostringstream os;
+    os << "waypoints specified in path constraints have three consicutive colinear points";
+    throw ConsicutiveColinearWaypoints(os.str());
+  }
   // create velocity profile
   std::unique_ptr<KDL::VelocityProfile> vp(
       cartesianTrapVelocityProfile(req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor, path));