diff --git a/CMakeLists.txt b/CMakeLists.txt
index 21b2efd..ac31c5d 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -12,16 +12,18 @@ set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
 
 find_package(Eigen3 REQUIRED)
 find_package(OpenCV REQUIRED)
-find_package(Boost 1.67.0 COMPONENTS filesystem system iostreams REQUIRED)
-# find_package(Ceres REQUIRED)
+find_package(Boost 1.67.0 COMPONENTS filesystem system iostreams program_options REQUIRED)
+find_package(Ceres REQUIRED)
+find_package(ompl REQUIRED)
 
 include_directories(${EIGEN3_INCLUDE_DIR})
 include_directories(include)
+include_directories(${OMPL_INCLUDE_DIRS})
+
 
 include_directories(/usr/local/include)
 link_directories(/usr/local/lib)
 
-
 # Create library
 ## Declare a cpp library
 add_library(${PROJECT_NAME}_core
@@ -32,26 +34,24 @@ add_library(${PROJECT_NAME}_core
     src/localization/ukf.cpp
     src/localization/particle_filter.cpp
 
-    # # path tracking
-    # src/path_tracking/mpc.cpp
-    # src/path_tracking/move_to_pose.cpp
-    # src/path_tracking/stanley_control.cpp
+    # path tracking
+    src/path_tracking/mpc.cpp
+    src/path_tracking/move_to_pose.cpp
+    src/path_tracking/stanley_control.cpp
 
-    # # path planning
-    # src/path_planning/potential_field.cpp
-    # src/path_planning/quintic_polynomial.cpp
+    # path planning
+    src/path_planning/potential_field.cpp
+    src/path_planning/quintic_polynomial.cpp
     src/path_planning/trajectory_optimizer.cpp
     src/path_planning/state_lattice.cpp
-    # src/path_planning/cubic_spline.cpp
-    # src/path_planning/dwa.cpp
-    # src/path_planning/prm.cpp
-    # src/path_planning/grid_search.cpp
-    # src/path_planning/rrt.cpp
-    # src/path_planning/rrt_star.cpp
+    src/path_planning/cubic_spline.cpp
+    src/path_planning/dwa.cpp
+    src/path_planning/prm.cpp
+    src/path_planning/grid_search.cpp
+    src/path_planning/rrt.cpp
+    src/path_planning/rrt_star.cpp
 )
 
-
-
 # ##############################
 # #Localization
 # ##############################
@@ -70,53 +70,57 @@ target_link_libraries(particle_filter ${Boost_LIBRARIES} ${PROJECT_NAME}_core)
 # #PathPlanning
 # ##############################
 
-# add_executable(dijkstra examples/path_planning/example_dijkstra.cpp)
-# target_link_libraries(dijkstra ${Boost_LIBRARIES} ${PROJECT_NAME}_core)
+add_executable(dijkstra examples/path_planning/example_dijkstra.cpp)
+target_link_libraries(dijkstra ${Boost_LIBRARIES} ${PROJECT_NAME}_core)
 
-# add_executable(astar examples/path_planning/example_astar.cpp)
-# target_link_libraries(astar ${Boost_LIBRARIES} ${PROJECT_NAME}_core)
+add_executable(astar examples/path_planning/example_astar.cpp)
+target_link_libraries(astar ${Boost_LIBRARIES} ${PROJECT_NAME}_core)
 
-# add_executable(prm examples/path_planning/example_prm.cpp) # use opencv for kdtree
-# target_link_libraries(prm ${OpenCV_LIBS} ${Boost_LIBRARIES} ${PROJECT_NAME}_core)
+add_executable(prm examples/path_planning/example_prm.cpp) # use opencv for kdtree
+target_link_libraries(prm ${OpenCV_LIBS} ${Boost_LIBRARIES} ${PROJECT_NAME}_core)
 
-# add_executable(rrt examples/path_planning/example_rrt.cpp)
-# target_link_libraries(rrt ${Boost_LIBRARIES} ${PROJECT_NAME}_core)
+add_executable(rrt examples/path_planning/example_rrt.cpp)
+target_link_libraries(rrt ${Boost_LIBRARIES} ${PROJECT_NAME}_core)
 
-# add_executable(rrt_star examples/path_planning/example_rrt_star.cpp)
-# target_link_libraries(rrt_star ${Boost_LIBRARIES} ${PROJECT_NAME}_core)
+add_executable(rrt_star examples/path_planning/example_rrt_star.cpp)
+target_link_libraries(rrt_star ${Boost_LIBRARIES} ${PROJECT_NAME}_core)
 
-# add_executable(potential_field examples/path_planning/example_potential_field.cpp)
-# target_link_libraries(potential_field ${Boost_LIBRARIES} ${PROJECT_NAME}_core)
+add_executable(potential_field examples/path_planning/example_potential_field.cpp)
+target_link_libraries(potential_field ${Boost_LIBRARIES} ${PROJECT_NAME}_core)
 
-# add_executable(dwa examples/path_planning/example_dwa.cpp)
-# target_link_libraries(dwa ${Boost_LIBRARIES} ${PROJECT_NAME}_core)
+add_executable(dwa examples/path_planning/example_dwa.cpp)
+target_link_libraries(dwa ${Boost_LIBRARIES} ${PROJECT_NAME}_core)
 
-# add_executable(quintic_polynomial examples/path_planning/example_quintic_polynomial.cpp)
-# target_link_libraries(quintic_polynomial ${Boost_LIBRARIES} ${PROJECT_NAME}_core)
+add_executable(quintic_polynomial examples/path_planning/example_quintic_polynomial.cpp)
+target_link_libraries(quintic_polynomial ${Boost_LIBRARIES} ${PROJECT_NAME}_core)
 
-# add_executable(cubic_spline examples/path_planning/example_cubic_spline.cpp)
-# target_link_libraries(cubic_spline ${Boost_LIBRARIES}  ${PROJECT_NAME}_core)
+add_executable(cubic_spline examples/path_planning/example_cubic_spline.cpp)
+target_link_libraries(cubic_spline ${Boost_LIBRARIES}  ${PROJECT_NAME}_core)
 
 add_executable(trajectory_optimizer examples/path_planning/example_trajectory_optimizer.cpp)
 target_link_libraries(trajectory_optimizer ${Boost_LIBRARIES} ${PROJECT_NAME}_core)
 
-# add_executable(lookup_table_generation examples/path_planning/example_lookup_table_generation.cpp)
-# target_link_libraries(lookup_table_generation ${Boost_LIBRARIES} ${PROJECT_NAME}_core)
+add_executable(lookup_table_generation examples/path_planning/example_lookup_table_generation.cpp)
+target_link_libraries(lookup_table_generation ${Boost_LIBRARIES} ${PROJECT_NAME}_core)
 
 add_executable(state_lattice examples/path_planning/example_state_lattice.cpp)
 target_link_libraries(state_lattice ${Boost_LIBRARIES} ${PROJECT_NAME}_core)
 
 
+add_executable(dubins examples/path_planning/example_dubins.cpp)
+target_link_libraries(dubins ${Boost_LIBRARIES} ${OMPL_LIBRARIES}  ${PROJECT_NAME}_core)
+
+
 # ##############################
 # #PathTracking
 # ##############################
 
-# add_executable(move_to_pose examples/path_tracking/example_move_to_pose.cpp)
-# target_link_libraries(move_to_pose ${Boost_LIBRARIES} ${PROJECT_NAME}_core)
+add_executable(move_to_pose examples/path_tracking/example_move_to_pose.cpp)
+target_link_libraries(move_to_pose ${Boost_LIBRARIES} ${PROJECT_NAME}_core)
 
-# add_executable(stanley_control examples/path_tracking/example_stanley_control.cpp )
-# target_link_libraries(stanley_control ${Boost_LIBRARIES} ${PROJECT_NAME}_core)
+add_executable(stanley_control examples/path_tracking/example_stanley_control.cpp )
+target_link_libraries(stanley_control ${Boost_LIBRARIES} ${PROJECT_NAME}_core)
 
-# add_executable(mpc examples/path_tracking/example_mpc.cpp)
-# target_link_libraries(mpc ${Boost_LIBRARIES} ${PROJECT_NAME}_core ipopt)
+add_executable(mpc examples/path_tracking/example_mpc.cpp)
+target_link_libraries(mpc ${Boost_LIBRARIES} ${PROJECT_NAME}_core ipopt)
 
diff --git a/Dockerfile b/Dockerfile
index a29f45c..4436204 100644
--- a/Dockerfile
+++ b/Dockerfile
@@ -12,9 +12,14 @@ RUN wget https://github.com/Kitware/CMake/releases/download/v3.18.2/cmake-3.18.2
 ENV PATH="/usr/bin/cmake/bin:${PATH}"
 
 # deps
-RUN apt-get update &&\
+RUN apt-get update  &&\
     apt-get install -y --no-install-recommends \
-    libboost-all-dev libopencv-dev python3-opencv libeigen3-dev cppad gnuplot &&\
+    libboost-all-dev\
+    libopencv-dev\
+    python3-opencv\
+    libeigen3-dev\
+    cppad\
+    gnuplot &&\
     apt-get clean
 
 # ipopt
@@ -30,6 +35,11 @@ RUN tar zxf ceres-solver-2.0.0.tar.gz
 RUN mkdir ceres-bin
 RUN cd ceres-bin && cmake ../ceres-solver-2.0.0 && make -j3 && make install
 
+# ompl
+RUN git clone https://github.com/ompl/ompl.git
+RUN cd ompl && mkdir -p build/Release && cd build/Release \
+    && cmake ../.. &&  make install -j 4
+
 FROM deps_stage AS build_stage
 
 COPY CMakeLists.txt /root/LearnRoboticsCpp/CMakeLists.txt
@@ -38,7 +48,7 @@ COPY include /root/LearnRoboticsCpp/include
 COPY examples /root/LearnRoboticsCpp/examples
 
 WORKDIR /root/LearnRoboticsCpp
-# RUN mkdir build && cd build && cmake .. && make -j 4
+RUN mkdir build && cd build && cmake .. && make -j 4
 
+ENV LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/lib
 
-# ENTRYPOINT [ "bash", "-c" ]
diff --git a/README.md b/README.md
index 806919d..b7a6e30 100644
--- a/README.md
+++ b/README.md
@@ -41,52 +41,37 @@ Tested on Ubuntu 18.04
 
 
 ## Running with Docker
+The Docker image is about 3GB.
+
+Deployment
 ```
-$ sudo docker build -f Dockerfile -t learn_robotics_cpp .
-$ sudo docker run --mount type=bind,source="$(pwd)",target=/root/LearnRoboticsCpp -it learn_robotics_cpp
+$ sudo docker build -f Dockerfile -t ctfchan/learn-robotics-cpp:latest .
+$ sudo docker push ctfchan/learn-robotics-cpp:latest
 ```
 
-## Dependencies Installation
-1. apt installs
+Make sure `-it ctfchan/learn-robotics-cpp` goes last when you do `docker run`.
 ```
-sudo apt update
-sudo apt install build-essential
-sudo apt install cmake
-sudo apt install gnuplot
-sudo apt install libboost-all-dev
-sudo apt install libopencv-dev python3-opencv
-sudo apt install libeigen3-dev
-sudo apt install cppad
+$ sudo docker pull ctfchan/learn-robotics-cpp
+$ sudo docker run --name learn-robotics-cpp --mount type=bind,source="$(pwd)",target=/root/LearnRoboticsCpp -it ctfchan/learn-robotics-cpp
 ```
 
-2. ipoptd install
+From inside the Docker
 ```
-sudo apt install gfortran
-sudo apt install unzip
+$ cd ~/LearnRoboticsCpp
+$ ./bin/state_lattice # or whatever executable you want to run
 ```
+The images will show up in the `animations` directory.
 
-You probably want to `cd` into another directory like `Downloads` for this folder
+`docker stop` when you're done. `docker rm` when you want to get rid of the container to start over or something.
 ```
-wget https://www.coin-or.org/download/source/Ipopt/Ipopt-3.12.7.zip && unzip Ipopt-3.12.7.zip && rm Ipopt-3.12.7.zip
+$ sudo docker stop learn-robotics-cpp
+$ sudo docker rm learn-robotics-cpp
 ```
-Call `install_ipopt.sh` in this repo with sudo and the source directory as the first argument (e.g. `sudo bash install_ipopt.sh ~/Ipopt-3.12.7`)
-
-3. `Xming` (for Windows Subsystem Linux users only)
-Need to install xming for gnuplot visualization: https://sourceforge.net/projects/xming/.
-Set display to `gnuplot` with: `$ export DISPLAY=:0 gnuplot`.
-
 
-4. Install Ceres
-
-## Build
-```console
-$ git clone https://github.com/CtfChan/LearnRoboticsCpp.git
-$ mkdir build
-$ cd build
-$ cmake ../
-$ make -j4
+`docker exec` to run it after you stop it.
+```
+$ sudo docker exec learn-robotics-cpp bash
 ```
-
 
 ## Path Planning
 ### DWA
diff --git a/examples/localization/example_ukf.cpp b/examples/localization/example_ukf.cpp
index 157954e..1dc73be 100644
--- a/examples/localization/example_ukf.cpp
+++ b/examples/localization/example_ukf.cpp
@@ -14,7 +14,7 @@ int main(){
     float beta = 2.0f; // for gaussian distribution, beta=2 is optimal
     float kappa = 0.0f;
 
-    // control input (v, w) will be constant 
+    // control input (v, w) will be constant
     Eigen::Vector2f u = Eigen::Vector2f(1.0f, 0.1f);
 
     // noisy control for dr
@@ -46,7 +46,7 @@ int main(){
     R(1,1) = 1.0;
 
     float control_noise_v = 1.0;
-    float control_noise_w = std::pow(deg2rad(30.0), 2); 
+    float control_noise_w = std::pow(deg2rad(30.0), 2);
 
     float obs_noise_x =  0.5 * 0.5;
     float obs_noise_y = 0.5 * 0.5;
@@ -108,7 +108,7 @@ int main(){
         gp.send1d(obs_path);
         gp.send1d(error_ellipse);
         gp.send1d(est_path);
-        // sleep(0.2);        
+        // sleep(0.2);
 
     }
 
diff --git a/examples/path_planning/example_dubins.cpp b/examples/path_planning/example_dubins.cpp
new file mode 100644
index 0000000..1b264e5
--- /dev/null
+++ b/examples/path_planning/example_dubins.cpp
@@ -0,0 +1,55 @@
+#include "common.hpp"
+#include "gnuplot-iostream.h"
+
+#include <ompl/base/ScopedState.h>
+#include <ompl/base/spaces/DubinsStateSpace.h>
+#include <ompl/geometric/SimpleSetup.h>
+#include <boost/program_options.hpp>
+
+namespace ob = ompl::base;
+namespace og = ompl::geometric;
+
+int main() {
+    ob::StateSpacePtr space(std::make_shared<ob::DubinsStateSpace>());
+    ob::ScopedState<> from(space), to(space), s(space);
+
+    from[0] = 0.0;
+    from[1] = 0.0;
+    from[2] = 0.0;
+
+    to[0] = 1.0;
+    to[1] = 3.0;
+    to[2] = 1.0;
+
+    Gnuplot gp;
+
+    gp << "set term png\n";
+    gp << "set output '../animations/dubins.png'\n";
+
+    // plot start and goal
+    gp << "plot '-' with vector title 'start' lw 2,"
+          "'-' with vector title 'goal' lw 2,"
+          "'-' with vector title 'trajectory' lw 2\n";
+
+    gp.send1d(poseToVector(from[0], from[1], from[2], 0.5));
+    gp.send1d(poseToVector(to[0], to[1], to[2], 0.5));
+
+    // plot trajectory
+    std::vector<std::tuple<float, float, float, float>> traj;
+    const unsigned int num_pts = 50;
+    std::vector<double> reals;
+
+    std::cout << "distance: " << space->distance(from(), to()) << "\npath:\n";
+    for (unsigned int i = 0; i <= num_pts; ++i) {
+        space->interpolate(from(), to(), (double)i / num_pts, s());
+        reals = s.reals();
+        float x = static_cast<float>(reals[0]);
+        float y = static_cast<float>(reals[1]);
+        float theta = static_cast<float>(reals[2]);
+        auto arrow = poseToVector(x, y, theta, 0.2f);
+        traj.push_back(arrow[0]);
+    }
+    gp.send1d(traj);
+
+    gp << "set output\n";
+}
\ No newline at end of file
diff --git a/examples/path_planning/example_trajectory_optimizer.cpp b/examples/path_planning/example_trajectory_optimizer.cpp
index 0701464..261b73e 100644
--- a/examples/path_planning/example_trajectory_optimizer.cpp
+++ b/examples/path_planning/example_trajectory_optimizer.cpp
@@ -2,26 +2,26 @@
 #include "path_planning/trajectory_optimizer.hpp"
 
 int main() {
-  Gnuplot gp;
+    Gnuplot gp;
 
-  // trajectory is robot centric, so start of optimization always at (0, 0, 0)
-  // (x, y, yaw, v, wheel_base, max_steer)
-  BicycleModelRobot state{0.f, 0.f, 0.f, 3.0f, 1.0f, deg2rad(90.0f)};
+    // trajectory is robot centric, so start of optimization always at (0, 0, 0)
+    // (x, y, yaw, v, wheel_base, max_steer)
+    BicycleModelRobot state{0.f, 0.f, 0.f, 3.0f, 1.0f, deg2rad(90.0f)};
 
-  // initialize optimizer
-  size_t max_iter = 100;
-  float cost_th = 0.1f;
-  TrajectoryParam delta_param{0.5f, 0.02f, 0.02f};
-  float ds = 0.1f;  // discretization of path length for trajectory generation
-  TrajectoryOptimizer optim(state, max_iter, cost_th, delta_param, ds);
+    // initialize optimizer
+    size_t max_iter = 100;
+    float cost_th = 0.1f;
+    TrajectoryParam delta_param{0.5f, 0.02f, 0.02f};
+    float ds = 0.1f;  // discretization of path length for trajectory generation
+    TrajectoryOptimizer optim(state, max_iter, cost_th, delta_param, ds);
 
-  // do optimization
-  Pose2D target_pose{5.0f, 2.0f, deg2rad(90.0f)};
-  TrajectoryParam init_param{
-      6.0f, 0.0f, 0.0f};  // p = (path len, middle_steer, final_steer)
-  float k0 = 0.0f;        // initial steer
-  Pose2DTrajectory traj =
-      optim.optimizeTrajectory(target_pose, init_param, k0, gp);
+    // do optimization
+    Pose2D target_pose{5.0f, 2.0f, deg2rad(90.0f)};
+    TrajectoryParam init_param{
+        6.0f, 0.0f, 0.0f};  // p = (path len, middle_steer, final_steer)
+    float k0 = 0.0f;        // initial steer
+    Pose2DTrajectory traj =
+        optim.optimizeTrajectory(target_pose, init_param, k0, gp);
 
-  return 0;
+    return 0;
 }
\ No newline at end of file
diff --git a/src/common.cpp b/src/common.cpp
index b558322..f17d5d4 100644
--- a/src/common.cpp
+++ b/src/common.cpp
@@ -6,19 +6,19 @@
 
 #include <Eigen/Eigen>
 
-BicycleModelRobot::BicycleModelRobot(float _x, float _y, float _yaw, 
-                float _v, float _L, float _max_steer, 
+BicycleModelRobot::BicycleModelRobot(float _x, float _y, float _yaw,
+                float _v, float _L, float _max_steer,
                 float _min_speed, float _max_speed, float _max_accel,
                 bool _normalize_yaw ) :
-                x(_x), y(_y), yaw(_yaw), v(_v), L(_L), 
+                x(_x), y(_y), yaw(_yaw), v(_v), L(_L),
                 max_steer(_max_steer),
-                min_speed(_min_speed), max_speed(_max_speed), 
+                min_speed(_min_speed), max_speed(_max_speed),
                 max_accel(_max_accel), normalize_yaw(_normalize_yaw) {}
 
 
 void BicycleModelRobot::update(float acc, float delta, float dt) {
     delta = std::clamp(delta, -max_steer, max_steer);
-    
+
     x += v * std::cos(yaw) * dt;
     y += v * std::sin(yaw) * dt;
     yaw += v / L * std::tan(delta) * dt;
@@ -40,7 +40,7 @@ float deg2rad(float deg) {
 float normalizeAngle(float angle) {
     while (angle > M_PI)
         angle-= 2.0f * M_PI;
-    while (angle < -M_PI) 
+    while (angle < -M_PI)
         angle += 2.0f*M_PI;
     return angle;
 }
@@ -56,7 +56,7 @@ std::array<float, 3> quadraticCoefficients(std::array<float, 3>& x, std::array<f
     b << y[0], y[1], y[2];
 
     Eigen::Vector3f coeffs = A.inverse() * b;
-    
+
     return {coeffs[0] , coeffs[1], coeffs[2]};
 }
 
@@ -76,8 +76,6 @@ Arrow poseToVector(float x, float y, float theta, float r) {
 }
 
 
-
-
 Arrow trajectoryToVector(Pose2DTrajectory& traj) {
     Arrow arr;
 
diff --git a/src/path_planning/state_lattice.cpp b/src/path_planning/state_lattice.cpp
index f9388c4..77caa86 100644
--- a/src/path_planning/state_lattice.cpp
+++ b/src/path_planning/state_lattice.cpp
@@ -7,8 +7,8 @@
 
 std::vector<Pose2D> sampleStates(std::vector<float>& angle_samples, float a_min,
                  float a_max, float d, float p_max, float p_min, size_t nh) {
-    std::vector<Pose2D> states; 
-    
+    std::vector<Pose2D> states;
+
     float xf, yf, yawf;
     for (float i : angle_samples) {
         float a = a_min + (a_max - a_min) * i;
@@ -17,7 +17,7 @@ std::vector<Pose2D> sampleStates(std::vector<float>& angle_samples, float a_min,
             yf = d * std::sin(a);
             if (nh == 1)
                 yawf = (p_max - p_min) / 2.f + a;
-            else 
+            else
                 yawf = p_min + (p_max - p_min) * j / (nh - 1.f) + a;
             Pose2D final_state {xf, yf, yawf};
             states.push_back(final_state);
@@ -36,7 +36,7 @@ std::vector<Pose2D> calculateUniformPolarStates(size_t nxy, size_t nh, float d,
         angle_samples[i] = i / (nxy - 1.f);
 
     std::vector<Pose2D> states = sampleStates(angle_samples, a_min, a_max, d, p_max, p_min, nh);
-    
+
     return states;
 }
 
@@ -64,7 +64,7 @@ std::vector<Pose2D> calculateBiasedPolarStates(float goal_angle, int ns, int nxy
     std::vector<float> cumsum_cnav(cnav.size());
     std::partial_sum(cnav.begin(), cnav.end(), cumsum_cnav.begin());
 
-    // output angles 
+    // output angles
     std::vector<float> di;
     int li = 0;
     for (int i = 0; i < nxy; ++i ) {
@@ -76,15 +76,15 @@ std::vector<Pose2D> calculateBiasedPolarStates(float goal_angle, int ns, int nxy
             }
         }
     }
-    
+
     auto states = sampleStates(di, a_min, a_max, d, p_max, p_min, nh);
-    return states;  
+    return states;
 }
 
 
 
 std::vector<Pose2D> calculateLaneStates(
-    float l_center, float l_heading, float l_width, 
+    float l_center, float l_heading, float l_width,
     float v_width, float d, size_t nxy) {
     float xc = d * std::cos(l_heading) + l_center * std::sin(l_heading);
     float yc = d * std::sin(l_heading) + l_center * std::cos(l_heading);
@@ -105,7 +105,7 @@ std::vector<Pose2D> calculateLaneStates(
 
 std::pair<std::vector<TableEntry>, Arrow> generatePaths(TrajectoryOptimizer& optim, std::vector<TableEntry>& lookup,
                                      std::vector<Pose2D>& states, float k0) {
-    
+
     // track all valid end points states
     std::vector<TableEntry> table;
 
diff --git a/src/path_planning/trajectory_optimizer.cpp b/src/path_planning/trajectory_optimizer.cpp
index 85cb8fd..a7d6924 100644
--- a/src/path_planning/trajectory_optimizer.cpp
+++ b/src/path_planning/trajectory_optimizer.cpp
@@ -41,9 +41,6 @@ Pose2DTrajectory TrajectoryOptimizer::optimizeTrajectory(Pose2D &target_pose,
     Pose2D final_pose{traj.x.back(), traj.y.back(), traj.theta.back()};
     Eigen::Vector3f d = calculatePoseErrorVector(target_pose, final_pose);
     float cost = d.norm();
-    // std::cout << "final_pose: " << final_pose.x << " " << final_pose.y << " "
-    // << final_pose.theta << std::endl; std::cout << "current cost " << cost <<
-    // std::endl;
 
     gp << "plot '-' with line \n";
     gp.send1d(boost::make_tuple(traj.x, traj.y));
@@ -60,13 +57,6 @@ Pose2DTrajectory TrajectoryOptimizer::optimizeTrajectory(Pose2D &target_pose,
     params.s += alpha * dp(0);
     params.km += alpha * dp(1);
     params.kf += alpha * dp(2);
-
-    // std::cout << "alpha: " << alpha << std::endl;
-    // std::cout << "params: " << params.s  << " " << params.km << " " <<
-    // params.kf << std::endl;
-
-    // std::cout << "dp: " << dp << std::endl;
-    // std::cout << "jacob: " << J  << std::endl;
   }
 
   return traj;