Skip to content

Commit 7f2c00c

Browse files
Levi-ArmstrongLevi-Armstrong
committed
Fix issues with how Collision Check Config was being used
1 parent 85144b0 commit 7f2c00c

31 files changed

+203
-177
lines changed

‎tesseract_examples/src/basic_cartesian_example.cpp‎

Lines changed: 8 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -228,9 +228,11 @@ bool BasicCartesianExample::run()
228228
{
229229
auto composite_profile = std::make_shared<TrajOptIfoptDefaultCompositeProfile>();
230230
composite_profile->collision_cost_config = trajopt_common::TrajOptCollisionConfig(0.025, 20);
231-
composite_profile->collision_cost_config.type = tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE;
231+
composite_profile->collision_cost_config.collision_check_config.type =
232+
tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE;
232233
composite_profile->collision_constraint_config = trajopt_common::TrajOptCollisionConfig(0.0, 20);
233-
composite_profile->collision_constraint_config.type = tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE;
234+
composite_profile->collision_constraint_config.collision_check_config.type =
235+
tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE;
234236

235237
composite_profile->smooth_velocities = true;
236238
composite_profile->smooth_accelerations = false;
@@ -252,9 +254,11 @@ bool BasicCartesianExample::run()
252254
{
253255
auto composite_profile = std::make_shared<TrajOptDefaultCompositeProfile>();
254256
composite_profile->collision_cost_config = trajopt_common::TrajOptCollisionConfig(0.025, 20);
255-
composite_profile->collision_cost_config.type = tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE;
257+
composite_profile->collision_cost_config.collision_check_config.type =
258+
tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE;
256259
composite_profile->collision_constraint_config = trajopt_common::TrajOptCollisionConfig(0.0, 20);
257-
composite_profile->collision_constraint_config.type = tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE;
260+
composite_profile->collision_constraint_config.collision_check_config.type =
261+
tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE;
258262
composite_profile->smooth_velocities = true;
259263
composite_profile->smooth_accelerations = false;
260264
composite_profile->smooth_jerks = false;

‎tesseract_examples/src/car_seat_example.cpp‎

Lines changed: 10 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -283,16 +283,17 @@ bool CarSeatExample::run()
283283
// Create TrajOpt_Ifopt Profile
284284
auto trajopt_ifopt_composite_profile = std::make_shared<TrajOptIfoptDefaultCompositeProfile>();
285285
trajopt_ifopt_composite_profile->collision_constraint_config = trajopt_common::TrajOptCollisionConfig(0.0, 1);
286-
trajopt_ifopt_composite_profile->collision_constraint_config.type =
286+
trajopt_ifopt_composite_profile->collision_constraint_config.collision_check_config.type =
287287
tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS;
288+
trajopt_ifopt_composite_profile->collision_constraint_config.collision_check_config.longest_valid_segment_length =
289+
0.1;
288290
trajopt_ifopt_composite_profile->collision_constraint_config.collision_margin_buffer = 0.005;
289-
trajopt_ifopt_composite_profile->collision_constraint_config.longest_valid_segment_length = 0.1;
290291

291292
trajopt_ifopt_composite_profile->collision_cost_config = trajopt_common::TrajOptCollisionConfig(0.005, 50);
292-
trajopt_ifopt_composite_profile->collision_cost_config.type =
293+
trajopt_ifopt_composite_profile->collision_cost_config.collision_check_config.type =
293294
tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS;
295+
trajopt_ifopt_composite_profile->collision_cost_config.collision_check_config.longest_valid_segment_length = 0.1;
294296
trajopt_ifopt_composite_profile->collision_cost_config.collision_margin_buffer = 0.01;
295-
trajopt_ifopt_composite_profile->collision_cost_config.longest_valid_segment_length = 0.1;
296297

297298
trajopt_ifopt_composite_profile->smooth_velocities = false;
298299
trajopt_ifopt_composite_profile->velocity_coeff = Eigen::VectorXd::Ones(1);
@@ -321,15 +322,16 @@ bool CarSeatExample::run()
321322
// Create TrajOpt Profile
322323
auto trajopt_composite_profile = std::make_shared<TrajOptDefaultCompositeProfile>();
323324
trajopt_composite_profile->collision_constraint_config = trajopt_common::TrajOptCollisionConfig(0.0, 10);
324-
trajopt_composite_profile->collision_constraint_config.type =
325+
trajopt_composite_profile->collision_constraint_config.collision_check_config.type =
325326
tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS;
327+
trajopt_composite_profile->collision_constraint_config.collision_check_config.longest_valid_segment_length = 0.1;
326328
trajopt_composite_profile->collision_constraint_config.collision_margin_buffer = 0.005;
327-
trajopt_composite_profile->collision_constraint_config.longest_valid_segment_length = 0.1;
328329

329330
trajopt_composite_profile->collision_cost_config = trajopt_common::TrajOptCollisionConfig(0.005, 50);
330-
trajopt_composite_profile->collision_cost_config.type = tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS;
331+
trajopt_composite_profile->collision_cost_config.collision_check_config.type =
332+
tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS;
333+
trajopt_composite_profile->collision_cost_config.collision_check_config.longest_valid_segment_length = 0.1;
331334
trajopt_composite_profile->collision_cost_config.collision_margin_buffer = 0.01;
332-
trajopt_composite_profile->collision_cost_config.longest_valid_segment_length = 0.1;
333335

334336
auto trajopt_plan_profile = std::make_shared<TrajOptDefaultPlanProfile>();
335337
trajopt_plan_profile->cartesian_cost_config.enabled = false;

‎tesseract_examples/src/glass_upright_example.cpp‎

Lines changed: 8 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -202,11 +202,13 @@ bool GlassUprightExample::run()
202202
{
203203
auto composite_profile = std::make_shared<TrajOptIfoptDefaultCompositeProfile>();
204204
composite_profile->collision_cost_config = trajopt_common::TrajOptCollisionConfig(0.01, 20);
205-
composite_profile->collision_cost_config.type = tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE;
205+
composite_profile->collision_cost_config.collision_check_config.type =
206+
tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE;
206207
composite_profile->collision_cost_config.collision_margin_buffer = 0.01;
207208

208209
composite_profile->collision_constraint_config = trajopt_common::TrajOptCollisionConfig(0.01, 20);
209-
composite_profile->collision_constraint_config.type = tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE;
210+
composite_profile->collision_constraint_config.collision_check_config.type =
211+
tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE;
210212
composite_profile->collision_constraint_config.collision_margin_buffer = 0.01;
211213

212214
composite_profile->smooth_velocities = true;
@@ -231,11 +233,13 @@ bool GlassUprightExample::run()
231233
{
232234
auto composite_profile = std::make_shared<TrajOptDefaultCompositeProfile>();
233235
composite_profile->collision_cost_config = trajopt_common::TrajOptCollisionConfig(0.01, 1);
234-
composite_profile->collision_cost_config.type = tesseract_collision::CollisionEvaluatorType::DISCRETE;
236+
composite_profile->collision_cost_config.collision_check_config.type =
237+
tesseract_collision::CollisionEvaluatorType::DISCRETE;
235238
composite_profile->collision_cost_config.collision_margin_buffer = 0.01;
236239

237240
composite_profile->collision_constraint_config = trajopt_common::TrajOptCollisionConfig(0.01, 1);
238-
composite_profile->collision_constraint_config.type = tesseract_collision::CollisionEvaluatorType::DISCRETE;
241+
composite_profile->collision_constraint_config.collision_check_config.type =
242+
tesseract_collision::CollisionEvaluatorType::DISCRETE;
239243
composite_profile->collision_constraint_config.collision_margin_buffer = 0.01;
240244

241245
composite_profile->smooth_velocities = true;

‎tesseract_examples/src/online_planning_example.cpp‎

Lines changed: 10 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -215,15 +215,15 @@ bool OnlinePlanningExample::setupProblem(const std::vector<Eigen::VectorXd>& ini
215215
// Add a collision cost for all steps
216216
double margin_coeff = 10;
217217
double margin = 0.1;
218-
auto collision_config = std::make_shared<trajopt_common::TrajOptCollisionConfig>(margin, margin_coeff);
219-
collision_config->contact_request.type = tesseract_collision::ContactTestType::ALL;
220-
collision_config->type = CollisionEvaluatorType::DISCRETE;
221-
collision_config->collision_margin_buffer = 0.10;
218+
trajopt_common::TrajOptCollisionConfig collision_config(margin, margin_coeff);
219+
collision_config.collision_check_config.contact_request.type = tesseract_collision::ContactTestType::ALL;
220+
collision_config.collision_check_config.type = CollisionEvaluatorType::DISCRETE;
221+
collision_config.collision_margin_buffer = 0.10;
222222

223223
auto collision_cache = std::make_shared<trajopt_ifopt::CollisionCache>(steps_);
224224
if (use_continuous_)
225225
{
226-
std::array<bool, 2> position_vars_fixed{ true, false };
226+
std::array<bool, 2> vars_fixed{ true, false };
227227
for (std::size_t i = 1; i < static_cast<std::size_t>(steps_); i++)
228228
{
229229
auto collision_evaluator = std::make_shared<trajopt_ifopt::LVSDiscreteCollisionEvaluator>(
@@ -233,14 +233,15 @@ bool OnlinePlanningExample::setupProblem(const std::vector<Eigen::VectorXd>& ini
233233
auto collision_constraint =
234234
std::make_shared<trajopt_ifopt::ContinuousCollisionConstraint>(collision_evaluator,
235235
position_vars,
236-
position_vars_fixed,
237-
collision_config->max_num_cnt,
236+
vars_fixed[0],
237+
vars_fixed[1],
238+
collision_config.max_num_cnt,
238239
false,
239240
"LVSDiscreteCollision_" + std::to_string(i));
240241
// nlp_->addCostSet(collision_constraint, trajopt_sqp::CostPenaltyType::HINGE);
241242
nlp_->addConstraintSet(collision_constraint);
242243

243-
position_vars_fixed = { false, false };
244+
vars_fixed = { false, false };
244245
}
245246
}
246247
else
@@ -253,7 +254,7 @@ bool OnlinePlanningExample::setupProblem(const std::vector<Eigen::VectorXd>& ini
253254
auto collision_constraint =
254255
std::make_shared<trajopt_ifopt::DiscreteCollisionConstraint>(collision_evaluator,
255256
vars[i],
256-
collision_config->max_num_cnt,
257+
collision_config.max_num_cnt,
257258
false,
258259
"SingleTimestepCollision_" + std::to_string(i));
259260
// nlp_->addCostSet(collision_constraint, trajopt_sqp::CostPenaltyType::HINGE);

‎tesseract_examples/src/pick_and_place_example.cpp‎

Lines changed: 10 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -237,16 +237,17 @@ bool PickAndPlaceExample::run()
237237

238238
auto trajopt_ifopt_composite_profile = std::make_shared<TrajOptIfoptDefaultCompositeProfile>();
239239
trajopt_ifopt_composite_profile->collision_constraint_config = trajopt_common::TrajOptCollisionConfig(0.0, 10);
240-
trajopt_ifopt_composite_profile->collision_constraint_config.type =
240+
trajopt_ifopt_composite_profile->collision_constraint_config.collision_check_config.type =
241241
tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE;
242+
trajopt_ifopt_composite_profile->collision_constraint_config.collision_check_config.longest_valid_segment_length =
243+
0.05;
242244
trajopt_ifopt_composite_profile->collision_constraint_config.collision_margin_buffer = 0.005;
243-
trajopt_ifopt_composite_profile->collision_constraint_config.longest_valid_segment_length = 0.05;
244245

245246
trajopt_ifopt_composite_profile->collision_cost_config = trajopt_common::TrajOptCollisionConfig(0.005, 50);
246-
trajopt_ifopt_composite_profile->collision_cost_config.type =
247+
trajopt_ifopt_composite_profile->collision_cost_config.collision_check_config.type =
247248
tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE;
249+
trajopt_ifopt_composite_profile->collision_cost_config.collision_check_config.longest_valid_segment_length = 0.05;
248250
trajopt_ifopt_composite_profile->collision_cost_config.collision_margin_buffer = 0.01;
249-
trajopt_ifopt_composite_profile->collision_cost_config.longest_valid_segment_length = 0.05;
250251

251252
trajopt_ifopt_composite_profile->smooth_velocities = true;
252253
trajopt_ifopt_composite_profile->velocity_coeff = 0.1 * Eigen::VectorXd::Ones(1);
@@ -273,15 +274,16 @@ bool PickAndPlaceExample::run()
273274

274275
auto trajopt_composite_profile = std::make_shared<TrajOptDefaultCompositeProfile>();
275276
trajopt_composite_profile->collision_constraint_config = trajopt_common::TrajOptCollisionConfig(0.0, 10);
276-
trajopt_composite_profile->collision_constraint_config.type =
277+
trajopt_composite_profile->collision_constraint_config.collision_check_config.type =
277278
tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE;
279+
trajopt_composite_profile->collision_constraint_config.collision_check_config.longest_valid_segment_length = 0.05;
278280
trajopt_composite_profile->collision_constraint_config.collision_margin_buffer = 0.005;
279-
trajopt_composite_profile->collision_constraint_config.longest_valid_segment_length = 0.05;
280281

281282
trajopt_composite_profile->collision_cost_config = trajopt_common::TrajOptCollisionConfig(0.005, 50);
282-
trajopt_composite_profile->collision_cost_config.type = tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE;
283+
trajopt_composite_profile->collision_cost_config.collision_check_config.type =
284+
tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE;
285+
trajopt_composite_profile->collision_cost_config.collision_check_config.longest_valid_segment_length = 0.05;
283286
trajopt_composite_profile->collision_cost_config.collision_margin_buffer = 0.01;
284-
trajopt_composite_profile->collision_cost_config.longest_valid_segment_length = 0.05;
285287

286288
auto trajopt_solver_profile = std::make_shared<TrajOptOSQPSolverProfile>();
287289
trajopt_solver_profile->opt_params.max_iter = 100;

‎tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp‎

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -241,7 +241,8 @@ bool PuzzlePieceAuxillaryAxesExample::run()
241241
auto trajopt_ifopt_composite_profile = std::make_shared<TrajOptIfoptDefaultCompositeProfile>();
242242
trajopt_ifopt_composite_profile->collision_constraint_config.enabled = false;
243243
trajopt_ifopt_composite_profile->collision_cost_config = trajopt_common::TrajOptCollisionConfig(0.025, 2);
244-
trajopt_ifopt_composite_profile->collision_cost_config.type = tesseract_collision::CollisionEvaluatorType::DISCRETE;
244+
trajopt_ifopt_composite_profile->collision_cost_config.collision_check_config.type =
245+
tesseract_collision::CollisionEvaluatorType::DISCRETE;
245246

246247
trajopt_ifopt_composite_profile->smooth_velocities = false;
247248
trajopt_ifopt_composite_profile->velocity_coeff = Eigen::VectorXd::Ones(1);
@@ -275,7 +276,8 @@ bool PuzzlePieceAuxillaryAxesExample::run()
275276
auto trajopt_composite_profile = std::make_shared<TrajOptDefaultCompositeProfile>();
276277
trajopt_composite_profile->collision_constraint_config.enabled = false;
277278
trajopt_composite_profile->collision_cost_config = trajopt_common::TrajOptCollisionConfig(0.025, 1);
278-
trajopt_composite_profile->collision_cost_config.type = tesseract_collision::CollisionEvaluatorType::DISCRETE;
279+
trajopt_composite_profile->collision_cost_config.collision_check_config.type =
280+
tesseract_collision::CollisionEvaluatorType::DISCRETE;
279281

280282
auto trajopt_solver_profile = std::make_shared<TrajOptOSQPSolverProfile>();
281283
trajopt_solver_profile->settings.adaptive_rho = 0;

‎tesseract_examples/src/puzzle_piece_example.cpp‎

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -233,7 +233,8 @@ bool PuzzlePieceExample::run()
233233
auto trajopt_ifopt_composite_profile = std::make_shared<TrajOptIfoptDefaultCompositeProfile>();
234234
trajopt_ifopt_composite_profile->collision_constraint_config.enabled = false;
235235
trajopt_ifopt_composite_profile->collision_cost_config = trajopt_common::TrajOptCollisionConfig(0.025, 20);
236-
trajopt_ifopt_composite_profile->collision_cost_config.type = tesseract_collision::CollisionEvaluatorType::DISCRETE;
236+
trajopt_ifopt_composite_profile->collision_cost_config.collision_check_config.type =
237+
tesseract_collision::CollisionEvaluatorType::DISCRETE;
237238

238239
trajopt_ifopt_composite_profile->smooth_velocities = false;
239240
trajopt_ifopt_composite_profile->velocity_coeff = Eigen::VectorXd::Ones(1);
@@ -264,7 +265,8 @@ bool PuzzlePieceExample::run()
264265
auto trajopt_composite_profile = std::make_shared<TrajOptDefaultCompositeProfile>();
265266
trajopt_composite_profile->collision_constraint_config.enabled = false;
266267
trajopt_composite_profile->collision_cost_config = trajopt_common::TrajOptCollisionConfig(0.025, 20);
267-
trajopt_composite_profile->collision_cost_config.type = tesseract_collision::CollisionEvaluatorType::DISCRETE;
268+
trajopt_composite_profile->collision_cost_config.collision_check_config.type =
269+
tesseract_collision::CollisionEvaluatorType::DISCRETE;
268270

269271
auto trajopt_solver_profile = std::make_shared<TrajOptOSQPSolverProfile>();
270272
trajopt_solver_profile->opt_params.num_threads = 0;

‎tesseract_motion_planners/core/src/utils.cpp‎

Lines changed: 4 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -349,8 +349,6 @@ bool contactCheckProgram(std::vector<tesseract_collision::ContactResultMap>& con
349349
throw std::runtime_error("contactCheckProgram was given continuous contact manager with a trajectory that only has "
350350
"one state.");
351351

352-
manager.applyContactManagerConfig(config.contact_manager_config);
353-
354352
bool debug_logging = console_bridge::getLogLevel() < console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_INFO;
355353

356354
tesseract_collision::ContactTrajectoryResults::UPtr traj_contacts;
@@ -546,7 +544,7 @@ bool contactCheckProgram(std::vector<tesseract_collision::ContactResultMap>& con
546544
}
547545

548546
tesseract_environment::checkTrajectorySegment(
549-
state_results, manager, state0.link_transforms, state1.link_transforms, config);
547+
state_results, manager, state0.link_transforms, state1.link_transforms, config.contact_request);
550548
if (!state_results.empty())
551549
{
552550
found = true;
@@ -609,7 +607,7 @@ bool contactCheckProgram(std::vector<tesseract_collision::ContactResultMap>& con
609607
}
610608

611609
tesseract_environment::checkTrajectorySegment(
612-
state_results, manager, state0.link_transforms, state1.link_transforms, config);
610+
state_results, manager, state0.link_transforms, state1.link_transforms, config.contact_request);
613611
if (!state_results.empty())
614612
{
615613
found = true;
@@ -656,8 +654,6 @@ bool contactCheckProgram(std::vector<tesseract_collision::ContactResultMap>& con
656654
if (mi.empty())
657655
throw std::runtime_error("contactCheckProgram was given continuous contact manager with empty trajectory.");
658656

659-
manager.applyContactManagerConfig(config.contact_manager_config);
660-
661657
bool debug_logging = console_bridge::getLogLevel() < console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_INFO;
662658

663659
tesseract_collision::ContactTrajectoryResults::UPtr traj_contacts;
@@ -824,7 +820,8 @@ bool contactCheckProgram(std::vector<tesseract_collision::ContactResultMap>& con
824820

825821
tesseract_scene_graph::SceneState state = state_solver.getState(jn, subtraj.row(iSubStep));
826822
sub_state_results.clear();
827-
tesseract_environment::checkTrajectoryState(sub_state_results, manager, state.link_transforms, config);
823+
tesseract_environment::checkTrajectoryState(
824+
sub_state_results, manager, state.link_transforms, config.contact_request);
828825
if (!sub_state_results.empty())
829826
{
830827
found = true;

0 commit comments

Comments
 (0)