Skip to content

Commit 0b07ef3

Browse files
marrtsLevi-Armstrong
marrts
authored and
Levi-Armstrong
committed
Updated uses of fixed size interpolation to lvs interpolation
1 parent 5a6cfbf commit 0b07ef3

File tree

5 files changed

+28
-23
lines changed

5 files changed

+28
-23
lines changed

‎tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/interface_utils.h

Lines changed: 9 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -34,16 +34,18 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
3434
TESSERACT_COMMON_IGNORE_WARNINGS_POP
3535

3636
#include <tesseract_motion_planners/simple/simple_motion_planner.h>
37-
#include <tesseract_motion_planners/simple/profile/simple_planner_default_plan_profile.h>
37+
#include <tesseract_motion_planners/simple/profile/simple_planner_default_lvs_plan_profile.h>
3838

3939
namespace tesseract_planning
4040
{
4141
/** @brief Provided for backwards compatibility */
4242
inline CompositeInstruction generateSeed(const CompositeInstruction& instructions,
4343
const tesseract_environment::EnvState::ConstPtr& current_state,
4444
const tesseract::Tesseract::ConstPtr& tesseract,
45-
int freespace_steps = 10,
46-
int cartesian_steps = 10)
45+
double state_longest_valid_segment_length = 5 * M_PI / 180,
46+
double translation_longest_valid_segment_length = 0.15,
47+
double rotation_longest_valid_segment_length = 5 * M_PI / 180,
48+
int min_steps = 1)
4749
{
4850
// Fill out request and response
4951
PlannerRequest request;
@@ -55,7 +57,10 @@ inline CompositeInstruction generateSeed(const CompositeInstruction& instruction
5557
// Set up planner
5658
SimpleMotionPlanner planner;
5759

58-
auto profile = std::make_shared<SimplePlannerDefaultPlanProfile>(freespace_steps, cartesian_steps);
60+
auto profile = std::make_shared<SimplePlannerDefaultLVSPlanProfile>(state_longest_valid_segment_length,
61+
translation_longest_valid_segment_length,
62+
rotation_longest_valid_segment_length,
63+
min_steps);
5964
planner.plan_profiles[instructions.getProfile()] = profile;
6065
auto flat = flattenProgram(instructions);
6166
for (const auto& i : flat)

‎tesseract/tesseract_planning/tesseract_motion_planners/src/simple/simple_motion_planner.cpp

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -32,7 +32,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
3232
TESSERACT_COMMON_IGNORE_WARNINGS_POP
3333

3434
#include <tesseract_motion_planners/simple/simple_motion_planner.h>
35-
#include <tesseract_motion_planners/simple/profile/simple_planner_default_plan_profile.h>
35+
#include <tesseract_motion_planners/simple/profile/simple_planner_default_lvs_plan_profile.h>
3636
#include <tesseract_motion_planners/core/utils.h>
3737
#include <tesseract_command_language/command_language.h>
3838
#include <tesseract_command_language/utils/utils.h>
@@ -72,7 +72,7 @@ std::string SimpleMotionPlannerStatusCategory::message(int code) const
7272
SimpleMotionPlanner::SimpleMotionPlanner(std::string name)
7373
: MotionPlanner(std::move(name)), status_category_(std::make_shared<const SimpleMotionPlannerStatusCategory>(name_))
7474
{
75-
plan_profiles[DEFAULT_PROFILE_KEY] = std::make_shared<SimplePlannerDefaultPlanProfile>();
75+
plan_profiles[DEFAULT_PROFILE_KEY] = std::make_shared<SimplePlannerDefaultLVSPlanProfile>();
7676
}
7777

7878
bool SimpleMotionPlanner::terminate()
@@ -215,7 +215,7 @@ CompositeInstruction SimpleMotionPlanner::processCompositeInstruction(const Comp
215215

216216
std::string profile = getProfileString(plan_instruction->getProfile(), name_, request.plan_profile_remapping);
217217
SimplePlannerPlanProfile::Ptr start_plan_profile = getProfile<SimplePlannerPlanProfile>(
218-
profile, plan_profiles, std::make_shared<SimplePlannerDefaultPlanProfile>());
218+
profile, plan_profiles, std::make_shared<SimplePlannerDefaultLVSPlanProfile>());
219219
if (!start_plan_profile)
220220
throw std::runtime_error("SimpleMotionPlanner: Invalid start profile");
221221

‎tesseract/tesseract_planning/tesseract_motion_planners/test/descartes_planner_tests.cpp

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -132,7 +132,7 @@ TEST_F(TesseractPlanningDescartesUnit, DescartesPlannerFixedPoses) // NOLINT
132132
program.push_back(plan_f1);
133133

134134
// Create a seed
135-
CompositeInstruction seed = generateSeed(program, cur_state, tesseract_ptr_);
135+
CompositeInstruction seed = generateSeed(program, cur_state, tesseract_ptr_, 3.14, 1.0, 3.14, 10);
136136

137137
// Create Profiles
138138
auto plan_profile = std::make_shared<DescartesDefaultPlanProfileD>();
@@ -249,7 +249,7 @@ TEST_F(TesseractPlanningDescartesUnit, DescartesPlannerAxialSymetric) // NOLINT
249249
program.push_back(plan_f1);
250250

251251
// Create a seed
252-
CompositeInstruction seed = generateSeed(program, cur_state, tesseract_ptr_);
252+
CompositeInstruction seed = generateSeed(program, cur_state, tesseract_ptr_, 3.14, 1.0, 3.14, 10);
253253

254254
// Create Profiles
255255
auto plan_profile = std::make_shared<DescartesDefaultPlanProfileD>();
@@ -356,7 +356,7 @@ TEST_F(TesseractPlanningDescartesUnit, DescartesPlannerCollisionEdgeEvaluator)
356356
program.push_back(plan_f1);
357357

358358
// Create a seed
359-
CompositeInstruction seed = generateSeed(program, cur_state, tesseract_ptr_, 2, 2);
359+
CompositeInstruction seed = generateSeed(program, cur_state, tesseract_ptr_, 3.14, 1.0, 3.14, 2);
360360

361361
// Create Profiles
362362
auto plan_profile = std::make_shared<DescartesDefaultPlanProfileD>();

‎tesseract/tesseract_planning/tesseract_motion_planners/test/ompl_planner_tests.cpp

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -195,7 +195,7 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespacePlannerUnit)
195195
program.push_back(plan_f1);
196196

197197
// Create a seed
198-
CompositeInstruction seed = generateSeed(program, cur_state, tesseract);
198+
CompositeInstruction seed = generateSeed(program, cur_state, tesseract, 3.14, 1.0, 3.14, 10);
199199

200200
// Create Profiles
201201
auto plan_profile = std::make_shared<OMPLDefaultPlanProfile>();
@@ -249,7 +249,7 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespacePlannerUnit)
249249
program.push_back(plan_f1);
250250

