From ff8389fd72ca439639827be8749fbc9f2ce33272 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Wed, 10 Jan 2024 09:33:40 +0100 Subject: [PATCH] Added unit tests for ifopt examples --- .../src/pick_and_place_example.cpp | 10 ++-- .../test/car_seat_example_unit.cpp | 19 +++++++- .../test/freespace_hybrid_example_unit.cpp | 48 +++++++++++++++++++ .../test/pick_and_place_example_unit.cpp | 19 +++++++- ...zzle_piece_auxillary_axes_example_unit.cpp | 19 +++++++- .../test/puzzle_piece_example_unit.cpp | 19 +++++++- 6 files changed, 121 insertions(+), 13 deletions(-) create mode 100644 tesseract_examples/test/freespace_hybrid_example_unit.cpp diff --git a/tesseract_examples/src/pick_and_place_example.cpp b/tesseract_examples/src/pick_and_place_example.cpp index a29bd82747f..26fdf0d8eeb 100644 --- a/tesseract_examples/src/pick_and_place_example.cpp +++ b/tesseract_examples/src/pick_and_place_example.cpp @@ -210,24 +210,24 @@ bool PickAndPlaceExample::run() { // Create TrajOpt_Ifopt Profile auto trajopt_ifopt_plan_profile = std::make_shared(); - trajopt_ifopt_plan_profile->cartesian_coeff = Eigen::VectorXd::Ones(6); + trajopt_ifopt_plan_profile->cartesian_coeff = Eigen::VectorXd::Constant(6, 1, 10); trajopt_ifopt_plan_profile->joint_coeff = Eigen::VectorXd::Ones(7); auto trajopt_ifopt_composite_profile = std::make_shared(); trajopt_ifopt_composite_profile->collision_constraint_config->type = - tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS; + tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE; trajopt_ifopt_composite_profile->collision_constraint_config->contact_manager_config = tesseract_collision::ContactManagerConfig(0.00); trajopt_ifopt_composite_profile->collision_constraint_config->collision_margin_buffer = 0.005; trajopt_ifopt_composite_profile->collision_constraint_config->collision_coeff_data = - trajopt_common::CollisionCoeffData(1); + trajopt_common::CollisionCoeffData(10); trajopt_ifopt_composite_profile->collision_cost_config->type = - tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS; + tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE; trajopt_ifopt_composite_profile->collision_cost_config->contact_manager_config = tesseract_collision::ContactManagerConfig(0.005); trajopt_ifopt_composite_profile->collision_cost_config->collision_margin_buffer = 0.01; trajopt_ifopt_composite_profile->collision_cost_config->collision_coeff_data = - trajopt_common::CollisionCoeffData(5); + trajopt_common::CollisionCoeffData(50); trajopt_ifopt_composite_profile->smooth_velocities = false; trajopt_ifopt_composite_profile->velocity_coeff = Eigen::VectorXd::Ones(1); trajopt_ifopt_composite_profile->smooth_accelerations = true; diff --git a/tesseract_examples/test/car_seat_example_unit.cpp b/tesseract_examples/test/car_seat_example_unit.cpp index 50b55d19818..d72287d3119 100644 --- a/tesseract_examples/test/car_seat_example_unit.cpp +++ b/tesseract_examples/test/car_seat_example_unit.cpp @@ -10,7 +10,7 @@ using namespace tesseract_examples; using namespace tesseract_common; using namespace tesseract_environment; -TEST(TesseractExamples, CarSeatCppExampleUnit) // NOLINT +TEST(TesseractExamples, CarSeatCppTrajOptExampleUnit) // NOLINT { auto locator = std::make_shared(); tesseract_common::fs::path urdf_path = @@ -21,7 +21,22 @@ TEST(TesseractExamples, CarSeatCppExampleUnit) // NOLINT if (!env->init(urdf_path, srdf_path, locator)) exit(1); - CarSeatExample example(env, nullptr); + CarSeatExample example(env, nullptr, false, false); + EXPECT_TRUE(example.run()); +} + +TEST(TesseractExamples, CarSeatCppTrajOptIfoptExampleUnit) // NOLINT +{ + auto locator = std::make_shared(); + tesseract_common::fs::path urdf_path = + locator->locateResource("package://tesseract_support/urdf/car_seat_demo.urdf")->getFilePath(); + tesseract_common::fs::path srdf_path = + locator->locateResource("package://tesseract_support/urdf/car_seat_demo.srdf")->getFilePath(); + auto env = std::make_shared(); + if (!env->init(urdf_path, srdf_path, locator)) + exit(1); + + CarSeatExample example(env, nullptr, true, false); EXPECT_TRUE(example.run()); } diff --git a/tesseract_examples/test/freespace_hybrid_example_unit.cpp b/tesseract_examples/test/freespace_hybrid_example_unit.cpp new file mode 100644 index 00000000000..88317ffea28 --- /dev/null +++ b/tesseract_examples/test/freespace_hybrid_example_unit.cpp @@ -0,0 +1,48 @@ +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include +#include + +using namespace tesseract_examples; +using namespace tesseract_common; +using namespace tesseract_environment; + +TEST(TesseractExamples, FreespaceHybridTrajOptExampleUnit) // NOLINT +{ + auto locator = std::make_shared(); + tesseract_common::fs::path urdf_path = + locator->locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.urdf")->getFilePath(); + tesseract_common::fs::path srdf_path = + locator->locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.srdf")->getFilePath(); + auto env = std::make_shared(); + if (!env->init(urdf_path, srdf_path, locator)) + exit(1); + + FreespaceHybridExample example(env, nullptr, false, false); + EXPECT_TRUE(example.run()); +} + +TEST(TesseractExamples, FreespaceHybridTrajOptIfoptExampleUnit) // NOLINT +{ + auto locator = std::make_shared(); + tesseract_common::fs::path urdf_path = + locator->locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.urdf")->getFilePath(); + tesseract_common::fs::path srdf_path = + locator->locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.srdf")->getFilePath(); + auto env = std::make_shared(); + if (!env->init(urdf_path, srdf_path, locator)) + exit(1); + + FreespaceHybridExample example(env, nullptr, true, false); + EXPECT_TRUE(example.run()); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} diff --git a/tesseract_examples/test/pick_and_place_example_unit.cpp b/tesseract_examples/test/pick_and_place_example_unit.cpp index ea7ba8e3c2d..e7f2d9662dd 100644 --- a/tesseract_examples/test/pick_and_place_example_unit.cpp +++ b/tesseract_examples/test/pick_and_place_example_unit.cpp @@ -10,7 +10,7 @@ using namespace tesseract_examples; using namespace tesseract_common; using namespace tesseract_environment; -TEST(TesseractExamples, PickAndPlaceCppExampleUnit) // NOLINT +TEST(TesseractExamples, PickAndPlaceCppTrajOptExampleUnit) // NOLINT { auto locator = std::make_shared(); tesseract_common::fs::path urdf_path = @@ -21,7 +21,22 @@ TEST(TesseractExamples, PickAndPlaceCppExampleUnit) // NOLINT if (!env->init(urdf_path, srdf_path, locator)) exit(1); - PickAndPlaceExample example(env, nullptr); + PickAndPlaceExample example(env, nullptr, false, false); + EXPECT_TRUE(example.run()); +} + +TEST(TesseractExamples, PickAndPlaceCppTrajOptIfoptExampleUnit) // NOLINT +{ + auto locator = std::make_shared(); + tesseract_common::fs::path urdf_path = + locator->locateResource("package://tesseract_support/urdf/pick_and_place_plan.urdf")->getFilePath(); + tesseract_common::fs::path srdf_path = + locator->locateResource("package://tesseract_support/urdf/pick_and_place_plan.srdf")->getFilePath(); + auto env = std::make_shared(); + if (!env->init(urdf_path, srdf_path, locator)) + exit(1); + + PickAndPlaceExample example(env, nullptr, true, false); EXPECT_TRUE(example.run()); } diff --git a/tesseract_examples/test/puzzle_piece_auxillary_axes_example_unit.cpp b/tesseract_examples/test/puzzle_piece_auxillary_axes_example_unit.cpp index 44c33a13966..cd12f6becb0 100644 --- a/tesseract_examples/test/puzzle_piece_auxillary_axes_example_unit.cpp +++ b/tesseract_examples/test/puzzle_piece_auxillary_axes_example_unit.cpp @@ -10,7 +10,7 @@ using namespace tesseract_examples; using namespace tesseract_common; using namespace tesseract_environment; -TEST(TesseractExamples, DISABLED_PuzzlePieceAuxillaryAxesCppExampleUnit) // NOLINT +TEST(TesseractExamples, DISABLED_PuzzlePieceAuxillaryAxesCppTrajOptExampleUnit) // NOLINT { auto locator = std::make_shared(); tesseract_common::fs::path urdf_path = @@ -21,7 +21,22 @@ TEST(TesseractExamples, DISABLED_PuzzlePieceAuxillaryAxesCppExampleUnit) // NOL if (!env->init(urdf_path, srdf_path, locator)) exit(1); - PuzzlePieceAuxillaryAxesExample example(env, nullptr); + PuzzlePieceAuxillaryAxesExample example(env, nullptr, false, false); + EXPECT_TRUE(example.run()); +} + +TEST(TesseractExamples, DISABLED_PuzzlePieceAuxillaryAxesCppTrajOptIfoptExampleUnit) // NOLINT +{ + auto locator = std::make_shared(); + tesseract_common::fs::path urdf_path = + locator->locateResource("package://tesseract_support/urdf/puzzle_piece_workcell.urdf")->getFilePath(); + tesseract_common::fs::path srdf_path = + locator->locateResource("package://tesseract_support/urdf/puzzle_piece_workcell.srdf")->getFilePath(); + auto env = std::make_shared(); + if (!env->init(urdf_path, srdf_path, locator)) + exit(1); + + PuzzlePieceAuxillaryAxesExample example(env, nullptr, true, false); EXPECT_TRUE(example.run()); } diff --git a/tesseract_examples/test/puzzle_piece_example_unit.cpp b/tesseract_examples/test/puzzle_piece_example_unit.cpp index 09a1dc67569..978fa6691e4 100644 --- a/tesseract_examples/test/puzzle_piece_example_unit.cpp +++ b/tesseract_examples/test/puzzle_piece_example_unit.cpp @@ -10,7 +10,7 @@ using namespace tesseract_examples; using namespace tesseract_common; using namespace tesseract_environment; -TEST(TesseractExamples, PuzzlePieceCppExampleUnit) // NOLINT +TEST(TesseractExamples, PuzzlePieceCppTrajOptExampleUnit) // NOLINT { auto locator = std::make_shared(); tesseract_common::fs::path urdf_path = @@ -21,7 +21,22 @@ TEST(TesseractExamples, PuzzlePieceCppExampleUnit) // NOLINT if (!env->init(urdf_path, srdf_path, locator)) exit(1); - PuzzlePieceExample example(env, nullptr); + PuzzlePieceExample example(env, nullptr, false, false); + EXPECT_TRUE(example.run()); +} + +TEST(TesseractExamples, DISABLED_PuzzlePieceCppTrajOptIfoptExampleUnit) // NOLINT +{ + auto locator = std::make_shared(); + tesseract_common::fs::path urdf_path = + locator->locateResource("package://tesseract_support/urdf/puzzle_piece_workcell.urdf")->getFilePath(); + tesseract_common::fs::path srdf_path = + locator->locateResource("package://tesseract_support/urdf/puzzle_piece_workcell.srdf")->getFilePath(); + auto env = std::make_shared(); + if (!env->init(urdf_path, srdf_path, locator)) + exit(1); + + PuzzlePieceExample example(env, nullptr, true, false); EXPECT_TRUE(example.run()); }