From c30af8fb3029ac99aac04c695601993a81c067ea Mon Sep 17 00:00:00 2001 From: Levi-Armstrong Date: Thu, 11 Nov 2021 15:50:23 -0600 Subject: [PATCH] Add max_steps to lvs no IK simple planner --- .../profile/simple_planner_lvs_no_ik_plan_profile.h | 7 ++++++- .../profile/simple_planner_lvs_no_ik_plan_profile.cpp | 8 +++++++- 2 files changed, 13 insertions(+), 2 deletions(-) diff --git a/tesseract_motion_planners/core/include/tesseract_motion_planners/simple/profile/simple_planner_lvs_no_ik_plan_profile.h b/tesseract_motion_planners/core/include/tesseract_motion_planners/simple/profile/simple_planner_lvs_no_ik_plan_profile.h index 204da500091..9591217da44 100644 --- a/tesseract_motion_planners/core/include/tesseract_motion_planners/simple/profile/simple_planner_lvs_no_ik_plan_profile.h +++ b/tesseract_motion_planners/core/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::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 index e073e841a31..fca5a35691a 100644 --- 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);