Update due to changes in TrajOpt CollisionTerm supporting longest valid segment length

Author: Levi-Armstrong

tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt/config/trajopt_planner_default_config.h

@@ -88,6 +88,8 @@ struct TrajOptPlannerDefaultConfig : public TrajOptPlannerConfig
    * singularities */
   JointWaypoint::ConstPtr configuration = nullptr;
 
+  /** @brief The type of contact test to perform: FIRST, CLOSEST, ALL */
+  tesseract_collision::ContactTestType contact_test_type = tesseract_collision::ContactTestType::ALL;
   /** @brief If true, collision checking will be enabled. Default: true*/
   bool collision_check = true;
   /** @brief If true, use continuous collision checking */

tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt/config/utils.h

@@ -77,11 +77,14 @@ trajopt::TermInfo::Ptr createConfigurationTermInfo(const JointWaypoint::ConstPtr
                                                    double coeff = 1.0,
                                                    const std::string& name = "configuration_cost");
 
-trajopt::TermInfo::Ptr createCollisionTermInfo(int n_steps,
-                                               double collision_safety_margin,
-                                               bool collision_continuous = true,
-                                               double coeff = 20.0,
-                                               const std::string& name = "collision_cost");
+trajopt::TermInfo::Ptr createCollisionTermInfo(
+    int n_steps,
+    double collision_safety_margin,
+    bool collision_continuous = true,
+    double coeff = 20.0,
+    tesseract_collision::ContactTestType contact_test_type = tesseract_collision::ContactTestType::ALL,
+    double longest_valid_segment_length = 0.5,
+    const std::string& name = "collision_cost");
 
 trajopt::TermInfo::Ptr
 createSmoothVelocityTermInfo(int n_steps, int n_joints, double coeff = 5.0, const std::string& name = "joint_vel_cost");

tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt/config/trajopt_planner_default_config.cpp

@@ -26,6 +26,8 @@
 #include <tesseract_motion_planners/trajopt/config/trajopt_planner_default_config.h>
 #include <tesseract_motion_planners/trajopt/config/utils.h>
 
