Skip to content

Commit 85144b0

Browse files
Levi-ArmstrongLevi-Armstrong
committed
Update to changes with legacy trajopt leveraging trajopt_common::TrajOptCollisionConfig
1 parent 658d8ba commit 85144b0

File tree

19 files changed

+163
-431
lines changed

19 files changed

+163
-431
lines changed

‎tesseract_examples/src/basic_cartesian_example.cpp‎

Lines changed: 9 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -227,8 +227,11 @@ bool BasicCartesianExample::run()
227227
if (ifopt_)
228228
{
229229
auto composite_profile = std::make_shared<TrajOptIfoptDefaultCompositeProfile>();
230-
composite_profile->collision_cost_config->type = tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE;
231-
composite_profile->collision_constraint_config->type = tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE;
230+
composite_profile->collision_cost_config = trajopt_common::TrajOptCollisionConfig(0.025, 20);
231+
composite_profile->collision_cost_config.type = tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE;
232+
composite_profile->collision_constraint_config = trajopt_common::TrajOptCollisionConfig(0.0, 20);
233+
composite_profile->collision_constraint_config.type = tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE;
234+
232235
composite_profile->smooth_velocities = true;
233236
composite_profile->smooth_accelerations = false;
234237
composite_profile->smooth_jerks = false;
@@ -248,8 +251,10 @@ bool BasicCartesianExample::run()
248251
else
249252
{
250253
auto composite_profile = std::make_shared<TrajOptDefaultCompositeProfile>();
251-
composite_profile->collision_cost_config.enabled = true;
252-
composite_profile->collision_constraint_config.enabled = true;
254+
composite_profile->collision_cost_config = trajopt_common::TrajOptCollisionConfig(0.025, 20);
255+
composite_profile->collision_cost_config.type = tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE;
256+
composite_profile->collision_constraint_config = trajopt_common::TrajOptCollisionConfig(0.0, 20);
257+
composite_profile->collision_constraint_config.type = tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE;
253258
composite_profile->smooth_velocities = true;
254259
composite_profile->smooth_accelerations = false;
255260
composite_profile->smooth_jerks = false;

‎tesseract_examples/src/car_seat_example.cpp‎

Lines changed: 20 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -282,27 +282,24 @@ bool CarSeatExample::run()
282282
{
283283
// Create TrajOpt_Ifopt Profile
284284
auto trajopt_ifopt_composite_profile = std::make_shared<TrajOptIfoptDefaultCompositeProfile>();
285-
trajopt_ifopt_composite_profile->collision_constraint_config->type =
285+
trajopt_ifopt_composite_profile->collision_constraint_config = trajopt_common::TrajOptCollisionConfig(0.0, 1);
286+
trajopt_ifopt_composite_profile->collision_constraint_config.type =
286287
tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS;
287-
trajopt_ifopt_composite_profile->collision_constraint_config->contact_manager_config =
288-
tesseract_collision::ContactManagerConfig(0.00);
289-
trajopt_ifopt_composite_profile->collision_constraint_config->collision_margin_buffer = 0.005;
290-
trajopt_ifopt_composite_profile->collision_constraint_config->collision_coeff_data =
291-
trajopt_common::CollisionCoeffData(1);
292-
trajopt_ifopt_composite_profile->collision_cost_config->type =
288+
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;
290+
291+
trajopt_ifopt_composite_profile->collision_cost_config = trajopt_common::TrajOptCollisionConfig(0.005, 50);
292+
trajopt_ifopt_composite_profile->collision_cost_config.type =
293293
tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS;
294-
trajopt_ifopt_composite_profile->collision_cost_config->contact_manager_config =
295-
tesseract_collision::ContactManagerConfig(0.005);
296-
trajopt_ifopt_composite_profile->collision_cost_config->collision_margin_buffer = 0.01;
297-
trajopt_ifopt_composite_profile->collision_cost_config->collision_coeff_data =
298-
trajopt_common::CollisionCoeffData(50);
294+
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;
296+
299297
trajopt_ifopt_composite_profile->smooth_velocities = false;
300298
trajopt_ifopt_composite_profile->velocity_coeff = Eigen::VectorXd::Ones(1);
301299
trajopt_ifopt_composite_profile->smooth_accelerations = true;
302300
trajopt_ifopt_composite_profile->acceleration_coeff = Eigen::VectorXd::Ones(1);
303301
trajopt_ifopt_composite_profile->smooth_jerks = true;
304302
trajopt_ifopt_composite_profile->jerk_coeff = Eigen::VectorXd::Ones(1);
305-
trajopt_ifopt_composite_profile->longest_valid_segment_length = 0.1;
306303

307304
auto trajopt_ifopt_plan_profile = std::make_shared<TrajOptIfoptDefaultPlanProfile>();
308305
trajopt_ifopt_plan_profile->cartesian_cost_config.enabled = false;
@@ -323,13 +320,16 @@ bool CarSeatExample::run()
323320
{
324321
// Create TrajOpt Profile
325322
auto trajopt_composite_profile = std::make_shared<TrajOptDefaultCompositeProfile>();
326-
trajopt_composite_profile->collision_constraint_config.enabled = true;
327-
trajopt_composite_profile->collision_constraint_config.safety_margin = 0.00;
328-
trajopt_composite_profile->collision_constraint_config.safety_margin_buffer = 0.005;
329-
trajopt_composite_profile->collision_constraint_config.coeff = 10;
330-
trajopt_composite_profile->collision_cost_config.safety_margin = 0.005;
331-
trajopt_composite_profile->collision_cost_config.safety_margin_buffer = 0.01;
332-
trajopt_composite_profile->collision_cost_config.coeff = 50;
323+
trajopt_composite_profile->collision_constraint_config = trajopt_common::TrajOptCollisionConfig(0.0, 10);
324+
trajopt_composite_profile->collision_constraint_config.type =
325+
tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS;
326+
trajopt_composite_profile->collision_constraint_config.collision_margin_buffer = 0.005;
327+
trajopt_composite_profile->collision_constraint_config.longest_valid_segment_length = 0.1;
328+
329+
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_margin_buffer = 0.01;
332+
trajopt_composite_profile->collision_cost_config.longest_valid_segment_length = 0.1;
333333

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

‎tesseract_examples/src/freespace_hybrid_example.cpp‎

Lines changed: 2 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -210,10 +210,8 @@ bool FreespaceHybridExample::run()
210210
{
211211
// Create TrajOpt_Ifopt Profile
212212
auto trajopt_ifopt_composite_profile = std::make_shared<TrajOptIfoptDefaultCompositeProfile>();
213-
trajopt_ifopt_composite_profile->collision_constraint_config->contact_manager_config =
214-
tesseract_collision::ContactManagerConfig(0.025);
215-
trajopt_ifopt_composite_profile->collision_cost_config->contact_manager_config =
216-
tesseract_collision::ContactManagerConfig(0.025);
213+
trajopt_ifopt_composite_profile->collision_constraint_config = trajopt_common::TrajOptCollisionConfig(0.025, 20);
214+
trajopt_ifopt_composite_profile->collision_cost_config = trajopt_common::TrajOptCollisionConfig(0.025, 20);
217215
trajopt_ifopt_composite_profile->velocity_coeff = Eigen::VectorXd::Ones(1);
218216
trajopt_ifopt_composite_profile->acceleration_coeff = Eigen::VectorXd::Ones(1);
219217
trajopt_ifopt_composite_profile->jerk_coeff = Eigen::VectorXd::Ones(1);

‎tesseract_examples/src/glass_upright_example.cpp‎

Lines changed: 14 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -201,13 +201,14 @@ bool GlassUprightExample::run()
201201
if (ifopt_)
202202
{
203203
auto composite_profile = std::make_shared<TrajOptIfoptDefaultCompositeProfile>();
204-
composite_profile->collision_cost_config->type = tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE;
205-
composite_profile->collision_cost_config->contact_manager_config = tesseract_collision::ContactManagerConfig(0.01);
206-
composite_profile->collision_cost_config->collision_margin_buffer = 0.01;
207-
composite_profile->collision_constraint_config->type = tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE;
208-
composite_profile->collision_constraint_config->contact_manager_config =
209-
tesseract_collision::ContactManagerConfig(0.01);
210-
composite_profile->collision_constraint_config->collision_margin_buffer = 0.01;
204+
composite_profile->collision_cost_config = trajopt_common::TrajOptCollisionConfig(0.01, 20);
205+
composite_profile->collision_cost_config.type = tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE;
206+
composite_profile->collision_cost_config.collision_margin_buffer = 0.01;
207+
208+
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_margin_buffer = 0.01;
211+
211212
composite_profile->smooth_velocities = true;
212213
composite_profile->smooth_accelerations = false;
213214
composite_profile->smooth_jerks = false;
@@ -229,16 +230,14 @@ bool GlassUprightExample::run()
229230
else
230231
{
231232
auto composite_profile = std::make_shared<TrajOptDefaultCompositeProfile>();
232-
composite_profile->collision_cost_config.enabled = true;
233+
composite_profile->collision_cost_config = trajopt_common::TrajOptCollisionConfig(0.01, 1);
233234
composite_profile->collision_cost_config.type = tesseract_collision::CollisionEvaluatorType::DISCRETE;
234-
composite_profile->collision_cost_config.safety_margin = 0.01;
235-
composite_profile->collision_cost_config.safety_margin_buffer = 0.01;
236-
composite_profile->collision_cost_config.coeff = 1;
237-
composite_profile->collision_constraint_config.enabled = true;
235+
composite_profile->collision_cost_config.collision_margin_buffer = 0.01;
236+
237+
composite_profile->collision_constraint_config = trajopt_common::TrajOptCollisionConfig(0.01, 1);
238238
composite_profile->collision_constraint_config.type = tesseract_collision::CollisionEvaluatorType::DISCRETE;
239-
composite_profile->collision_constraint_config.safety_margin = 0.01;
240-
composite_profile->collision_constraint_config.safety_margin_buffer = 0.01;
241-
composite_profile->collision_constraint_config.coeff = 1;
239+
composite_profile->collision_constraint_config.collision_margin_buffer = 0.01;
240+
242241
composite_profile->smooth_velocities = true;
243242
composite_profile->smooth_accelerations = false;
244243
composite_profile->smooth_jerks = false;

‎tesseract_examples/src/pick_and_place_example.cpp‎

Lines changed: 20 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -236,27 +236,24 @@ bool PickAndPlaceExample::run()
236236
trajopt_ifopt_plan_profile->cartesian_constraint_config.coeff = Eigen::VectorXd::Constant(6, 1, 10);
237237

238238
auto trajopt_ifopt_composite_profile = std::make_shared<TrajOptIfoptDefaultCompositeProfile>();
239-
trajopt_ifopt_composite_profile->collision_constraint_config->type =
239+
trajopt_ifopt_composite_profile->collision_constraint_config = trajopt_common::TrajOptCollisionConfig(0.0, 10);
240+
trajopt_ifopt_composite_profile->collision_constraint_config.type =
240241
tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE;
241-
trajopt_ifopt_composite_profile->collision_constraint_config->contact_manager_config =
242-
tesseract_collision::ContactManagerConfig(0.00);
243-
trajopt_ifopt_composite_profile->collision_constraint_config->collision_margin_buffer = 0.005;
244-
trajopt_ifopt_composite_profile->collision_constraint_config->collision_coeff_data =
245-
trajopt_common::CollisionCoeffData(10);
246-
trajopt_ifopt_composite_profile->collision_cost_config->type =
242+
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;
244+
245+
trajopt_ifopt_composite_profile->collision_cost_config = trajopt_common::TrajOptCollisionConfig(0.005, 50);
246+
trajopt_ifopt_composite_profile->collision_cost_config.type =
247247
tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE;
248-
trajopt_ifopt_composite_profile->collision_cost_config->contact_manager_config =
249-
tesseract_collision::ContactManagerConfig(0.005);
250-
trajopt_ifopt_composite_profile->collision_cost_config->collision_margin_buffer = 0.01;
251-
trajopt_ifopt_composite_profile->collision_cost_config->collision_coeff_data =
252-
trajopt_common::CollisionCoeffData(50);
248+
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;
250+
253251
trajopt_ifopt_composite_profile->smooth_velocities = true;
254252
trajopt_ifopt_composite_profile->velocity_coeff = 0.1 * Eigen::VectorXd::Ones(1);
255253
trajopt_ifopt_composite_profile->smooth_accelerations = true;
256254
trajopt_ifopt_composite_profile->acceleration_coeff = Eigen::VectorXd::Ones(1);
257255
trajopt_ifopt_composite_profile->smooth_jerks = true;
258256
trajopt_ifopt_composite_profile->jerk_coeff = Eigen::VectorXd::Ones(1);
259-
trajopt_ifopt_composite_profile->longest_valid_segment_length = 0.05;
260257

261258
auto trajopt_ifopt_solver_profile = std::make_shared<TrajOptIfoptOSQPSolverProfile>();
262259
trajopt_ifopt_solver_profile->opt_params.max_iterations = 100;
@@ -275,14 +272,16 @@ bool PickAndPlaceExample::run()
275272
trajopt_plan_profile->cartesian_constraint_config.coeff = Eigen::VectorXd::Constant(6, 1, 10);
276273

277274
auto trajopt_composite_profile = std::make_shared<TrajOptDefaultCompositeProfile>();
278-
trajopt_composite_profile->longest_valid_segment_length = 0.05;
279-
trajopt_composite_profile->collision_constraint_config.enabled = true;
280-
trajopt_composite_profile->collision_constraint_config.safety_margin = 0.0;
281-
trajopt_composite_profile->collision_constraint_config.safety_margin_buffer = 0.005;
282-
trajopt_composite_profile->collision_constraint_config.coeff = 10;
283-
trajopt_composite_profile->collision_cost_config.safety_margin = 0.005;
284-
trajopt_composite_profile->collision_cost_config.safety_margin_buffer = 0.01;
285-
trajopt_composite_profile->collision_cost_config.coeff = 50;
275+
trajopt_composite_profile->collision_constraint_config = trajopt_common::TrajOptCollisionConfig(0.0, 10);
276+
trajopt_composite_profile->collision_constraint_config.type =
277+
tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE;
278+
trajopt_composite_profile->collision_constraint_config.collision_margin_buffer = 0.005;
279+
trajopt_composite_profile->collision_constraint_config.longest_valid_segment_length = 0.05;
280+
281+
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_margin_buffer = 0.01;
284+
trajopt_composite_profile->collision_cost_config.longest_valid_segment_length = 0.05;
286285

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

‎tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp‎

Lines changed: 5 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -239,14 +239,10 @@ bool PuzzlePieceAuxillaryAxesExample::run()
239239
trajopt_ifopt_plan_profile->cartesian_constraint_config.coeff(5) = 0;
240240

241241
auto trajopt_ifopt_composite_profile = std::make_shared<TrajOptIfoptDefaultCompositeProfile>();
242-
trajopt_ifopt_composite_profile->collision_constraint_config = nullptr;
243-
trajopt_ifopt_composite_profile->collision_cost_config->type =
244-
tesseract_collision::CollisionEvaluatorType::DISCRETE;
245-
trajopt_ifopt_composite_profile->collision_cost_config->contact_manager_config =
246-
tesseract_collision::ContactManagerConfig(0.025);
247-
trajopt_ifopt_composite_profile->collision_cost_config->collision_margin_buffer = 0.05;
248-
trajopt_ifopt_composite_profile->collision_cost_config->collision_coeff_data =
249-
trajopt_common::CollisionCoeffData(2);
242+
trajopt_ifopt_composite_profile->collision_constraint_config.enabled = false;
243+
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;
245+
250246
trajopt_ifopt_composite_profile->smooth_velocities = false;
251247
trajopt_ifopt_composite_profile->velocity_coeff = Eigen::VectorXd::Ones(1);
252248
trajopt_ifopt_composite_profile->smooth_accelerations = true;
@@ -278,10 +274,8 @@ bool PuzzlePieceAuxillaryAxesExample::run()
278274

279275
auto trajopt_composite_profile = std::make_shared<TrajOptDefaultCompositeProfile>();
280276
trajopt_composite_profile->collision_constraint_config.enabled = false;
281-
trajopt_composite_profile->collision_cost_config.enabled = true;
282-
trajopt_composite_profile->collision_cost_config.safety_margin = 0.025;
277+
trajopt_composite_profile->collision_cost_config = trajopt_common::TrajOptCollisionConfig(0.025, 1);
283278
trajopt_composite_profile->collision_cost_config.type = tesseract_collision::CollisionEvaluatorType::DISCRETE;
284-
trajopt_composite_profile->collision_cost_config.coeff = 1;
285279

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

‎tesseract_examples/src/puzzle_piece_example.cpp‎

Lines changed: 5 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -231,13 +231,10 @@ bool PuzzlePieceExample::run()
231231
trajopt_ifopt_plan_profile->cartesian_constraint_config.coeff(5) = 0;
232232

233233
auto trajopt_ifopt_composite_profile = std::make_shared<TrajOptIfoptDefaultCompositeProfile>();
234-
trajopt_ifopt_composite_profile->collision_constraint_config = nullptr;
235-
trajopt_ifopt_composite_profile->collision_cost_config->type =
236-
tesseract_collision::CollisionEvaluatorType::DISCRETE;
237-
trajopt_ifopt_composite_profile->collision_cost_config->contact_manager_config =
238-
tesseract_collision::ContactManagerConfig(0.025);
239-
trajopt_ifopt_composite_profile->collision_cost_config->collision_coeff_data =
240-
trajopt_common::CollisionCoeffData(20);
234+
trajopt_ifopt_composite_profile->collision_constraint_config.enabled = false;
235+
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;
237+
241238
trajopt_ifopt_composite_profile->smooth_velocities = false;
242239
trajopt_ifopt_composite_profile->velocity_coeff = Eigen::VectorXd::Ones(1);
243240
trajopt_ifopt_composite_profile->smooth_accelerations = true;
@@ -266,10 +263,8 @@ bool PuzzlePieceExample::run()
266263

267264
auto trajopt_composite_profile = std::make_shared<TrajOptDefaultCompositeProfile>();
268265
trajopt_composite_profile->collision_constraint_config.enabled = false;
269-
trajopt_composite_profile->collision_cost_config.enabled = true;
270-
trajopt_composite_profile->collision_cost_config.safety_margin = 0.025;
266+
trajopt_composite_profile->collision_cost_config = trajopt_common::TrajOptCollisionConfig(0.025, 20);
271267
trajopt_composite_profile->collision_cost_config.type = tesseract_collision::CollisionEvaluatorType::DISCRETE;
272-
trajopt_composite_profile->collision_cost_config.coeff = 20;
273268

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

‎tesseract_motion_planners/trajopt/CMakeLists.txt‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -5,7 +5,6 @@ find_package(osqp REQUIRED)
55
# Trajopt Planner
66
add_library(
77
${PROJECT_NAME}_trajopt
8-
src/trajopt_collision_config.cpp
98
src/trajopt_motion_planner.cpp
109
src/trajopt_utils.cpp
1110
src/trajopt_waypoint_config.cpp

0 commit comments

Comments
 (0)