From 6c29eaed577090fb24e5a1ea2b4fc33e4442a416 Mon Sep 17 00:00:00 2001
From: methylDragon <methylDragon@google.com>
Date: Mon, 20 Nov 2023 19:32:26 -0800
Subject: [PATCH] Sort constraints for reduced cardinality

Signed-off-by: methylDragon <methylDragon@gmail.com>
---
 .../src/motion_plan_cache.cpp                 | 56 +++++++++++++++++++
 1 file changed, 56 insertions(+)

diff --git a/nexus_motion_planner/src/motion_plan_cache.cpp b/nexus_motion_planner/src/motion_plan_cache.cpp
index 8470182..af889a8 100644
--- 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