diff --git a/descartes_light/src/descartes_light/collision_checker.cpp b/descartes_light/src/descartes_light/collision_checker.cpp
index 422ddc7..a74c521 100644
--- a/descartes_light/src/descartes_light/collision_checker.cpp
+++ b/descartes_light/src/descartes_light/collision_checker.cpp
@@ -13,9 +13,10 @@ descartes_light::TesseractCollision::TesseractCollision(tesseract::BasicEnvConst
   }
 
   contact_manager_->setActiveCollisionObjects(kin_group_->getLinkNames());
+  contact_manager_->setContactDistanceThreshold(0);
   contact_manager_->setIsContactAllowedFn(std::bind(&TesseractCollision::isContactAllowed, this, std::placeholders::_1,
                                                     std::placeholders::_2));
-  }
+}
 
 bool descartes_light::TesseractCollision::validate(const double* pos, std::size_t size)
 {
diff --git a/workcell1/workcell1_demos/src/workcell1_demo.cpp b/workcell1/workcell1_demos/src/workcell1_demo.cpp
index 6050bbb..ac72243 100644
--- a/workcell1/workcell1_demos/src/workcell1_demo.cpp
+++ b/workcell1/workcell1_demos/src/workcell1_demo.cpp
@@ -113,19 +113,19 @@ trajopt::TrajOptProbPtr makeProblem(const hybrid_planning_common::EnvironmentDef
   pci.init_info.data = hybrid_planning_common::jointTrajectoryToTrajopt(seed);
 
   // Populate Cost Info
-  auto jv = std::make_shared<trajopt::JointVelCostInfo>();
+  auto jv = std::make_shared<trajopt::JointVelTermInfo>();
   jv->coeffs = std::vector<double>(dof, 2.5);
   jv->name = "joint_vel";
   jv->term_type = trajopt::TT_COST;
   pci.cost_infos.push_back(jv);
 
-  auto ja = std::make_shared<trajopt::JointAccCostInfo>();
+  auto ja = std::make_shared<trajopt::JointAccTermInfo>();
   ja->coeffs = std::vector<double>(dof, 5.0);
   ja->name = "joint_acc";
   ja->term_type = trajopt::TT_COST;
   pci.cost_infos.push_back(ja);
 
-  auto collision = std::make_shared<trajopt::CollisionCostInfo>();
+  auto collision = std::make_shared<trajopt::CollisionTermInfo>();
   collision->name = "collision";
   collision->term_type = trajopt::TT_COST;
   collision->continuous = false;
@@ -156,7 +156,7 @@ trajopt::TrajOptProbPtr makeProblem(const hybrid_planning_common::EnvironmentDef
   // Populate Constraints
   for (std::size_t i = 0; i < pass.size(); ++i)
   {
-    auto pose = std::make_shared<trajopt::StaticPoseCostInfo>();
+    auto pose = std::make_shared<trajopt::CartPoseTermInfo>();
     pose->term_type = trajopt::TT_CNT;
     pose->name = "waypoint_cart_" + std::to_string(i);
     pose->link = "sander_tcp";
diff --git a/workcell2/workcell2_demos/src/workcell2_demo.cpp b/workcell2/workcell2_demos/src/workcell2_demo.cpp
index 53fc25d..eaa458d 100644
--- a/workcell2/workcell2_demos/src/workcell2_demo.cpp
+++ b/workcell2/workcell2_demos/src/workcell2_demo.cpp
@@ -113,19 +113,19 @@ trajopt::TrajOptProbPtr makeProblem(const hybrid_planning_common::EnvironmentDef
   pci.init_info.data = hybrid_planning_common::jointTrajectoryToTrajopt(seed);
 
   // Populate Cost Info
-  auto jv = std::make_shared<trajopt::JointVelCostInfo>();
+  auto jv = std::make_shared<trajopt::JointVelTermInfo>();
   jv->coeffs = std::vector<double>(dof, 2.5);
   jv->name = "joint_vel";
   jv->term_type = trajopt::TT_COST;
   pci.cost_infos.push_back(jv);
 
-  auto ja = std::make_shared<trajopt::JointAccCostInfo>();
+  auto ja = std::make_shared<trajopt::JointAccTermInfo>();
   ja->coeffs = std::vector<double>(dof, 5.0);
   ja->name = "joint_acc";
   ja->term_type = trajopt::TT_COST;
   pci.cost_infos.push_back(ja);
 
-  auto collision = std::make_shared<trajopt::CollisionCostInfo>();
+  auto collision = std::make_shared<trajopt::CollisionTermInfo>();
   collision->name = "collision";
   collision->term_type = trajopt::TT_COST;
   collision->continuous = false;
@@ -156,7 +156,7 @@ trajopt::TrajOptProbPtr makeProblem(const hybrid_planning_common::EnvironmentDef
   // Populate Constraints
   for (std::size_t i = 0; i < pass.size(); ++i)
   {
-    auto pose = std::make_shared<trajopt::StaticPoseCostInfo>();
+    auto pose = std::make_shared<trajopt::CartPoseTermInfo>();
     pose->term_type = trajopt::TT_CNT;
     pose->name = "waypoint_cart_" + std::to_string(i);
     pose->link = "sander_tcp";
diff --git a/workcell3/workcell3_demos/src/workcell3_demo.cpp b/workcell3/workcell3_demos/src/workcell3_demo.cpp
index 32cb02e..0fb536e 100644
--- a/workcell3/workcell3_demos/src/workcell3_demo.cpp
+++ b/workcell3/workcell3_demos/src/workcell3_demo.cpp
@@ -77,19 +77,19 @@ trajopt::TrajOptProbPtr makeProblem(const hybrid_planning_common::EnvironmentDef
   pci.init_info.data = hybrid_planning_common::jointTrajectoryToTrajopt(seed);
 
   // Populate Cost Info
-  auto jv = std::make_shared<trajopt::JointVelCostInfo>();
+  auto jv = std::make_shared<trajopt::JointVelTermInfo>();
   jv->coeffs = std::vector<double>(dof, 2.5);
   jv->name = "joint_vel";
   jv->term_type = trajopt::TT_COST;
   pci.cost_infos.push_back(jv);
 
-  auto ja = std::make_shared<trajopt::JointAccCostInfo>();
+  auto ja = std::make_shared<trajopt::JointAccTermInfo>();
   ja->coeffs = std::vector<double>(dof, 10.0);
   ja->name = "joint_acc";
   ja->term_type = trajopt::TT_COST;
   pci.cost_infos.push_back(ja);
 
-  auto collision = std::make_shared<trajopt::CollisionCostInfo>();
+  auto collision = std::make_shared<trajopt::CollisionTermInfo>();
   collision->name = "collision";
   collision->term_type = trajopt::TT_COST;
   collision->continuous = false;
@@ -112,7 +112,7 @@ trajopt::TrajOptProbPtr makeProblem(const hybrid_planning_common::EnvironmentDef
   {
     // NOTE that here we are using "PoseCostInfo" instead of "StaticPoseCostInfo" as seen in
     // examples 1 & 2. This is because the cell
-    auto pose = std::make_shared<trajopt::PoseCostInfo>();
+    auto pose = std::make_shared<trajopt::DynamicCartPoseTermInfo>();
     pose->term_type = trajopt::TT_CNT;
     pose->name = "waypoint_cart_" + std::to_string(i);
     pose->link = "positioner";
diff --git a/workcell4/workcell4_demos/src/workcell4_demo.cpp b/workcell4/workcell4_demos/src/workcell4_demo.cpp
index c1305f0..5ec926a 100644
--- a/workcell4/workcell4_demos/src/workcell4_demo.cpp
+++ b/workcell4/workcell4_demos/src/workcell4_demo.cpp
@@ -82,20 +82,20 @@ trajopt::TrajOptProbPtr makeProblem(const hybrid_planning_common::EnvironmentDef
   pci.init_info.data = hybrid_planning_common::jointTrajectoryToTrajopt(seed);
 
   // Populate Cost Info
-  auto jv = std::make_shared<trajopt::JointVelCostInfo>();
+  auto jv = std::make_shared<trajopt::JointVelTermInfo>();
   jv->coeffs = std::vector<double>(dof, 1.0);
   jv->coeffs.back() = 0.0;
   jv->name = "joint_vel";
   jv->term_type = trajopt::TT_COST;
   pci.cost_infos.push_back(jv);
 
-  auto ja = std::make_shared<trajopt::JointAccCostInfo>();
+  auto ja = std::make_shared<trajopt::JointAccTermInfo>();
   ja->coeffs = std::vector<double>(dof, 10.0);
   ja->name = "joint_acc";
   ja->term_type = trajopt::TT_COST;
   pci.cost_infos.push_back(ja);
 
-  auto collision = std::make_shared<trajopt::CollisionCostInfo>();
+  auto collision = std::make_shared<trajopt::CollisionTermInfo>();
   collision->name = "collision";
   collision->term_type = trajopt::TT_COST;
   collision->continuous = false;
@@ -115,7 +115,7 @@ trajopt::TrajOptProbPtr makeProblem(const hybrid_planning_common::EnvironmentDef
   // Populate Constraints
   for (std::size_t i = 0; i < pass.size(); ++i)
   {
-    auto pose = std::make_shared<trajopt::PoseCostInfo>();
+    auto pose = std::make_shared<trajopt::DynamicCartPoseTermInfo>();
     pose->term_type = trajopt::TT_CNT;
     pose->name = "waypoint_cart_" + std::to_string(i);
     pose->link = "positioner";