+static const double LONGEST_VALID_SEGMENT_FRACTION_DEFAULT = 0.01;
+
 namespace tesseract_motion_planners
 {
 TrajOptPlannerDefaultConfig::TrajOptPlannerDefaultConfig(tesseract::Tesseract::ConstPtr tesseract_,
@@ -163,9 +165,34 @@ std::shared_ptr<trajopt::ProblemConstructionInfo> TrajOptPlannerDefaultConfig::g
   // Set costs for the rest of the points
   if (collision_check)
   {
+    // Calculate longest valid segment length
+    const Eigen::MatrixX2d& limits = kin->getLimits();
+    double length = 0;
+    double extent = (limits.col(1) - limits.col(0)).norm();
+    if (longest_valid_segment_fraction > 0 && longest_valid_segment_length > 0)
+    {
+      length = std::min(longest_valid_segment_fraction * extent, longest_valid_segment_length);
+    }
+    else if (longest_valid_segment_fraction > 0)
+    {
+      length = longest_valid_segment_fraction * extent;
+    }
+    else if (longest_valid_segment_length > 0)
+    {
+      length = longest_valid_segment_length;
+    }
+    else
+    {
+      length = LONGEST_VALID_SEGMENT_FRACTION_DEFAULT * extent;
+    }
+
     // Create a default collision term info
-    trajopt::TermInfo::Ptr ti =
-        createCollisionTermInfo(pci.basic_info.n_steps, collision_safety_margin, collision_continuous, collision_coeff);
+    trajopt::TermInfo::Ptr ti = createCollisionTermInfo(pci.basic_info.n_steps,
+                                                        collision_safety_margin,
+                                                        collision_continuous,
+                                                        collision_coeff,
+                                                        contact_test_type,
+                                                        length);
 
     // Update the term info with the (possibly) new start and end state indices for which to apply this cost
     std::shared_ptr<trajopt::CollisionTermInfo> ct = std::static_pointer_cast<trajopt::CollisionTermInfo>(ti);

tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt/config/trajopt_planner_freespace_config.cpp

@@ -26,6 +26,8 @@
 #include <tesseract_motion_planners/trajopt/config/trajopt_planner_freespace_config.h>
 #include <tesseract_motion_planners/trajopt/config/utils.h>
 
+static const double LONGEST_VALID_SEGMENT_FRACTION_DEFAULT = 0.01;
+
 namespace tesseract_motion_planners
 {
 std::shared_ptr<trajopt::ProblemConstructionInfo> TrajOptPlannerFreespaceConfig::generatePCI() const
@@ -159,9 +161,34 @@ std::shared_ptr<trajopt::ProblemConstructionInfo> TrajOptPlannerFreespaceConfig:
   // Set costs for the rest of the points
   if (collision_check)
   {
+    // Calculate longest valid segment length
+    const Eigen::MatrixX2d& limits = kin->getLimits();
+    double length = 0;
+    double extent = (limits.col(1) - limits.col(0)).norm();
+    if (longest_valid_segment_fraction > 0 && longest_valid_segment_length > 0)
+    {
+      length = std::min(longest_valid_segment_fraction * extent, longest_valid_segment_length);
+    }
+    else if (longest_valid_segment_fraction > 0)
+    {
+      length = longest_valid_segment_fraction * extent;
+    }
+    else if (longest_valid_segment_length > 0)
+    {
+      length = longest_valid_segment_length;
+    }
+    else
+    {
+      length = LONGEST_VALID_SEGMENT_FRACTION_DEFAULT * extent;
+    }
+
     // Create a default collision term info
-    trajopt::TermInfo::Ptr ti =
-        createCollisionTermInfo(pci.basic_info.n_steps, collision_safety_margin, collision_continuous, collision_coeff);
+    trajopt::TermInfo::Ptr ti = createCollisionTermInfo(pci.basic_info.n_steps,
+                                                        collision_safety_margin,
+                                                        collision_continuous,
+                                                        collision_coeff,
+                                                        contact_test_type,
+                                                        length);
 
     // Update the term info with the (possibly) new start and end state indices for which to apply this cost
     std::shared_ptr<trajopt::CollisionTermInfo> ct = std::static_pointer_cast<trajopt::CollisionTermInfo>(ti);

tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt/config/utils.cpp

@@ -218,6 +218,8 @@ trajopt::TermInfo::Ptr createCollisionTermInfo(int n_steps,
                                                double collision_safety_margin,
                                                bool collision_continuous,
                                                double coeff,
+                                               tesseract_collision::ContactTestType contact_test_type,
+                                               double longest_valid_segment_length,
                                                const std::string& name)
 {
   std::shared_ptr<trajopt::CollisionTermInfo> collision = std::make_shared<trajopt::CollisionTermInfo>();
@@ -226,7 +228,8 @@ trajopt::TermInfo::Ptr createCollisionTermInfo(int n_steps,
   collision->continuous = collision_continuous;
   collision->first_step = 0;
   collision->last_step = n_steps - 1;
-  collision->gap = 1;
+  collision->contact_test_type = contact_test_type;
+  collision->longest_valid_segment_length = longest_valid_segment_length;
   collision->info = trajopt::createSafetyMarginDataVector(n_steps, collision_safety_margin, coeff);
   return collision;
 }

tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt/trajopt_motion_planner.cpp

@@ -153,6 +153,7 @@ tesseract_common::StatusCode TrajOptMotionPlanner::solve(PlannerResponse& respon
   {
     length = LONGEST_VALID_SEGMENT_FRACTION_DEFAULT * extent;
   }
+
   std::vector<tesseract_collision::ContactResultMap> collisions;
   tesseract_environment::StateSolver::Ptr state_solver = config_->prob->GetEnv()->getStateSolver();
   tesseract_collision::ContinuousContactManager::Ptr continuous_manager =

tesseract_ros/tesseract_examples/tesseract_ros_examples/config/glass_up_right_plan.json

@@ -22,6 +22,7 @@
         "coeffs" : [20],
         "dist_pen" : [0.025],
         "continuous" : true,
+        "longest_valid_segment_length" : 0.1,
         "pairs" :
         [
           {

tesseract_ros/tesseract_examples/tesseract_ros_examples/src/basic_cartesian_example.cpp

@@ -165,7 +165,6 @@ TrajOptProb::Ptr BasicCartesianExample::cppMethod()
   collision->continuous = false;
   collision->first_step = 0;
   collision->last_step = pci.basic_info.n_steps - 1;
-  collision->gap = 1;
   collision->info = createSafetyMarginDataVector(pci.basic_info.n_steps, 0.025, 20);
 
   // Populate Constraints

tesseract_ros/tesseract_examples/tesseract_ros_examples/src/car_seat_example.cpp

@@ -295,7 +295,6 @@ std::shared_ptr<ProblemConstructionInfo> CarSeatExample::cppMethod(const std::st
   collision->continuous = true;
   collision->first_step = 0;
   collision->last_step = pci->basic_info.n_steps - 2;
-  collision->gap = 1;
   collision->info = createSafetyMarginDataVector(pci->basic_info.n_steps, 0.0001, 40);
   pci->cnt_infos.push_back(collision);
 

tesseract_ros/tesseract_examples/tesseract_ros_examples/src/glass_up_right_example.cpp

@@ -125,7 +125,6 @@ TrajOptProb::Ptr GlassUpRightExample::cppMethod()
   collision->continuous = false;
   collision->first_step = 0;
   collision->last_step = pci.basic_info.n_steps - 1;
-  collision->gap = 1;
   collision->info = createSafetyMarginDataVector(pci.basic_info.n_steps, 0.025, 20);
   for (auto& info : collision->info)
   {