diff --git a/tesseract_examples/src/pick_and_place_example.cpp b/tesseract_examples/src/pick_and_place_example.cpp
index a29bd82747..26fdf0d8ee 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<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;
diff --git a/tesseract_examples/test/car_seat_example_unit.cpp b/tesseract_examples/test/car_seat_example_unit.cpp
index 50b55d1981..d72287d311 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<TesseractSupportResourceLocator>();
   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<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());
 }
 
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 0000000000..88317ffea2
--- /dev/null
+++ b/tesseract_examples/test/freespace_hybrid_example_unit.cpp
@@ -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();
+}
diff --git a/tesseract_examples/test/pick_and_place_example_unit.cpp b/tesseract_examples/test/pick_and_place_example_unit.cpp
index ea7ba8e3c2..e7f2d9662d 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<TesseractSupportResourceLocator>();
   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<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());
 }
 
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 44c33a1396..cd12f6becb 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<TesseractSupportResourceLocator>();
   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<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());
 }
 
diff --git a/tesseract_examples/test/puzzle_piece_example_unit.cpp b/tesseract_examples/test/puzzle_piece_example_unit.cpp
index 09a1dc6756..978fa6691e 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<TesseractSupportResourceLocator>();
   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<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());
 }