Skip to content

Commit

Permalink
Added unit tests for ifopt examples
Browse files Browse the repository at this point in the history
  • Loading branch information
@rjoomen
rjoomen committed Jan 10, 2024
1 parent 7538d7c commit ff8389f
Show file tree
Hide file tree
Showing 6 changed files with 121 additions and 13 deletions.
10 changes: 5 additions & 5 deletions tesseract_examples/src/pick_and_place_example.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -210,24 +210,24 @@ bool PickAndPlaceExample::run()
{
// Create TrajOpt_Ifopt Profile
auto trajopt_ifopt_plan_profile = std::make_shared<TrajOptIfoptDefaultPlanProfile>();
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<TrajOptIfoptDefaultCompositeProfile>();
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;
Expand Down
19 changes: 17 additions & 2 deletions tesseract_examples/test/car_seat_example_unit.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<TesseractSupportResourceLocator>();
tesseract_common::fs::path urdf_path =
Expand All @@ -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<TesseractSupportResourceLocator>();
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<Environment>();
if (!env->init(urdf_path, srdf_path, locator))
exit(1);

CarSeatExample example(env, nullptr, true, false);
EXPECT_TRUE(example.run());
}

Expand Down
48 changes: 48 additions & 0 deletions tesseract_examples/test/freespace_hybrid_example_unit.cpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,48 @@
#include <tesseract_common/macros.h>
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <gtest/gtest.h>
TESSERACT_COMMON_IGNORE_WARNINGS_POP

#include <tesseract_examples/freespace_hybrid_example.h>
#include <tesseract_support/tesseract_support_resource_locator.h>

using namespace tesseract_examples;
using namespace tesseract_common;
using namespace tesseract_environment;

TEST(TesseractExamples, FreespaceHybridTrajOptExampleUnit) // NOLINT
{
auto locator = std::make_shared<TesseractSupportResourceLocator>();
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<Environment>();
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<TesseractSupportResourceLocator>();
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<Environment>();
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();
}
19 changes: 17 additions & 2 deletions tesseract_examples/test/pick_and_place_example_unit.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<TesseractSupportResourceLocator>();
tesseract_common::fs::path urdf_path =
Expand All @@ -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<TesseractSupportResourceLocator>();
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<Environment>();
if (!env->init(urdf_path, srdf_path, locator))
exit(1);

PickAndPlaceExample example(env, nullptr, true, false);
EXPECT_TRUE(example.run());
}

Expand Down
19 changes: 17 additions & 2 deletions tesseract_examples/test/puzzle_piece_auxillary_axes_example_unit.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<TesseractSupportResourceLocator>();
tesseract_common::fs::path urdf_path =
Expand All @@ -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<TesseractSupportResourceLocator>();
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<Environment>();
if (!env->init(urdf_path, srdf_path, locator))
exit(1);

PuzzlePieceAuxillaryAxesExample example(env, nullptr, true, false);
EXPECT_TRUE(example.run());
}

Expand Down
19 changes: 17 additions & 2 deletions tesseract_examples/test/puzzle_piece_example_unit.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<TesseractSupportResourceLocator>();
tesseract_common::fs::path urdf_path =
Expand All @@ -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<TesseractSupportResourceLocator>();
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<Environment>();
if (!env->init(urdf_path, srdf_path, locator))
exit(1);

PuzzlePieceExample example(env, nullptr, true, false);
EXPECT_TRUE(example.run());
}

Expand Down

0 comments on commit ff8389f

Please sign in to comment.