diff --git a/qr_head_tracking/CMakeLists.txt b/qr_head_tracking/CMakeLists.txt
index 13152711d608f902e2140b513c008f35613ad8b2..2d5f9cad7a81c6a76a55a2ea4b43bf68f4150baa 100644
--- a/qr_head_tracking/CMakeLists.txt
+++ b/qr_head_tracking/CMakeLists.txt
@@ -6,7 +6,7 @@ find_package(catkin REQUIRED)
 # ******************************************************************** 
 #                 Add catkin additional components here
 # ******************************************************************** 
-find_package(catkin REQUIRED COMPONENTS iri_base_algorithm sensor_msgs trajectory_msgs actionlib humanoid_common_msgs)
+find_package(catkin REQUIRED COMPONENTS iri_base_algorithm humanoid_common_msgs humanoid_modules)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
@@ -15,6 +15,7 @@ find_package(catkin REQUIRED COMPONENTS iri_base_algorithm sensor_msgs trajector
 #           Add system and labrobotica dependencies here
 # ******************************************************************** 
 # find_package(<dependency> REQUIRED)
+find_package(iriutils REQUIRED)
 
 # ******************************************************************** 
 #           Add topic, service and action definition here
@@ -60,7 +61,7 @@ catkin_package(
 # ******************************************************************** 
 #            Add ROS and IRI ROS run time dependencies
 # ******************************************************************** 
- CATKIN_DEPENDS iri_base_algorithm sensor_msgs trajectory_msgs actionlib humanoid_common_msgs
+  CATKIN_DEPENDS iri_base_algorithm humanoid_common_msgs humanoid_modules
 # ******************************************************************** 
 #      Add system and labrobotica run time dependencies here
 # ******************************************************************** 
@@ -76,6 +77,7 @@ catkin_package(
 # ******************************************************************** 
 include_directories(include)
 include_directories(${catkin_INCLUDE_DIRS})
+include_directories(${iriutils_INCLUDE_DIR})
 # include_directories(${<dependency>_INCLUDE_DIR})
 
 ## Declare a cpp library
@@ -88,6 +90,7 @@ add_executable(${PROJECT_NAME} src/qr_head_tracking_alg.cpp src/qr_head_tracking
 #                   Add the libraries
 # ******************************************************************** 
 target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
+target_link_libraries(${PROJECT_NAME} ${iriutils_LIBRARY})
 # target_link_libraries(${PROJECT_NAME} ${<dependency>_LIBRARY})
 
 # ******************************************************************** 
diff --git a/qr_head_tracking/include/qr_head_tracking_alg_node.h b/qr_head_tracking/include/qr_head_tracking_alg_node.h
index 1ba898ad28189ff8f5648a9ebcef4ed8cbc89431..fead9edf43e89ab30f6e407221624c87de055526 100644
--- a/qr_head_tracking/include/qr_head_tracking_alg_node.h
+++ b/qr_head_tracking/include/qr_head_tracking_alg_node.h
@@ -28,20 +28,15 @@
 #include <iri_base_algorithm/iri_base_algorithm.h>
 #include "qr_head_tracking_alg.h"
 
+#include <humanoid_modules/head_tracking_module.h>
+
 // [publisher subscriber headers]
 #include <sensor_msgs/JointState.h>
-#include <trajectory_msgs/JointTrajectoryPoint.h>
 #include <humanoid_common_msgs/tag_pose_array.h>
 
 // [service client headers]
-#include <humanoid_common_msgs/set_servo_modules.h>
-#include <humanoid_common_msgs/get_pid.h>
-#include <humanoid_common_msgs/set_pid.h>
 
 // [action server client headers]
-#include <actionlib/client/simple_action_client.h>
-#include <actionlib/client/terminal_state.h>
-#include <humanoid_common_msgs/humanoid_follow_targetAction.h>
 
 /**
  * \brief IRI ROS Specific Algorithm Class
@@ -51,8 +46,6 @@ class QrHeadTrackingAlgNode : public algorithm_base::IriBaseAlgorithm<QrHeadTrac
 {
   private:
     // [publisher attributes]
-    ros::Publisher head_target_publisher_;
-    trajectory_msgs::JointTrajectoryPoint head_target_JointTrajectoryPoint_msg_;
 
     // [subscriber attributes]
     ros::Subscriber joint_states_subscriber_;
@@ -70,30 +63,10 @@ class QrHeadTrackingAlgNode : public algorithm_base::IriBaseAlgorithm<QrHeadTrac
     // [service attributes]
 
     // [client attributes]
-    ros::ServiceClient set_servo_modules_client_;
-    humanoid_common_msgs::set_servo_modules set_servo_modules_srv_;
-
-    ros::ServiceClient get_tilt_pid_client_;
-    humanoid_common_msgs::get_pid get_tilt_pid_srv_;
-
-    ros::ServiceClient get_pan_pid_client_;
-    humanoid_common_msgs::get_pid get_pan_pid_srv_;
-
-    ros::ServiceClient set_tilt_pid_client_;
-    humanoid_common_msgs::set_pid set_tilt_pid_srv_;
-
-    ros::ServiceClient set_pan_pid_client_;
-    humanoid_common_msgs::set_pid set_pan_pid_srv_;
 
     // [action server attributes]
 
     // [action client attributes]
-    actionlib::SimpleActionClient<humanoid_common_msgs::humanoid_follow_targetAction> head_follow_target_client_;
-    humanoid_common_msgs::humanoid_follow_targetGoal head_follow_target_goal_;
-    bool head_follow_targetMakeActionRequest();
-    void head_follow_targetDone(const actionlib::SimpleClientGoalState& state,  const humanoid_common_msgs::humanoid_follow_targetResultConstPtr& result);
-    void head_follow_targetActive();
-    void head_follow_targetFeedback(const humanoid_common_msgs::humanoid_follow_targetFeedbackConstPtr& feedback);
 
    /**
     * \brief config variable
@@ -102,6 +75,7 @@ class QrHeadTrackingAlgNode : public algorithm_base::IriBaseAlgorithm<QrHeadTrac
     * Is updated everytime function config_update() is called.
     */
     Config config_;
+    CHeadTrackingModule tracking_module;
 
     bool tracking;
     double pan_angle;
diff --git a/qr_head_tracking/package.xml b/qr_head_tracking/package.xml
index 8546d91a7f9c148193fef6c3ed0e53404c23d873..a1ff095d0ca9535921d891d84a5447d81127ee31 100644
--- a/qr_head_tracking/package.xml
+++ b/qr_head_tracking/package.xml
@@ -41,20 +41,17 @@
   <!--   <test_depend>gtest</test_depend> -->
   <buildtool_depend>catkin</buildtool_depend>
   <build_depend>iri_base_algorithm</build_depend>
-  <build_depend>sensor_msgs</build_depend>
-  <build_depend>trajectory_msgs</build_depend>
-  <build_depend>actionlib</build_depend>
+  <build_depend>humanoid_modules</build_depend>
   <build_depend>humanoid_common_msgs</build_depend>
+  <build_depend>sensor_msgs</build_depend>
   <run_depend>iri_base_algorithm</run_depend>
-  <run_depend>sensor_msgs</run_depend>
-  <run_depend>trajectory_msgs</run_depend>
-  <run_depend>actionlib</run_depend>
+  <run_depend>humanoid_modules</run_depend>
   <run_depend>humanoid_common_msgs</run_depend>
-
+  <run_depend>sensor_msgs</run_depend>
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>
     <!-- Other tools can request additional information be placed here -->
 
   </export>
-</package>
\ No newline at end of file
+</package>
diff --git a/qr_head_tracking/src/qr_head_tracking_alg_node.cpp b/qr_head_tracking/src/qr_head_tracking_alg_node.cpp
index aa7fcf8a31cc816b70ab44c43dfa04e90466f921..ce5e19c881dbb565fbf02cebe570417a6b647c8b 100644
--- a/qr_head_tracking/src/qr_head_tracking_alg_node.cpp
+++ b/qr_head_tracking/src/qr_head_tracking_alg_node.cpp
@@ -2,13 +2,12 @@
 
 QrHeadTrackingAlgNode::QrHeadTrackingAlgNode(void) :
   algorithm_base::IriBaseAlgorithm<QrHeadTrackingAlgorithm>(),
-  head_follow_target_client_("head_follow_target", true)
+  tracking_module("head_tracking_client")
 {
   //init class attributes if necessary
   //this->loop_rate_ = 2;//in [Hz]
 
   // [init publishers]
-  this->head_target_publisher_ = this->public_node_handle_.advertise<trajectory_msgs::JointTrajectoryPoint>("head_target", 1);
   
   // [init subscribers]
   this->joint_states_subscriber_ = this->public_node_handle_.subscribe("joint_states", 1, &QrHeadTrackingAlgNode::joint_states_callback, this);
@@ -20,15 +19,6 @@ QrHeadTrackingAlgNode::QrHeadTrackingAlgNode(void) :
   // [init services]
   
   // [init clients]
-  set_servo_modules_client_ = this->public_node_handle_.serviceClient<humanoid_common_msgs::set_servo_modules>("set_servo_modules");
-
-  get_tilt_pid_client_ = this->public_node_handle_.serviceClient<humanoid_common_msgs::get_pid>("get_tilt_pid");
-
-  get_pan_pid_client_ = this->public_node_handle_.serviceClient<humanoid_common_msgs::get_pid>("get_pan_pid");
-
-  set_tilt_pid_client_ = this->public_node_handle_.serviceClient<humanoid_common_msgs::set_pid>("set_tilt_pid");
-
-  set_pan_pid_client_ = this->public_node_handle_.serviceClient<humanoid_common_msgs::set_pid>("set_pan_pid");
 
   // [init action servers]
   
@@ -39,13 +29,6 @@ QrHeadTrackingAlgNode::QrHeadTrackingAlgNode(void) :
   this->current_pan_angle=0.0;
   this->tilt_angle=0.0;
   this->current_tilt_angle=0.0;
-  this->head_target_JointTrajectoryPoint_msg_.positions.resize(2);
-  this->set_servo_modules_srv_.request.names.resize(2);
-  this->set_servo_modules_srv_.request.modules.resize(2);
-  this->set_servo_modules_srv_.request.names[0]="j_pan";
-  this->set_servo_modules_srv_.request.modules[0]="head";
-  this->set_servo_modules_srv_.request.names[1]="j_tilt";
-  this->set_servo_modules_srv_.request.modules[1]="head";
 }
 
 QrHeadTrackingAlgNode::~QrHeadTrackingAlgNode(void)
@@ -57,52 +40,16 @@ QrHeadTrackingAlgNode::~QrHeadTrackingAlgNode(void)
 
 void QrHeadTrackingAlgNode::mainNodeThread(void)
 {
-  static bool first=true;
-
   // [fill msg structures]
   
   // [fill srv structure and make request to the server]
 
-  if(first)
-  {
-    ROS_INFO("QrHeadTrackingAlgNode:: Sending New Request!");
-    if (get_tilt_pid_client_.call(get_tilt_pid_srv_))
-    {
-      this->config_.tilt_p=this->get_tilt_pid_srv_.response.p;
-      this->config_.tilt_i=this->get_tilt_pid_srv_.response.i;
-      this->config_.tilt_d=this->get_tilt_pid_srv_.response.d;
-      this->config_.tilt_i_clamp = this->get_tilt_pid_srv_.response.i_clamp;
-    }
-    else
-    {
-      ROS_INFO("QrHeadTrackingAlgNode:: Failed to Call Server on topic get_tilt_pid ");
-    }
-
-    ROS_INFO("QrHeadTrackingAlgNode:: Sending New Request!");
-    if (get_pan_pid_client_.call(get_pan_pid_srv_))
-    {
-      this->config_.pan_p=this->get_pan_pid_srv_.response.p;
-      this->config_.pan_i=this->get_pan_pid_srv_.response.i;
-      this->config_.pan_d=this->get_pan_pid_srv_.response.d;
-      this->config_.pan_i_clamp = this->get_pan_pid_srv_.response.i_clamp;
-    }
-    else
-    {
-      ROS_INFO("QrHeadTrackingAlgNode:: Failed to Call Server on topic get_pan_pid ");
-    }
-    first=false;
-  }
-
   // [fill action structure and make request to the action server]
 
   // [publish messages]
   this->alg_.lock();
   if(this->tracking)
-  {
-    this->head_target_JointTrajectoryPoint_msg_.positions[0]=this->pan_angle;
-    this->head_target_JointTrajectoryPoint_msg_.positions[1]=this->tilt_angle;
-    this->head_target_publisher_.publish(this->head_target_JointTrajectoryPoint_msg_);
-  }
+    this->tracking_module.update_target(this->pan_angle,this->tilt_angle);
   this->alg_.unlock();
 }
 
@@ -111,7 +58,6 @@ void QrHeadTrackingAlgNode::joint_states_callback(const sensor_msgs::JointState:
 {
   unsigned int i;
 
-  ROS_INFO("QrHeadTrackingAlgNode::joint_states_callback: New Message Received");
   //use appropiate mutex to shared variables if necessary
   //this->alg_.lock();
   this->joint_states_mutex_enter();
@@ -124,7 +70,7 @@ void QrHeadTrackingAlgNode::joint_states_callback(const sensor_msgs::JointState:
       this->current_tilt_angle=(msg->position[i]);
     }
   }
-  ROS_DEBUG("angle_pan: %f angle_tilt: %f",this->current_pan_angle,this->current_tilt_angle);
+  ROS_INFO("angle_pan: %f angle_tilt: %f",this->current_pan_angle,this->current_tilt_angle);
   //unlock previously blocked shared variables
   //this->alg_.unlock();
   this->joint_states_mutex_exit();
@@ -140,6 +86,7 @@ void QrHeadTrackingAlgNode::joint_states_mutex_exit(void)
   pthread_mutex_unlock(&this->joint_states_mutex_);
 }
 
+
 void QrHeadTrackingAlgNode::qr_pose_callback(const humanoid_common_msgs::tag_pose_array::ConstPtr& msg)
 {
   ROS_INFO("QrHeadTrackingAlgNode::qr_pose_callback: New Message Received");
@@ -168,84 +115,8 @@ void QrHeadTrackingAlgNode::qr_pose_mutex_exit(void)
 /*  [service callbacks] */
 
 /*  [action callbacks] */
-void QrHeadTrackingAlgNode::head_follow_targetDone(const actionlib::SimpleClientGoalState& state,  const humanoid_common_msgs::humanoid_follow_targetResultConstPtr& result)
-{
-  alg_.lock();
-  if( state == actionlib::SimpleClientGoalState::SUCCEEDED )
-    ROS_INFO("QrHeadTrackingAlgNode::head_follow_targetDone: Goal Achieved!");
-  else
-    ROS_INFO("QrHeadTrackingAlgNode::head_follow_targetDone: %s", state.toString().c_str());
-  this->tracking=false;
-
-  //copy & work with requested result
-  alg_.unlock();
-}
-
-void QrHeadTrackingAlgNode::head_follow_targetActive()
-{
-  alg_.lock();
-  //ROS_INFO("QrHeadTrackingAlgNode::head_follow_targetActive: Goal just went active!");
-  alg_.unlock();
-}
-
-void QrHeadTrackingAlgNode::head_follow_targetFeedback(const humanoid_common_msgs::humanoid_follow_targetFeedbackConstPtr& feedback)
-{
-  alg_.lock();
-  //ROS_INFO("QrHeadTrackingAlgNode::head_follow_targetFeedback: Got Feedback!");
-
-  bool feedback_is_ok = true;
-  ROS_INFO("Current angles: pan %f of %f, tilt %f of %f",feedback->current_pan,this->pan_angle,feedback->current_tilt,this->tilt_angle);
-
-  //analyze feedback
-  //my_var = feedback->var;
-
-  //if feedback is not what expected, cancel requested goal
-  if( !feedback_is_ok )
-  {
-    head_follow_target_client_.cancelGoal();
-    //ROS_INFO("QrHeadTrackingAlgNode::head_follow_targetFeedback: Cancelling Action!");
-  }
-  alg_.unlock();
-}
 
 /*  [action requests] */
-bool QrHeadTrackingAlgNode::head_follow_targetMakeActionRequest()
-{
-  // IMPORTANT: Please note that all mutex used in the client callback functions
-  // must be unlocked before calling any of the client class functions from an
-  // other thread (MainNodeThread).
-  // this->alg_.unlock();
-  if(this->set_servo_modules_client_.call(this->set_servo_modules_srv_))
-  {
-    this->head_follow_target_client_.waitForServer();
-    //ROS_DEBUG("QrHeadTrackingAlgNode::head_follow_targetMakeActionRequest: Server is Available!");
-    //send a goal to the action server
-    //head_follow_target_goal_.data = my_desired_goal;
-    this->head_follow_target_client_.waitForServer();
-    this->head_follow_target_goal_.target_pan=this->pan_angle;
-    this->head_follow_target_goal_.target_tilt=this->tilt_angle;
-    this->head_follow_target_goal_.pan_range.resize(2);
-    this->head_follow_target_goal_.pan_range[0]=this->config_.max_pan;
-    this->head_follow_target_goal_.pan_range[1]=this->config_.min_pan;
-    this->head_follow_target_goal_.tilt_range.resize(2);
-    this->head_follow_target_goal_.tilt_range[0]=this->config_.max_tilt;
-    this->head_follow_target_goal_.tilt_range[1]=this->config_.min_tilt;
-    this->head_follow_target_client_.sendGoal(head_follow_target_goal_,
-                boost::bind(&QrHeadTrackingAlgNode::head_follow_targetDone,     this, _1, _2),
-                boost::bind(&QrHeadTrackingAlgNode::head_follow_targetActive,   this),
-                boost::bind(&QrHeadTrackingAlgNode::head_follow_targetFeedback, this, _1));
-    this->tracking=true;
-    // this->alg_.lock();
-    // ROS_DEBUG("QrHeadTrackingAlgNode::MakeActionRequest: Goal Sent.");
-    return true;
-  }
-  else
-  {
-    // this->alg_.lock();
-    // ROS_DEBUG("QrHeadTrackingAlgNode::head_follow_targetMakeActionRequest: HRI server is not connected");
-    return false;
-  }
-}
 
 void QrHeadTrackingAlgNode::node_config_update(Config &config, uint32_t level)
 {
@@ -256,59 +127,28 @@ void QrHeadTrackingAlgNode::node_config_update(Config &config, uint32_t level)
       ROS_WARN("The tracking is already active");
     else
     {
-      // send the first target angles
-      this->head_target_JointTrajectoryPoint_msg_.positions[0]=this->pan_angle;
-      this->head_target_JointTrajectoryPoint_msg_.positions[1]=this->tilt_angle;
-      this->head_target_publisher_.publish(this->head_target_JointTrajectoryPoint_msg_);
-      // start the tracking
-      this->head_follow_targetMakeActionRequest();
+      this->tracking_module.start_tracking(this->pan_angle,this->tilt_angle);
+      this->tracking=true;
     }
     config.start_tracking=false;
   }
   else if(config.stop_tracking)
   {
     if(this->tracking)
-      this->head_follow_target_client_.cancelGoal();
+    {
+      this->tracking_module.stop_tracking();
+      this->tracking=false;
+    }
     config.stop_tracking=false;
   }
   else if(config.update_pan_pid)
   {
-    set_pan_pid_srv_.request.p = config.pan_p;
-    set_pan_pid_srv_.request.i = config.pan_i;
-    set_pan_pid_srv_.request.d = config.pan_d;
-    set_pan_pid_srv_.request.i_clamp = config.pan_i_clamp;
-    ROS_INFO("DarwinHeadTrackingClientAlgNode:: Sending New Request!");
-    if(this->set_pan_pid_client_.call(set_pan_pid_srv_))
-    {
-      if(this->set_pan_pid_srv_.response.ret)
-        ROS_INFO("DarwinHeadTrackingClientAlgNode:: Pan pid changed successfully");
-      else
-        ROS_INFO("DarwinHeadTrackingClientAlgNode:: Impossible to change pan pid");
-    }
-    else
-    {
-      ROS_INFO("DarwinHeadTrackingClientAlgNode:: Failed to Call Server on topic set_pan_pid ");
-    }
+    this->tracking_module.set_pan_pid(config.pan_p,config.pan_i,config.pan_d,config.pan_i_clamp);
     config.update_pan_pid=false;
   }
   else if(config.update_tilt_pid)
   {
-    set_tilt_pid_srv_.request.p = config.tilt_p;
-    set_tilt_pid_srv_.request.i = config.tilt_i;
-    set_tilt_pid_srv_.request.d = config.tilt_d;
-    set_tilt_pid_srv_.request.i_clamp = config.tilt_i_clamp;
-    ROS_INFO("DarwinHeadTrackingClientAlgNode:: Sending New Request!");
-    if (this->set_tilt_pid_client_.call(set_tilt_pid_srv_))
-    {
-      if(this->set_tilt_pid_srv_.response.ret)
-        ROS_INFO("DarwinHeadTrackingClientAlgNode:: Tilt pid changed successfully");
-      else
-        ROS_INFO("DarwinHeadTrackingClientAlgNode:: Impossible to change tilt pid");
-    }
-    else
-    {
-      ROS_INFO("DarwinHeadTrackingClientAlgNode:: Failed to Call Server on topic set_tilt_pid ");
-    }
+    this->tracking_module.set_tilt_pid(config.tilt_p,config.tilt_i,config.tilt_d,config.tilt_i_clamp);
     config.update_tilt_pid=false;
   }
   this->config_=config;
diff --git a/teleop/CMakeLists.txt b/teleop/CMakeLists.txt
index 0c9de17823ab44c120605d16c6ce0be7a1a29136..c0ccb616286306977b0d4464549553b4ba678c29 100644
--- a/teleop/CMakeLists.txt
+++ b/teleop/CMakeLists.txt
@@ -6,7 +6,7 @@ find_package(catkin REQUIRED)
 # ******************************************************************** 
 #                 Add catkin additional components here
 # ******************************************************************** 
-find_package(catkin REQUIRED COMPONENTS iri_base_algorithm trajectory_msgs geometry_msgs sensor_msgs actionlib humanoid_common_msgs)
+find_package(catkin REQUIRED COMPONENTS iri_base_algorithm humanoid_modules)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
@@ -15,6 +15,7 @@ find_package(catkin REQUIRED COMPONENTS iri_base_algorithm trajectory_msgs geome
 #           Add system and labrobotica dependencies here
 # ******************************************************************** 
 # find_package(<dependency> REQUIRED)
+find_package(iriutils REQUIRED)
 
 # ******************************************************************** 
 #           Add topic, service and action definition here
@@ -60,7 +61,7 @@ catkin_package(
 # ******************************************************************** 
 #            Add ROS and IRI ROS run time dependencies
 # ******************************************************************** 
- CATKIN_DEPENDS iri_base_algorithm trajectory_msgs geometry_msgs sensor_msgs actionlib humanoid_common_msgs
+ CATKIN_DEPENDS iri_base_algorithm humanoid_modules
 # ******************************************************************** 
 #      Add system and labrobotica run time dependencies here
 # ******************************************************************** 
@@ -76,6 +77,7 @@ catkin_package(
 # ******************************************************************** 
 include_directories(include)
 include_directories(${catkin_INCLUDE_DIRS})
+include_directories(${iriutils_INCLUDE_DIR})
 # include_directories(${<dependency>_INCLUDE_DIR})
 
 ## Declare a cpp library
@@ -88,6 +90,7 @@ add_executable(${PROJECT_NAME} src/teleop_alg.cpp src/teleop_alg_node.cpp)
 #                   Add the libraries
 # ******************************************************************** 
 target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
+target_link_libraries(${PROJECT_NAME} ${iriutils_LIBRARY})
 # target_link_libraries(${PROJECT_NAME} ${<dependency>_LIBRARY})
 
 # ******************************************************************** 
diff --git a/teleop/include/teleop_alg_node.h b/teleop/include/teleop_alg_node.h
index 96a49b57964630ff6aefbea3d30d670ff5f8bcae..c0199ec4ae3bc28418794adc71df01b7fa8b5287 100644
--- a/teleop/include/teleop_alg_node.h
+++ b/teleop/include/teleop_alg_node.h
@@ -28,22 +28,16 @@
 #include <iri_base_algorithm/iri_base_algorithm.h>
 #include "teleop_alg.h"
 
+#include <humanoid_modules/walk_module.h>
+#include <humanoid_modules/action_module.h>
+#include <humanoid_modules/head_tracking_module.h>
+
 // [publisher subscriber headers]
-#include <trajectory_msgs/JointTrajectoryPoint.h>
-#include <geometry_msgs/Twist.h>
 #include <sensor_msgs/Joy.h>
 
 // [service client headers]
-#include <humanoid_common_msgs/set_pid.h>
-#include <humanoid_common_msgs/get_walk_params.h>
-#include <humanoid_common_msgs/set_walk_params.h>
-#include <humanoid_common_msgs/set_servo_modules.h>
 
 // [action server client headers]
-#include <humanoid_common_msgs/humanoid_follow_targetAction.h>
-#include <actionlib/client/simple_action_client.h>
-#include <actionlib/client/terminal_state.h>
-#include <humanoid_common_msgs/humanoid_motionAction.h>
 
 // note on plain values:
 // buttons are either 0 or 1
@@ -96,13 +90,6 @@ class TeleopAlgNode : public algorithm_base::IriBaseAlgorithm<TeleopAlgorithm>
 {
   private:
     // [publisher attributes]
-    ros::Publisher head_target_publisher_;
-    trajectory_msgs::JointTrajectoryPoint head_target_JointTrajectoryPoint_msg_;
-
-    ros::Publisher cmd_vel_publisher_;
-    geometry_msgs::Twist cmd_vel_Twist_msg_;
-    void doWalking(void);
-    bool walking;
 
     // [subscriber attributes]
     ros::Subscriber joy_subscriber_;
@@ -118,41 +105,10 @@ class TeleopAlgNode : public algorithm_base::IriBaseAlgorithm<TeleopAlgorithm>
     // [service attributes]
 
     // [client attributes]
-    ros::ServiceClient set_tilt_pid_client_;
-    humanoid_common_msgs::set_pid set_tilt_pid_srv_;
-
-    ros::ServiceClient set_pan_pid_client_;
-    humanoid_common_msgs::set_pid set_pan_pid_srv_;
-
-    ros::ServiceClient get_walk_params_client_;
-    humanoid_common_msgs::get_walk_params get_walk_params_srv_;
-
-    ros::ServiceClient set_walk_params_client_;
-    humanoid_common_msgs::set_walk_params set_walk_params_srv_;
-
-    ros::ServiceClient set_servo_modules_client_;
-    humanoid_common_msgs::set_servo_modules set_servo_modules_srv_;
 
     // [action server attributes]
 
     // [action client attributes]
-    actionlib::SimpleActionClient<humanoid_common_msgs::humanoid_follow_targetAction> head_follow_target_client_;
-    humanoid_common_msgs::humanoid_follow_targetGoal head_follow_target_goal_;
-    bool head_follow_targetMakeActionRequest();
-    void head_follow_targetDone(const actionlib::SimpleClientGoalState& state,  const humanoid_common_msgs::humanoid_follow_targetResultConstPtr& result);
-    void head_follow_targetActive();
-    void head_follow_targetFeedback(const humanoid_common_msgs::humanoid_follow_targetFeedbackConstPtr& feedback);
-    bool tracking;
-    void doTracking(void);
-
-    actionlib::SimpleActionClient<humanoid_common_msgs::humanoid_motionAction> motion_action_client_;
-    humanoid_common_msgs::humanoid_motionGoal motion_action_goal_;
-    bool motion_actionMakeActionRequest();
-    void motion_actionDone(const actionlib::SimpleClientGoalState& state,  const humanoid_common_msgs::humanoid_motionResultConstPtr& result);
-    void motion_actionActive();
-    void motion_actionFeedback(const humanoid_common_msgs::humanoid_motionFeedbackConstPtr& feedback);
-    bool executing_action;
-    void doAction(void);
 
    /**
     * \brief config variable
@@ -161,6 +117,12 @@ class TeleopAlgNode : public algorithm_base::IriBaseAlgorithm<TeleopAlgorithm>
     * Is updated everytime function config_update() is called.
     */
     Config config_;
+    CWalkModule walk_client;
+    void doWalking(void);
+    CHeadTrackingModule head_tracking_client;
+    void doTracking(void);
+    CActionModule action_client;
+    void doAction(void);
   public:
    /**
     * \brief Constructor
diff --git a/teleop/launch/teleop.launch b/teleop/launch/teleop.launch
index 8a9127ec8eadb1bcc0f6bd4ae99dc623512aa4ca..20516f1a7928ac77368d5020b94b7466a6cd24f9 100644
--- a/teleop/launch/teleop.launch
+++ b/teleop/launch/teleop.launch
@@ -46,24 +46,36 @@
     <param name="tilt_i_clamp"        value="0.0"/>
     <param name="max_tilt"            value="1.5707"/>
     <param name="min_tilt"            value="-1.5707"/>
-    <remap from="/darwin/teleop/cmd_vel"
+    <remap from="/darwin/action_client/action_client/motion_action"
+             to="/darwin/robot/motion_action"/>
+    <remap from="/darwin/action_client/action_client/set_servo_modules"
+             to="/darwin/robot/set_servo_modules"/>
+    <remap from="/darwin/walk_client/walk_client/cmd_vel"
              to="/darwin/robot/cmd_vel"/>
-    <remap from="/darwin/teleop/set_walk_params"
+    <remap from="/darwin/walk_client/walk_client/set_walk_params"
              to="/darwin/robot/set_walk_params"/>
-    <remap from="/darwin/teleop/set_servo_modules"
+    <remap from="/darwin/walk_client/walk_client/get_walk_params"
+             to="/darwin/robot/get_walk_params"/>
+    <remap from="/darwin/walk_client/walk_client/set_servo_modules"
              to="/darwin/robot/set_servo_modules"/>
-    <remap from="/darwin/motion_action"
-             to="/darwin/robot/motion_action"/>
-    <remap from="/darwin/teleop/set_servo_modules"
-             to="/darwin/robot/set_servo_modules"/>
-    <remap from="/darwin/head_follow_target"
+    <remap from="/darwin/walk_client/walk_client/joint_states"
+             to="/darwin/joint_states"/>
+    <remap from="/darwin/walk_client/walk_client/fallen_state"
+             to="/darwin/robot/fallen_state"/>
+    <remap from="/darwin/head_tracking_client/head_tracking_client/head_tracking_action"
              to="/darwin/robot/head_follow_target"/>
-    <remap from="/darwin/teleop/head_target"
+    <remap from="/darwin/head_tracking_client/head_tracking_client/head_target"
              to="/darwin/robot/head_target"/>
-    <remap from="/darwin/teleop/set_pan_pid"
+    <remap from="/darwin/head_tracking_client/head_tracking_client/set_servo_modules"
+             to="/darwin/robot/set_servo_modules"/>
+    <remap from="/darwin/head_tracking_client/head_tracking_client/set_pan_pid"
              to="/darwin/robot/set_pan_pid"/>
-    <remap from="/darwin/teleop/set_tilt_pid"
+    <remap from="/darwin/head_tracking_client/head_tracking_client/get_pan_pid"
+             to="/darwin/robot/get_pan_pid"/>
+    <remap from="/darwin/head_tracking_client/head_tracking_client/set_tilt_pid"
              to="/darwin/robot/set_tilt_pid"/>
+    <remap from="/darwin/head_tracking_client/head_tracking_client/get_tilt_pid"
+             to="/darwin/robot/get_tilt_pid"/>
   </node>
 
   <node pkg="joy"
diff --git a/teleop/launch/teleop_sim.launch b/teleop/launch/teleop_sim.launch
index d1a639eac6a725b27b04cdb584796bbe56551f3f..b28b2bdb1364a36bb5835c442d90e7cd0617b769 100644
--- a/teleop/launch/teleop_sim.launch
+++ b/teleop/launch/teleop_sim.launch
@@ -46,24 +46,36 @@
     <param name="tilt_i_clamp"        value="0.0"/>
     <param name="max_tilt"            value="1.5707"/>
     <param name="min_tilt"            value="-1.5707"/>
-    <remap from="/darwin/teleop/cmd_vel"
+    <remap from="/darwin/action_client/action_client/motion_action"
+             to="/darwin/robot/motion_action"/>
+    <remap from="/darwin/action_client/action_client/set_servo_modules"
+             to="/darwin/robot/set_servo_modules"/>
+    <remap from="/darwin/walk_client/walk_client/cmd_vel"
              to="/darwin/robot/cmd_vel"/>
-    <remap from="/darwin/teleop/set_walk_params"
+    <remap from="/darwin/walk_client/walk_client/set_walk_params"
              to="/darwin/robot/set_walk_params"/>
-    <remap from="/darwin/teleop/set_servo_modules"
+    <remap from="/darwin/walk_client/walk_client/get_walk_params"
+             to="/darwin/robot/get_walk_params"/>
+    <remap from="/darwin/walk_client/walk_client/set_servo_modules"
              to="/darwin/robot/set_servo_modules"/>
-    <remap from="/darwin/motion_action"
-             to="/darwin/robot/motion_action"/>
-    <remap from="/darwin/teleop/set_servo_modules"
-             to="/darwin/robot/set_servo_modules"/>
-    <remap from="/darwin/head_follow_target"
+    <remap from="/darwin/walk_client/walk_client/joint_states"
+             to="/darwin/joint_states"/>
+    <remap from="/darwin/walk_client/walk_client/fallen_state"
+             to="/darwin/robot/fallen_state"/>
+    <remap from="/darwin/head_tracking_client/head_tracking_client/head_tracking_action"
              to="/darwin/robot/head_follow_target"/>
-    <remap from="/darwin/teleop/head_target"
+    <remap from="/darwin/head_tracking_client/head_tracking_client/head_target"
              to="/darwin/robot/head_target"/>
-    <remap from="/darwin/teleop/set_pan_pid"
+    <remap from="/darwin/head_tracking_client/head_tracking_client/set_servo_modules"
+             to="/darwin/robot/set_servo_modules"/>
+    <remap from="/darwin/head_tracking_client/head_tracking_client/set_pan_pid"
              to="/darwin/robot/set_pan_pid"/>
-    <remap from="/darwin/teleop/set_tilt_pid"
+    <remap from="/darwin/head_tracking_client/head_tracking_client/get_pan_pid"
+             to="/darwin/robot/get_pan_pid"/>
+    <remap from="/darwin/head_tracking_client/head_tracking_client/set_tilt_pid"
              to="/darwin/robot/set_tilt_pid"/>
+    <remap from="/darwin/head_tracking_client/head_tracking_client/get_tilt_pid"
+             to="/darwin/robot/get_tilt_pid"/>
   </node>
 
   <node pkg="joy"
diff --git a/teleop/package.xml b/teleop/package.xml
index 45b6ffa3019aaffdcfc36f16700139953c7c2b64..e2b779bbe37cafb70e1dc4e1bec5d39acd7392ef 100644
--- a/teleop/package.xml
+++ b/teleop/package.xml
@@ -41,17 +41,9 @@
   <!--   <test_depend>gtest</test_depend> -->
   <buildtool_depend>catkin</buildtool_depend>
   <build_depend>iri_base_algorithm</build_depend>
-  <build_depend>trajectory_msgs</build_depend>
-  <build_depend>geometry_msgs</build_depend>
-  <build_depend>sensor_msgs</build_depend>
-  <build_depend>actionlib</build_depend>
-  <build_depend>humanoid_common_msgs</build_depend>
+  <build_depend>humanoid_modules</build_depend>
   <run_depend>iri_base_algorithm</run_depend>
-  <run_depend>trajectory_msgs</run_depend>
-  <run_depend>geometry_msgs</run_depend>
-  <run_depend>sensor_msgs</run_depend>
-  <run_depend>actionlib</run_depend>
-  <run_depend>humanoid_common_msgs</run_depend>
+  <run_depend>humanoid_modules</run_depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->
diff --git a/teleop/src/teleop_alg_node.cpp b/teleop/src/teleop_alg_node.cpp
index a56e980e486ecca38c72296d1ca9456ee7714dcc..8c852e159ca909bcc6723d1b76850cb69f4d3b02 100644
--- a/teleop/src/teleop_alg_node.cpp
+++ b/teleop/src/teleop_alg_node.cpp
@@ -1,52 +1,16 @@
 #include "teleop_alg_node.h"
 
-#define NUM_SERVOS 32
-// joint names
-const std::string servo_names[NUM_SERVOS]={std::string("Servo0"),
-                                           std::string("j_shoulder_pitch_r"),
-                                           std::string("j_shoulder_pitch_l"),
-                                           std::string("j_shoulder_roll_r"),
-                                           std::string("j_shoulder_roll_l"),
-                                           std::string("j_elbow_r"),
-                                           std::string("j_elbow_l"),
-                                           std::string("j_hip_yaw_r"),
-                                           std::string("j_hip_yaw_l"),
-                                           std::string("j_hip_roll_r"),
-                                           std::string("j_hip_roll_l"),
-                                           std::string("j_hip_pitch_r"),
-                                           std::string("j_hip_pitch_l"),
-                                           std::string("j_knee_r"),
-                                           std::string("j_knee_l"),
-                                           std::string("j_ankle_pitch_r"),
-                                           std::string("j_ankle_pitch_l"),
-                                           std::string("j_ankle_roll_r"),
-                                           std::string("j_ankle_roll_l"),
-                                           std::string("j_pan"),
-                                           std::string("j_tilt"),
-                                           std::string("Servo21"),
-                                           std::string("Servo22"),
-                                           std::string("Servo23"),
-                                           std::string("Servo24"),
-                                           std::string("Servo25"),
-                                           std::string("Servo26"),
-                                           std::string("Servo27"),
-                                           std::string("Servo28"),
-                                           std::string("Servo29"),
-                                           std::string("Servo30"),
-                                           std::string("Servo31")};
-
 TeleopAlgNode::TeleopAlgNode(void) :
   algorithm_base::IriBaseAlgorithm<TeleopAlgorithm>(),
-  head_follow_target_client_("head_follow_target", true),
-  motion_action_client_("motion_action", true)
+  action_client("action_client"),
+  walk_client("walk_client"),
+  head_tracking_client("head_tracking_client")
 {
   unsigned int i=0;
   //init class attributes if necessary
   //this->loop_rate_ = 2;//in [Hz]
 
   // [init publishers]
-  this->head_target_publisher_ = this->public_node_handle_.advertise<trajectory_msgs::JointTrajectoryPoint>("head_target", 1);
-  this->cmd_vel_publisher_ = this->public_node_handle_.advertise<geometry_msgs::Twist>("cmd_vel", 1);
   
   // [init subscribers]
   this->joy_subscriber_ = this->public_node_handle_.subscribe("joy", 1, &TeleopAlgNode::joy_callback, this);
@@ -58,32 +22,10 @@ TeleopAlgNode::TeleopAlgNode(void) :
   // [init services]
   
   // [init clients]
-  set_tilt_pid_client_ = this->public_node_handle_.serviceClient<humanoid_common_msgs::set_pid>("set_tilt_pid");
-
-  set_pan_pid_client_ = this->public_node_handle_.serviceClient<humanoid_common_msgs::set_pid>("set_pan_pid");
-
-  get_walk_params_client_ = this->public_node_handle_.serviceClient<humanoid_common_msgs::get_walk_params>("get_walk_params");
-
-  set_walk_params_client_ = this->public_node_handle_.serviceClient<humanoid_common_msgs::set_walk_params>("set_walk_params");
-
-  set_servo_modules_client_ = this->public_node_handle_.serviceClient<humanoid_common_msgs::set_servo_modules>("set_servo_modules");
-
   
   // [init action servers]
   
   // [init action clients]
-
-  this->executing_action=false;
-  this->walking=false;
-  this->tracking=false;
-  this->set_servo_modules_srv_.request.names.resize(NUM_SERVOS);
-  this->set_servo_modules_srv_.request.modules.resize(NUM_SERVOS);
-  for(i=0;i<NUM_SERVOS;i++)
-  {
-    this->set_servo_modules_srv_.request.names[i]=servo_names[i];
-    this->set_servo_modules_srv_.request.modules[i]="none";
-  }
-  this->head_target_JointTrajectoryPoint_msg_.positions.resize(2);
 }
 
 TeleopAlgNode::~TeleopAlgNode(void)
@@ -147,162 +89,6 @@ void TeleopAlgNode::joy_mutex_exit(void)
 /*  [service callbacks] */
 
 /*  [action callbacks] */
-void TeleopAlgNode::head_follow_targetDone(const actionlib::SimpleClientGoalState& state,  const humanoid_common_msgs::humanoid_follow_targetResultConstPtr& result)
-{
-  alg_.lock();
-  if( state == actionlib::SimpleClientGoalState::SUCCEEDED )
-    ROS_INFO("TeleopAlgNode::head_follow_targetDone: Goal Achieved!");
-  else
-    ROS_INFO("TeleopAlgNode::head_follow_targetDone: %s", state.toString().c_str());
-  this->tracking=false;
-
-  //copy & work with requested result
-  alg_.unlock();
-}
-
-void TeleopAlgNode::head_follow_targetActive()
-{
-  alg_.lock();
-  //ROS_INFO("TeleopAlgNode::head_follow_targetActive: Goal just went active!");
-  this->tracking=true;
-  alg_.unlock();
-}
-
-void TeleopAlgNode::head_follow_targetFeedback(const humanoid_common_msgs::humanoid_follow_targetFeedbackConstPtr& feedback)
-{
-  alg_.lock();
-  //ROS_INFO("TeleopAlgNode::head_follow_targetFeedback: Got Feedback!");
-
-  bool feedback_is_ok = true;
-
-  //analyze feedback
-  //my_var = feedback->var;
-
-  ROS_INFO("Current angles: pan %f, tilt %f",feedback->current_pan,feedback->current_tilt);
-
-  //if feedback is not what expected, cancel requested goal
-  if( !feedback_is_ok )
-  {
-    this->head_follow_target_client_.cancelGoal();
-    //ROS_INFO("TeleopAlgNode::head_follow_targetFeedback: Cancelling Action!");
-  }
-  alg_.unlock();
-}
-
-void TeleopAlgNode::motion_actionDone(const actionlib::SimpleClientGoalState& state,  const humanoid_common_msgs::humanoid_motionResultConstPtr& result)
-{
-  alg_.lock();
-  if( state == actionlib::SimpleClientGoalState::SUCCEEDED )
-    ROS_INFO("TeleopAlgNode::motion_actionDone: Goal Achieved!");
-  else
-    ROS_INFO("TeleopAlgNode::motion_actionDone: %s", state.toString().c_str());
-  this->executing_action=false;
-
-  //copy & work with requested result
-  alg_.unlock();
-}
-
-void TeleopAlgNode::motion_actionActive()
-{
-  alg_.lock();
-  //ROS_INFO("TeleopAlgNode::motion_actionActive: Goal just went active!");
-  this->executing_action=true;
-  alg_.unlock();
-}
-
-void TeleopAlgNode::motion_actionFeedback(const humanoid_common_msgs::humanoid_motionFeedbackConstPtr& feedback)
-{
-  alg_.lock();
-  //ROS_INFO("TeleopAlgNode::motion_actionFeedback: Got Feedback!");
-
-  bool feedback_is_ok = true;
-
-  //analyze feedback
-  //my_var = feedback->var;
-  ROS_INFO("Executing step %d of page %d",feedback->current_step,feedback->current_page);
-
-  //if feedback is not what expected, cancel requested goal
-  if( !feedback_is_ok )
-  {
-    this->motion_action_client_.cancelGoal();
-    //ROS_INFO("TeleopAlgNode::motion_actionFeedback: Cancelling Action!");
-  }
-  alg_.unlock();
-}
-
-
-/*  [action requests] */
-bool TeleopAlgNode::head_follow_targetMakeActionRequest()
-{
-  // IMPORTANT: Please note that all mutex used in the client callback functions
-  // must be unlocked before calling any of the client class functions from an
-  // other thread (MainNodeThread).
-  this->alg_.unlock();
-  this->set_servo_modules_srv_.request.modules[19]="head";
-  this->set_servo_modules_srv_.request.modules[20]="head";
-  if(this->set_servo_modules_client_.call(this->set_servo_modules_srv_))
-  {
-    this->head_follow_target_client_.waitForServer();
-    this->head_follow_target_goal_.pan_range.resize(2);
-    this->head_follow_target_goal_.pan_range[0]=this->config_.max_pan;
-    this->head_follow_target_goal_.pan_range[1]=this->config_.min_pan;
-    this->head_follow_target_goal_.tilt_range.resize(2);
-    this->head_follow_target_goal_.tilt_range[0]=this->config_.max_tilt;
-    this->head_follow_target_goal_.tilt_range[1]=this->config_.min_tilt;
-    this->head_follow_target_goal_.target_pan=3.14159*this->current_axes[PS3_AXIS_STICK_LEFT_LEFTWARDS];
-    this->head_follow_target_goal_.target_tilt=3.14159*this->current_axes[PS3_AXIS_STICK_LEFT_UPWARDS];
-
-    ROS_INFO("TeleopAlgNode::head_follow_targetMakeActionRequest: Server is Available!");
-    //send a goal to the action server
-    //head_follow_target_goal_.data = my_desired_goal;
-    head_follow_target_client_.sendGoal(head_follow_target_goal_,
-                boost::bind(&TeleopAlgNode::head_follow_targetDone,     this, _1, _2),
-                boost::bind(&TeleopAlgNode::head_follow_targetActive,   this),
-                boost::bind(&TeleopAlgNode::head_follow_targetFeedback, this, _1));
-    this->alg_.lock();
-    ROS_INFO("TeleopAlgNode::MakeActionRequest: Goal Sent.");
-    return true;
-  }
-  else
-  {
-    this->alg_.lock();
-    ROS_WARN("TeleopAlgNode::head_follow_targetMakeActionRequest: HRI server is not connected");
-    return false;
-  }
-}
-
-bool TeleopAlgNode::motion_actionMakeActionRequest()
-{
-  unsigned int i=0;
-
-  // IMPORTANT: Please note that all mutex used in the client callback functions
-  // must be unlocked before calling any of the client class functions from an
-  // other thread (MainNodeThread).
-  this->alg_.unlock();
-  for(i=0;i<NUM_SERVOS;i++)
-    this->set_servo_modules_srv_.request.modules[i]="action";
-  if(this->set_servo_modules_client_.call(this->set_servo_modules_srv_))
-  {
-    motion_action_client_.waitForServer();
-    ROS_INFO("TeleopAlgNode::motion_actionMakeActionRequest: Server is Available!");
-    //send a goal to the action server
-    //motion_action_goal_.data = my_desired_goal;
-    motion_action_client_.sendGoal(motion_action_goal_,
-                boost::bind(&TeleopAlgNode::motion_actionDone,     this, _1, _2),
-                boost::bind(&TeleopAlgNode::motion_actionActive,   this),
-                boost::bind(&TeleopAlgNode::motion_actionFeedback, this, _1));
-    this->alg_.lock();
-    ROS_INFO("TeleopAlgNode::MakeActionRequest: Goal Sent.");
-    return true;
-  }
-  else
-  {
-    this->alg_.lock();
-    ROS_WARN("TeleopAlgNode::motion_actionMakeActionRequest: Failed to call set_servo_modules service");
-    return false;
-  }
-}
-
 void TeleopAlgNode::node_config_update(Config &config, uint32_t level)
 {
   this->alg_.lock();
@@ -328,25 +114,24 @@ void TeleopAlgNode::doAction(void)
       this->current_buttons[PS3_BUTTON_REAR_RIGHT_1]==0 &&
       this->current_buttons[PS3_BUTTON_REAR_RIGHT_2]==0)
   {
-    this->motion_action_goal_.motion_id=-1;
-    if(this->current_buttons[PS3_BUTTON_ACTION_TRIANGLE]==1)
-      this->motion_action_goal_.motion_id=this->config_.triangle_action_id;
-    else if(this->current_buttons[PS3_BUTTON_ACTION_CIRCLE]==1)
-      this->motion_action_goal_.motion_id=this->config_.circle_action_id;
-    else if(this->current_buttons[PS3_BUTTON_ACTION_CROSS]==1)
-      this->motion_action_goal_.motion_id=this->config_.cross_action_id;
-    else if(this->current_buttons[PS3_BUTTON_ACTION_SQUARE]==1)
-      this->motion_action_goal_.motion_id=this->config_.square_action_id;
-    if(!this->executing_action && this->motion_action_goal_.motion_id!=-1)
-      if(!this->motion_actionMakeActionRequest())
-        ROS_WARN("TeleopAlgNode::doAction: Failed to start action");
+    if(this->action_client.is_finished())
+    {
+      if(this->current_buttons[PS3_BUTTON_ACTION_TRIANGLE]==1)
+        this->action_client.execute(this->config_.triangle_action_id);
+      else if(this->current_buttons[PS3_BUTTON_ACTION_CIRCLE]==1)
+        this->action_client.execute(this->config_.circle_action_id);
+      else if(this->current_buttons[PS3_BUTTON_ACTION_CROSS]==1)
+        this->action_client.execute(this->config_.cross_action_id);
+      else if(this->current_buttons[PS3_BUTTON_ACTION_SQUARE]==1)
+        this->action_client.execute(this->config_.square_action_id);
+    }
   }
   else
   {
-    if(this->executing_action)
+    if(!this->action_client.is_finished())
     {
       ROS_WARN("TeleopAlgNode::doAction: Cancelling current action");
-      this->motion_action_client_.cancelGoal();
+      this->action_client.stop();
     }
   }
 }
@@ -360,77 +145,34 @@ void TeleopAlgNode::doWalking(void)
       this->current_buttons[PS3_BUTTON_REAR_RIGHT_1]==1 &&
       this->current_buttons[PS3_BUTTON_REAR_RIGHT_2]==0)
   {
-    /* set the walking parameters */
-    if(!this->walking)
+    if(this->walk_client.is_finished())
     {
-      for(i=0;i<NUM_SERVOS;i++)
-        this->set_servo_modules_srv_.request.modules[i]="walking";
-      if(this->set_servo_modules_client_.call(this->set_servo_modules_srv_))
-      {
-        set_walk_params_srv_.request.params.Y_SWAP_AMPLITUDE = this->config_.Y_SWAP_AMPLITUDE;
-        set_walk_params_srv_.request.params.Z_SWAP_AMPLITUDE = this->config_.Z_SWAP_AMPLITUDE;
-        set_walk_params_srv_.request.params.ARM_SWING_GAIN = this->config_.ARM_SWING_GAIN;
-        set_walk_params_srv_.request.params.PELVIS_OFFSET = this->config_.PELVIS_OFFSET;
-        set_walk_params_srv_.request.params.HIP_PITCH_OFFSET = this->config_.HIP_PITCH_OFFSET;
-        set_walk_params_srv_.request.params.X_OFFSET = this->config_.X_OFFSET;
-        set_walk_params_srv_.request.params.Y_OFFSET = this->config_.Y_OFFSET;
-        set_walk_params_srv_.request.params.Z_OFFSET = this->config_.Z_OFFSET;
-        set_walk_params_srv_.request.params.A_OFFSET = this->config_.A_OFFSET;
-        set_walk_params_srv_.request.params.P_OFFSET = this->config_.P_OFFSET;
-        set_walk_params_srv_.request.params.R_OFFSET = this->config_.R_OFFSET;
-        set_walk_params_srv_.request.params.PERIOD_TIME = this->config_.PERIOD_TIME;
-        set_walk_params_srv_.request.params.DSP_RATIO = this->config_.DSP_RATIO;
-        set_walk_params_srv_.request.params.STEP_FB_RATIO = this->config_.STEP_FB_RATIO;
-        set_walk_params_srv_.request.params.FOOT_HEIGHT = this->config_.FOOT_HEIGHT;
-        set_walk_params_srv_.request.params.MAX_VEL = this->config_.MAX_VEL;
-        set_walk_params_srv_.request.params.MAX_ROT_VEL = this->config_.MAX_ROT_VEL;
-        ROS_INFO("TeleopAlgNode:: Setting walk parameters!");
-        if(set_walk_params_client_.call(set_walk_params_srv_))
-        {
-          if(set_walk_params_srv_.response.ret)
-          {
-            ROS_INFO("WalkClientAlgNode:: Walk parameters changed successfully");
-            this->cmd_vel_Twist_msg_.linear.x=this->config_.max_trans_speed*this->current_axes[PS3_AXIS_STICK_LEFT_UPWARDS];
-            this->cmd_vel_Twist_msg_.linear.y=this->config_.max_lat_speed*this->current_axes[PS3_AXIS_STICK_LEFT_LEFTWARDS];
-            this->cmd_vel_Twist_msg_.linear.z=0;
-            this->cmd_vel_Twist_msg_.angular.x=0;
-            this->cmd_vel_Twist_msg_.angular.y=0;
-            this->cmd_vel_Twist_msg_.angular.z=this->config_.max_rot_speed*this->current_axes[PS3_AXIS_STICK_RIGHT_LEFTWARDS];
-            this->walking=true;
-          } 
-          else
-            ROS_INFO("TeleopAlgNode:: Impossible to change walk parameters");
-        }
-        else
-          ROS_INFO("TeleopAlgNode:: Failed to Call Server on topic set_walk_params ");
-      }
-      else
-      {
-        ROS_INFO("TeleopAlgNode:: Failed to Call Server on topic set_servo_modules ");
-      }
+      this->walk_client.set_left_right_swing(this->config_.Y_SWAP_AMPLITUDE);
+      this->walk_client.set_top_down_swing(this->config_.Z_SWAP_AMPLITUDE);
+      this->walk_client.set_arm_swing_gain(this->config_.ARM_SWING_GAIN);
+      this->walk_client.set_hip_pitch_offset(this->config_.HIP_PITCH_OFFSET);
+      this->walk_client.set_pelvis_offset(this->config_.PELVIS_OFFSET);
+      this->walk_client.set_x_offset(this->config_.X_OFFSET);
+      this->walk_client.set_y_offset(this->config_.Y_OFFSET);
+      this->walk_client.set_z_offset(this->config_.Z_OFFSET);
+      this->walk_client.set_roll_offset(this->config_.R_OFFSET);
+      this->walk_client.set_pitch_offset(this->config_.P_OFFSET);
+      this->walk_client.set_yaw_offset(this->config_.A_OFFSET);
+      this->walk_client.set_period(this->config_.PERIOD_TIME);
+      this->walk_client.set_ssp_dsp_ratio(this->config_.DSP_RATIO);
+      this->walk_client.set_fwd_bwd_ratio(this->config_.STEP_FB_RATIO);
+      this->walk_client.set_foot_height(this->config_.FOOT_HEIGHT);
+      this->walk_client.set_trans_speed(this->config_.MAX_VEL);
+      this->walk_client.set_rot_speed(this->config_.MAX_ROT_VEL);
     }
-    else
-    {
-      // send command vel
-      this->cmd_vel_Twist_msg_.linear.x=this->config_.max_trans_speed*this->current_axes[PS3_AXIS_STICK_LEFT_UPWARDS];
-      this->cmd_vel_Twist_msg_.linear.y=this->config_.max_lat_speed*this->current_axes[PS3_AXIS_STICK_LEFT_LEFTWARDS];
-      this->cmd_vel_Twist_msg_.linear.z=0;
-      this->cmd_vel_Twist_msg_.angular.x=0;
-      this->cmd_vel_Twist_msg_.angular.y=0;
-      this->cmd_vel_Twist_msg_.angular.z=this->config_.max_rot_speed*this->current_axes[PS3_AXIS_STICK_RIGHT_LEFTWARDS];
-    }
-    // Uncomment the following line to publish the topic message
-    this->cmd_vel_publisher_.publish(this->cmd_vel_Twist_msg_);
+    this->walk_client.set_steps_size(this->config_.max_trans_speed*this->current_axes[PS3_AXIS_STICK_LEFT_UPWARDS],
+                                     this->config_.max_lat_speed*this->current_axes[PS3_AXIS_STICK_LEFT_LEFTWARDS],
+                                     this->config_.max_rot_speed*this->current_axes[PS3_AXIS_STICK_RIGHT_LEFTWARDS]);
   }
   else
-  { 
-    this->cmd_vel_Twist_msg_.linear.x=0;
-    this->cmd_vel_Twist_msg_.linear.y=0;
-    this->cmd_vel_Twist_msg_.linear.z=0;
-    this->cmd_vel_Twist_msg_.angular.x=0;
-    this->cmd_vel_Twist_msg_.angular.y=0;
-    this->cmd_vel_Twist_msg_.angular.z=0;
-    this->walking=false;
+  {
+    if(!this->walk_client.is_finished()) 
+      this->walk_client.stop();
   }
 }
 
@@ -442,61 +184,25 @@ void TeleopAlgNode::doTracking(void)
       this->current_buttons[PS3_BUTTON_REAR_RIGHT_1]==0 &&
       this->current_buttons[PS3_BUTTON_REAR_RIGHT_2]==0)
   {
-    if(!this->tracking)
+    if(this->head_tracking_client.is_finished())
     {
-      this->set_pan_pid_srv_.request.p = this->config_.pan_p;
-      this->set_pan_pid_srv_.request.i = this->config_.pan_i;
-      this->set_pan_pid_srv_.request.d = this->config_.pan_d;
-      this->set_pan_pid_srv_.request.i_clamp = this->config_.pan_i_clamp;
-      ROS_INFO("TeleopAlgNode:: Setting pan PID parameters!");
-      if(this->set_pan_pid_client_.call(this->set_pan_pid_srv_))
-      {
-        if(this->set_pan_pid_srv_.response.ret)
-        {
-          ROS_INFO("TeleopAlgNode:: Pan pid changed successfully");
-          this->set_tilt_pid_srv_.request.p = this->config_.tilt_p;
-          this->set_tilt_pid_srv_.request.i = this->config_.tilt_i;
-          this->set_tilt_pid_srv_.request.d = this->config_.tilt_d;
-          this->set_tilt_pid_srv_.request.i_clamp = this->config_.tilt_i_clamp;
-          ROS_INFO("TeleopAlgNode:: Setting tilt PID parameters!");
-          if (this->set_tilt_pid_client_.call(this->set_tilt_pid_srv_))
-          {
-            if(this->set_tilt_pid_srv_.response.ret)
-            {
-              ROS_INFO("TeleopAlgNode:: Tilt pid changed successfully");
-              if(!this->head_follow_targetMakeActionRequest())
-                ROS_WARN("TeleopAlgNode:: Impossible to start head tracking");
-            }
-            else
-              ROS_INFO("TeleopAlgNode:: Impossible to change tilt pid");
-          }
-          else
-          {
-            ROS_INFO("TeleopAlgNode:: Failed to Call Server on topic set_tilt_pid ");
-          }
-        }
-        else
-          ROS_INFO("TeleopAlgNode:: Impossible to change pan pid");
-      }
-      else
-      {
-        ROS_INFO("TeleopAlgNode:: Failed to Call Server on topic set_pan_pid ");
-      }
+      this->head_tracking_client.set_pan_pid(this->config_.pan_p,this->config_.pan_i,this->config_.pan_d,this->config_.pan_i_clamp);
+      this->head_tracking_client.set_pan_range(this->config_.max_pan,this->config_.min_pan);
+      this->head_tracking_client.set_tilt_pid(this->config_.tilt_p,this->config_.tilt_i,this->config_.tilt_d,this->config_.tilt_i_clamp);
+      this->head_tracking_client.set_tilt_range(this->config_.max_tilt,this->config_.min_tilt);
+      this->head_tracking_client.start_tracking(3.14159*this->current_axes[PS3_AXIS_STICK_LEFT_LEFTWARDS],3.14159*this->current_axes[PS3_AXIS_STICK_LEFT_UPWARDS]);
     }
     else
     {
-      this->head_target_JointTrajectoryPoint_msg_.positions[0]=3.14159*this->current_axes[PS3_AXIS_STICK_LEFT_LEFTWARDS];
-      this->head_target_JointTrajectoryPoint_msg_.positions[1]=3.14159*this->current_axes[PS3_AXIS_STICK_LEFT_UPWARDS];
-      // Uncomment the following line to publish the topic message
-      this->head_target_publisher_.publish(this->head_target_JointTrajectoryPoint_msg_);
+      this->head_tracking_client.update_target(3.14159*this->current_axes[PS3_AXIS_STICK_LEFT_LEFTWARDS],3.14159*this->current_axes[PS3_AXIS_STICK_LEFT_UPWARDS]);
     }
   }
   else
   {
-    if(this->tracking)
+    if(!this->head_tracking_client.is_finished())
     {
       ROS_WARN("TeleopAlgNode:: Stop tracking with the head ");
-      this->head_follow_target_client_.cancelGoal();
+      this->head_tracking_client.stop_tracking();
     }
   }
 }