diff --git a/CMakeLists.txt b/CMakeLists.txt
index 1829187c4985649c442f9ad54f37319d8cbf2262..b9d506f42acb366fadf6a0b083713e26ff598bee 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -67,7 +67,8 @@ ENDIF(UNIX)
 
 IF (UNIX)
   EXECUTE_PROCESS( COMMAND uname -m COMMAND tr -d '\n' OUTPUT_VARIABLE ARCHITECTURE )
-  SET(CPACK_PACKAGE_FILE_NAME "${PROJECT_NAME}-dev-${ARCHITECTURE}${CPACK_PACKAGE_VERSION}")
+  EXECUTE_PROCESS( COMMAND rosversion -d COMMAND tr -d '\n' OUTPUT_VARIABLE ROS_VERSION )
+  SET(CPACK_PACKAGE_FILE_NAME "${PROJECT_NAME}-dev-${ARCHITECTURE}-ros-${ROS_VERSION}")
   SET(CPACK_PACKAGE_NAME "${PROJECT_NAME}-dev")
   SET(CPACK_PACKAGE_DESCRIPTION_SUMMARY "Part of Angel Santamaria-Navarro libraries. More information at http://www.iri.upc.edu/people/asantamaria")
   SET(CPACK_PACKAGING_INSTALL_PREFIX ${CMAKE_INSTALL_PREFIX})
@@ -75,7 +76,7 @@ IF (UNIX)
   SET(CPACK_DEBIAN_PACKAGE_MAINTAINER "asantamaria@iri.upc.edu")
   SET(CPACK_SET_DESTDIR "ON")  # Necessary because of the absolute install paths
   SET(BOOST_MIN_VERSION 1.48.0.2)
-  SET(CPACK_DEBIAN_PACKAGE_DEPENDS "libboost-all-dev (>= ${BOOST_MIN_VERSION}), libeigen3-dev")
+  SET(CPACK_DEBIAN_PACKAGE_DEPENDS "libboost-all-dev (>= ${BOOST_MIN_VERSION}), libeigen3-dev, ros-${ROS_VERSION}-orocos-kdl")
 
   MESSAGE(STATUS "WARNING: liborocos-kdl-dev must be included as dependency")
 
diff --git a/Finduam_task_ctrl.cmake b/Finduam_task_ctrl.cmake
index 52786dd2d47e9f4f9b3b76af76988ee42ec9a567..04b6e8a65e887ba0bfe8f3b530962c148b799443 100644
--- a/Finduam_task_ctrl.cmake
+++ b/Finduam_task_ctrl.cmake
@@ -1,10 +1,13 @@
 #edit the following line to add the librarie's header files
-FIND_PATH(uam_task_ctrl_INCLUDE_DIR uam_task_ctrl.h /usr/include/iridrivers /usr/local/include/iridrivers)
+FIND_PATH(uam_task_ctrl_INCLUDE_DIR uam_task_ctrl.h /usr/include/iridrivers /usr/include/iridrivers/uam /usr/local/include/iridrivers /usr/local/include/iridrivers/uam)
+# get_filename_component(uam_task_ctrl_INCLUDE_DIR ${uam_task_ctrl_INCLUDE_DIR} DIRECTORY)	
 
 FIND_LIBRARY(uam_task_ctrl_LIBRARY
     NAMES uam_task_ctrl
     PATHS /usr/lib /usr/lib/iridrivers /usr/lib/iridrivers/uam /usr/local/lib /usr/local/lib/iridrivers /usr/local/lib/iridrivers/uam) 
 
+
+
 IF (uam_task_ctrl_INCLUDE_DIR AND uam_task_ctrl_LIBRARY)
    SET(uam_task_ctrl_FOUND TRUE)
 ENDIF (uam_task_ctrl_INCLUDE_DIR AND uam_task_ctrl_LIBRARY)
diff --git a/src/kinematics.cpp b/src/kinematics.cpp
index d4c450387a8b565ee05177208d83fa53b02deb88..28342797b1da6216aea49e47d855b4f414d0af73 100644
--- a/src/kinematics.cpp
+++ b/src/kinematics.cpp
@@ -90,6 +90,97 @@ Eigen::Matrix4d CKinematics::ArmFKine(UAM::CArm& arm)
   return Tarm;
 }
 
