Add max_steps to lvs no IK simple planner

tesseract-robotics · Nov 12, 2021 · c30af8f · c30af8f
1 parent a1d8cb6
commit c30af8f
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 2 deletions.
diff --git a/.../include/tesseract_motion_planners/simple/profile/simple_planner_lvs_no_ik_plan_profile.h b/.../include/tesseract_motion_planners/simple/profile/simple_planner_lvs_no_ik_plan_profile.h
@@ -54,11 +54,13 @@ class SimplePlannerLVSNoIKPlanProfile : public SimplePlannerPlanProfile
    * @param translation_longest_valid_segment_length The maximum translation distance between successive steps
    * @param rotation_longest_valid_segment_length The maximum rotational distance between successive steps
    * @param min_steps The minimum number of steps for the plan
+   * @param max_steps The maximum number of steps for the plan
    */
   SimplePlannerLVSNoIKPlanProfile(double state_longest_valid_segment_length = 5 * M_PI / 180,
                                   double translation_longest_valid_segment_length = 0.1,
                                   double rotation_longest_valid_segment_length = 5 * M_PI / 180,
-                                  int min_steps = 1);
+                                  int min_steps = 1,
+                                  int max_steps = std::numeric_limits<int>::max());
 
   CompositeInstruction generate(const PlanInstruction& prev_instruction,
                                 const PlanInstruction& base_instruction,
@@ -77,6 +79,9 @@ class SimplePlannerLVSNoIKPlanProfile : public SimplePlannerPlanProfile
   /** @brief The minimum number of steps for the plan */
   int min_steps;
 
+  /** @brief The maximum number of steps for the plan */
+  int max_steps;
+
 protected:
   /**
    * @brief JointWaypoint to JointWaypoint

diff --git a/tesseract_motion_planners/core/src/simple/profile/simple_planner_lvs_no_ik_plan_profile.cpp b/tesseract_motion_planners/core/src/simple/profile/simple_planner_lvs_no_ik_plan_profile.cpp
@@ -32,11 +32,13 @@ namespace tesseract_planning
 SimplePlannerLVSNoIKPlanProfile::SimplePlannerLVSNoIKPlanProfile(double state_longest_valid_segment_length,
                                                                  double translation_longest_valid_segment_length,
                                                                  double rotation_longest_valid_segment_length,
-                                                                 int min_steps)
+                                                                 int min_steps,
+                                                                 int max_steps)
   : state_longest_valid_segment_length(state_longest_valid_segment_length)
   , translation_longest_valid_segment_length(translation_longest_valid_segment_length)
   , rotation_longest_valid_segment_length(rotation_longest_valid_segment_length)
   , min_steps(min_steps)
+  , max_steps(max_steps)
 {
 }
 
@@ -82,6 +84,7 @@ SimplePlannerLVSNoIKPlanProfile::stateJointJointWaypoint(const JointGroupInstruc
   int steps = std::max(trans_steps, rot_steps);
   steps = std::max(steps, joint_steps);
   steps = std::max(steps, min_steps);
+  steps = std::min(steps, max_steps);
 
   // Linearly interpolate in joint space
   Eigen::MatrixXd states = interpolate(j1, j2, steps);
@@ -108,6 +111,7 @@ SimplePlannerLVSNoIKPlanProfile::stateJointCartWaypoint(const JointGroupInstruct
 
   // Check min steps requirement
   steps = std::max(steps, min_steps);
+  steps = std::min(steps, max_steps);
 
   // Convert to MoveInstructions
   Eigen::MatrixXd states = j1.replicate(1, steps + 1);
@@ -134,6 +138,7 @@ SimplePlannerLVSNoIKPlanProfile::stateCartJointWaypoint(const JointGroupInstruct
 
   // Check min steps requirement
   steps = std::max(steps, min_steps);
+  steps = std::min(steps, max_steps);
 
   // Convert to MoveInstructions
   Eigen::MatrixXd states = j2.replicate(1, steps + 1);
@@ -160,6 +165,7 @@ CompositeInstruction SimplePlannerLVSNoIKPlanProfile::stateCartCartWaypoint(cons
 
   // Check min steps requirement
   steps = std::max(steps, min_steps);
+  steps = std::min(steps, max_steps);
 
   // Convert to MoveInstructions
   Eigen::MatrixXd states = seed.replicate(1, steps + 1);