diff --git a/demo/src/pipeline_testbench_main.cpp b/demo/src/pipeline_testbench_main.cpp
index aa12c4d..a3c851a 100644
--- a/demo/src/pipeline_testbench_main.cpp
+++ b/demo/src/pipeline_testbench_main.cpp
@@ -366,8 +366,8 @@ int main(int argc, char** argv)
     RCLCPP_INFO(LOGGER, "Starting Pipeline Testbench example ...");
     for (const auto& motion_plan_req : demo.getMotionPlanRequests())
     {
-      demo.planAndVisualize({ /*{ "ompl", "RRTConnectkConfigDefault" }, { "stomp", "stomp" }, { "drake_ktopt", "" },*/ {
-                                "drake_toppra", "" } },
+      demo.planAndVisualize({ { "ompl", "RRTConnectkConfigDefault" },
+                              { "stomp", "stomp" }, { "drake_ktopt", "" }, { "drake_toppra", "" } },
                             motion_plan_req);
     }
     demo.getVisualTools().prompt("Press 'next' in the RvizVisualToolsGui window to finish the demo");
diff --git a/include/moveit/drake/conversions.hpp b/include/moveit/drake/conversions.hpp
index ba163f5..f4dde28 100644
--- a/include/moveit/drake/conversions.hpp
+++ b/include/moveit/drake/conversions.hpp
@@ -73,13 +73,6 @@ getPiecewisePolynomial(const ::robot_trajectory::RobotTrajectory& robot_trajecto
   std::vector<Eigen::MatrixXd> samples;
   samples.reserve(robot_trajectory.getWayPointCount());
 
-  // Print the first and last point of the trajectory
-  const auto& first_point = robot_trajectory.getWayPoint(0);
-  const auto& last_point = robot_trajectory.getWayPoint(robot_trajectory.getWayPointCount() - 1);
-  Eigen::VectorXd first_position(first_point.getVariableCount());
-  Eigen::VectorXd last_position(last_point.getVariableCount());
-  first_point.copyJointGroupPositions(group, first_position);
-  last_point.copyJointGroupPositions(group, last_position);
   // Create samples & breaks
   for (std::size_t i = 0; i < robot_trajectory.getWayPointCount(); ++i)
   {