+bool CKinematics::ArmIKine(const UAM::CArm& arm, const Eigen::MatrixXd& joint_limits, const Eigen::MatrixXd& Ttarget, const Eigen::MatrixXd& q_pos, Eigen::MatrixXd& q_new)
+{
+  std::cout << "-> Starting Inverse kinematics computation..." << std::endl;
+
+  // Create joint array
+  KDL::JntArray jnt_pos = KDL::JntArray(arm.nj);
+
+  // resizes the joint state vectors in non-realtime
+  jnt_pos.resize(arm.nj);
+
+  // Put current joint positions to kdl jnt_pos   
+  for(unsigned int i=0; i < arm.nj; i++){
+    // jnt_pos.data(i) = q_pos(i,0)*3.1416/180;
+    jnt_pos.data(i) = q_pos(i,0);
+  }
+
+  //fk solver
+  KDL::ChainFkSolverPos_recursive fksolver(KDL::ChainFkSolverPos_recursive(arm.chain));
+
+  // Create joint array
+  KDL::JntArray setpointJP = KDL::JntArray(arm.nj);
+  KDL::JntArray max = KDL::JntArray(arm.nj); //The maximum joint positions
+  KDL::JntArray min = KDL::JntArray(arm.nj); //The minimium joint positions
+
+  // double minjp[7] = {0,0,0,0,0};
+  // double maxjp[7] = {1.57,1.57,1.57,1.57,1.57};
+
+  for(unsigned int ii=0; ii < arm.nj; ii++){
+    // max(i) = maxjp[i];
+    // min(i) = minjp[i];
+    min(ii) = joint_limits(ii,0);
+    max(ii) = joint_limits(ii,1);
+  }
+
+  //Create inverse velocity solver
+  KDL::ChainIkSolverVel_pinv_givens iksolverv = KDL::ChainIkSolverVel_pinv_givens(arm.chain);
+
+  //Iksolver Position: Maximum 2000 iterations, stop at accuracy 1e-6
+  //KDL::ChainIkSolverPos_NR iksolver = KDL::ChainIkSolverPos_NR(arm.chain,fksolver,iksolverv,100,1e-6);
+  KDL::ChainIkSolverPos_NR_JL iksolver = KDL::ChainIkSolverPos_NR_JL(arm.chain, min, max, fksolver, iksolverv, 2000, 1e-6); //With Joints Limits
+
+  KDL::Frame cartpos;
+  KDL::JntArray jointpos = KDL::JntArray(arm.nj);
+
+  // Copying translation to KDL frame
+  for (size_t i = 0; i < 3; ++i)
+  {
+    cartpos.p(i) = Ttarget(i,3); 
+  }
+
+  // Copying Rotation to KDL frame
+  for (size_t j = 0; j < 3; ++j)
+  {
+    for (size_t i = 0; i < 3; ++i)
+       {
+          cartpos.M(i,j) = Ttarget(i,j);
+       }   
+  }
+
+  for(unsigned int i=0; i < arm.nj; i++)
+    jnt_pos(i) = q_pos(i,0);
+
+  // Calculate inverse kinematics to go from jnt_pos to cartpos. The result in jointpos
+  int ret = iksolver.CartToJnt(jnt_pos, cartpos, jointpos);
+
+  if (ret >= 0) {
+    std::cout << " Current Joint Position: [";
+    for(unsigned int i=0; i < arm.nj; i++)
+      std::cout << jnt_pos(i) << " ";
+    std::cout << "]"<< std::endl;
+    std::cout << "Cartesian Position " << cartpos << std::endl;
+    std::cout << "IK result Joint Position: [";
+    for(unsigned int i=0; i < arm.nj; i++)
+      std::cout << jointpos(i) << " ";
+    std::cout << "]"<< std::endl;
+
+    q_new = Eigen::MatrixXd::Zero(arm.nj,1);
+
+    for(unsigned int i=0; i < arm.nj; i++)
+      q_new(i,0) = jointpos(i);
+  } 
+  else 
+  {
+
+    std::cout << "Error: could not calculate inverse kinematics :(" << std::endl;
+    return false;
+
+  }
+  return true;
+}
+
 Eigen::MatrixXd CKinematics::ArmJac(UAM::CArm& arm)
 {
   // constructs the kdl solver in non-realtime
diff --git a/src/kinematics.h b/src/kinematics.h
index 210289901bcff7352d44dad3d7dff9a8d3883091..825f935e57ea6c6c6d7d3361f95c0a9ea80989fe 100644
--- a/src/kinematics.h
+++ b/src/kinematics.h
@@ -15,6 +15,10 @@
 #include <kdl/chainiksolver.hpp>
 #include <kdl/chainiksolverpos_nr.hpp>
 #include <kdl/chainiksolvervel_wdls.hpp>
+#include <kdl/chainiksolverpos_nr_jl.hpp>
+#include <kdl/chainiksolvervel_pinv.hpp>
+#include <kdl/chainiksolvervel_pinv_givens.hpp>
+#include <kdl/chainjnttojacsolver.hpp>
 #include <kdl/chainjnttojacsolver.hpp>
 
 // Own stuff
@@ -53,6 +57,15 @@ class CKinematics
     */      
     static Eigen::Matrix4d ArmFKine(UAM::CArm& arm);
 
+    /**
+    * \brief Compute Arm Forward Inverse Kinematics
+    *
+    * Compute the arm Forward Inverse Kinematics
+    *
+    */  
+    static bool ArmIKine(const UAM::CArm& arm, const Eigen::MatrixXd& joint_limits, const Eigen::MatrixXd& Ttarget, const Eigen::MatrixXd& q_pos, Eigen::MatrixXd& q_new);
+
+
     /**
     * \brief Compute Arm Jacobian
     *
diff --git a/src/uam_task_ctrl.cpp b/src/uam_task_ctrl.cpp
index 367dbc93d9f1894962b57016902f9c8785de8d5e..59de0e6951af769f1a22331b27f8a24000868b9b 100644
--- a/src/uam_task_ctrl.cpp
+++ b/src/uam_task_ctrl.cpp
@@ -19,6 +19,11 @@ 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 d18668c7aee0d02f2930210c107d817764e1fe15..1babdb26340a760baef867f57f181303ce394e78 100644
--- a/src/uam_task_ctrl.h
+++ b/src/uam_task_ctrl.h
@@ -111,6 +111,8 @@ class CHierarchTaskPCtrl
               UAM::CCtrlParams& CtrlParams,
               MatrixXd& robot_pos,
               MatrixXd& robot_vel);
+
+    void kktest(std::string& data);
 };
 
 #endif