diff --git a/FindKDL.cmake b/FindKDL.cmake
index f58a870fd9776bf523b1f7a14a79d45ffa2cc54b..44e132d4af529c02e2416b2a7628b6f4028e216b 100644
--- a/FindKDL.cmake
+++ b/FindKDL.cmake
@@ -2,7 +2,7 @@
 
 MESSAGE(STATUS "Detecting KDL: using orocos_kdl-config.cmake to locate files.")
 
-FIND_PATH(KDL_DIR orocos_kdl-config.cmake /usr/local/share/orocos_kdl/cmake)
+FIND_PATH(KDL_DIR orocos_kdl-config.cmake /usr/local/share/orocos_kdl)
 
 INCLUDE (${KDL_DIR}/orocos_kdl-config.cmake)
 
diff --git a/src/common_obj.h b/src/common_obj.h
index 42b97298428a731b50962185bc5ff07ee3a2504d..361a3157cf73c19157b90a8a58d940c0ae81d93b 100644
--- a/src/common_obj.h
+++ b/src/common_obj.h
@@ -52,7 +52,7 @@ class CArm{
     unsigned int nj;                                                        // number of joints.
     std::vector<CArmJoint> joint_info;                                      // joints info.
     Eigen::MatrixXd jnt_pos;                                                // Joint value.
-    boost::shared_ptr<KDL::ChainFkSolverPos_recursive> jnt_to_pose_solver;  // chain solver.
+    boost::scoped_ptr<KDL::ChainFkSolverPos_recursive> jnt_to_pose_solver;  // chain solver.
     boost::scoped_ptr<KDL::ChainJntToJacSolver> jnt_to_jac_solver;          // chain solver.
     KDL::JntArray jnt_pos_kdl;                                              // Array of arm positions.
     KDL::Jacobian jacobian;                                                 // Jacobian.
@@ -151,4 +151,4 @@ class TaskJac{
 
 } // End of UAM namespace
 
-#endif
\ No newline at end of file
+#endif
diff --git a/src/uam_task_ctrl.cpp b/src/uam_task_ctrl.cpp
index 6bc7f5b9fc32e82da1e09b6d512f3b9af94b94b8..9788c57f5d4ac2ea7014c523fad3fbfea50aa9b0 100644
--- a/src/uam_task_ctrl.cpp
+++ b/src/uam_task_ctrl.cpp
@@ -20,11 +20,6 @@ CHierarchTaskPCtrl::~CHierarchTaskPCtrl()
 {
 }
 
-void CHierarchTaskPCtrl::kktest(std::string& data)
-{
-  std::cout << data << std::endl;
-}
-
 // Main public function
 void CHierarchTaskPCtrl::htpc(const MatrixXd& quad_dist_obs,
                               const goal_obj& goal,
diff --git a/src/uam_task_ctrl.h b/src/uam_task_ctrl.h
index c8a2cb7366f8d532a1be7542f4dc938397817005..117fe7ac71109e0537dd2b71db1e46dbbe1a5bdd 100644
--- a/src/uam_task_ctrl.h
+++ b/src/uam_task_ctrl.h
@@ -92,8 +92,6 @@ class CHierarchTaskPCtrl
               UAM::CCtrlParams& CtrlParams,
               MatrixXd& robot_pos,
               MatrixXd& robot_vel);
-
-    void kktest(std::string& data);
 };
 
 #endif