251251
// Create a new seed
252-
seed = generateSeed(program, cur_state, tesseract);
252+
seed = generateSeed(program, cur_state, tesseract, 3.14, 1.0, 3.14, 10);
253253

254254
// Update Configuration
255255
request.instructions = program;
@@ -284,7 +284,7 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespacePlannerUnit)
284284
program.push_back(plan_f1);
285285

286286
// Create a new seed
287-
seed = generateSeed(program, cur_state, tesseract);
287+
seed = generateSeed(program, cur_state, tesseract, 3.14, 1.0, 3.14, 10);
288288

289289
// Update Configuration
290290
request.instructions = program;
@@ -345,7 +345,7 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespaceCartesianGoalPlannerUnit)
345345
program.push_back(plan_f1);
346346

347347
// Create a seed
348-
CompositeInstruction seed = generateSeed(program, cur_state, tesseract);
348+
CompositeInstruction seed = generateSeed(program, cur_state, tesseract, 3.14, 1.0, 3.14, 10);
349349

350350
// Create Profiles
351351
auto plan_profile = std::make_shared<OMPLDefaultPlanProfile>();
@@ -434,7 +434,7 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespaceCartesianStartPlannerUnit)
434434
program.push_back(plan_f1);
435435

436436
// Create a seed
437-
CompositeInstruction seed = generateSeed(program, cur_state, tesseract);
437+
CompositeInstruction seed = generateSeed(program, cur_state, tesseract, 3.14, 1.0, 3.14, 10);
438438

439439
// Create Profiles
440440
auto plan_profile = std::make_shared<OMPLDefaultPlanProfile>();

