Sort constraints for reduced cardinality

Signed-off-by: methylDragon <[email protected]>
osrf · Nov 21, 2023 · 6c29eae · 6c29eae
1 parent 6e5b963
commit 6c29eae
Showing 1 changed file with 56 additions and 0 deletions.
diff --git a/nexus_motion_planner/src/motion_plan_cache.cpp b/nexus_motion_planner/src/motion_plan_cache.cpp
@@ -417,6 +417,34 @@ MotionPlanCache::extract_and_append_plan_goal_to_query(
     {
       orientation_constraints.push_back(orientation_constraint);
     }
+
+    // Also sort for even less cardinality.
+    std::sort(
+      joint_constraints.begin(), joint_constraints.end(),
+      [](
+        const moveit_msgs::msg::JointConstraint& l,
+        const moveit_msgs::msg::JointConstraint& r)
+      {
+        return l.joint_name < r.joint_name;
+      });
+
+    std::sort(
+      position_constraints.begin(), position_constraints.end(),
+      [](
+        const moveit_msgs::msg::PositionConstraint& l,
+        const moveit_msgs::msg::PositionConstraint& r)
+      {
+        return l.link_name < r.link_name;
+      });
+
+    std::sort(
+      orientation_constraints.begin(), orientation_constraints.end(),
+      [](
+        const moveit_msgs::msg::OrientationConstraint& l,
+        const moveit_msgs::msg::OrientationConstraint& r)
+      {
+        return l.link_name < r.link_name;
+      });
   }
 
   // Joint constraints
@@ -742,6 +770,34 @@ MotionPlanCache::extract_and_append_plan_goal_to_metadata(
     {
       orientation_constraints.push_back(orientation_constraint);
     }
+
+    // Also sort for even less cardinality.
+    std::sort(
+      joint_constraints.begin(), joint_constraints.end(),
+      [](
+        const moveit_msgs::msg::JointConstraint& l,
+        const moveit_msgs::msg::JointConstraint& r)
+      {
+        return l.joint_name < r.joint_name;
+      });
+
+    std::sort(
+      position_constraints.begin(), position_constraints.end(),
+      [](
+        const moveit_msgs::msg::PositionConstraint& l,
+        const moveit_msgs::msg::PositionConstraint& r)
+      {
+        return l.link_name < r.link_name;
+      });
+
+    std::sort(
+      orientation_constraints.begin(), orientation_constraints.end(),
+      [](
+        const moveit_msgs::msg::OrientationConstraint& l,
+        const moveit_msgs::msg::OrientationConstraint& r)
+      {
+        return l.link_name < r.link_name;
+      });
   }
 
   // Joint constraints