‎tesseract/tesseract_planning/tesseract_motion_planners/test/trajopt_planner_tests.cpp

Lines changed: 8 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -153,7 +153,7 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptPlannerBooleanFlagsJointJoint) // N
153153
program.push_back(plan_f1);
154154

155155
// Create a seed
156-
CompositeInstruction seed = generateSeed(program, cur_state, tesseract_ptr_);
156+
CompositeInstruction seed = generateSeed(program, cur_state, tesseract_ptr_, 3.14, 1.0, 3.14, 10);
157157

158158
// Create Profiles
159159
auto plan_profile = std::make_shared<TrajOptDefaultPlanProfile>();
@@ -223,7 +223,7 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceJointJoint) // NOLINT
223223
program.push_back(plan_f1);
224224

225225
// Create a seed
226-
CompositeInstruction seed = generateSeed(program, cur_state, tesseract_ptr_);
226+
CompositeInstruction seed = generateSeed(program, cur_state, tesseract_ptr_, 3.14, 1.0, 3.14, 10);
227227

228228
// Create Profiles
229229
auto plan_profile = std::make_shared<TrajOptDefaultPlanProfile>();
@@ -298,7 +298,7 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceJointCart) // NOLINT
298298
program.push_back(plan_f1);
299299

300300
// Create a seed
301-
CompositeInstruction seed = generateSeed(program, cur_state, tesseract_ptr_);
301+
CompositeInstruction seed = generateSeed(program, cur_state, tesseract_ptr_, 3.14, 1.0, 3.14, 10);
302302

303303
// Create Profiles
304304
auto plan_profile = std::make_shared<TrajOptDefaultPlanProfile>();
@@ -378,7 +378,7 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceCartJoint) // NOLINT
378378
program.push_back(plan_f1);
379379

380380
// Create a seed
381-
CompositeInstruction seed = generateSeed(program, cur_state, tesseract_ptr_);
381+
CompositeInstruction seed = generateSeed(program, cur_state, tesseract_ptr_, 3.14, 1.0, 3.14, 10);
382382

383383
// Create Profiles
384384
auto plan_profile = std::make_shared<TrajOptDefaultPlanProfile>();
@@ -458,7 +458,7 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceCartCart) // NOLINT
458458
program.push_back(plan_f1);
459459

460460
// Create a seed
461-
CompositeInstruction seed = generateSeed(program, cur_state, tesseract_ptr_);
461+
CompositeInstruction seed = generateSeed(program, cur_state, tesseract_ptr_, 3.14, 1.0, 3.14, 10);
462462

463463
// Create Profiles
464464
auto plan_profile = std::make_shared<TrajOptDefaultPlanProfile>();
@@ -538,7 +538,7 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptPlannerBooleanFlagsCartCart) // NOL
538538
program.push_back(plan_f1);
539539

540540
// Create a seed
541-
CompositeInstruction seed = generateSeed(program, cur_state, tesseract_ptr_);
541+
CompositeInstruction seed = generateSeed(program, cur_state, tesseract_ptr_, 3.14, 1.0, 3.14, 10);
542542

543543
// Create Profiles
544544
auto plan_profile = std::make_shared<TrajOptDefaultPlanProfile>();
@@ -626,7 +626,7 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptArrayJointConstraint) // NOLINT
626626
}
627627

628628
// Create a seed
629-
CompositeInstruction seed = generateSeed(program, cur_state, tesseract_ptr_);
629+
CompositeInstruction seed = generateSeed(program, cur_state, tesseract_ptr_, 3.14, 1.0, 3.14, 10);
630630

631631
// Create Profiles
632632
auto plan_profile = std::make_shared<TrajOptDefaultPlanProfile>();
@@ -692,7 +692,7 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptArrayJointCost) // NOLINT
692692
}
693693

694694
// Create a seed
695-
CompositeInstruction seed = generateSeed(program, cur_state, tesseract_ptr_);
695+
CompositeInstruction seed = generateSeed(program, cur_state, tesseract_ptr_, 3.14, 1.0, 3.14, 10);
696696

697697
// Create Profiles
698698
auto plan_profile = std::make_shared<TrajOptDefaultPlanProfile>();

0 commit comments

Comments
 (0)