diff --git a/cfg/IriJointsClient.cfg b/cfg/IriJointsClient.cfg
index c2ad06bbb2b8907519ffa05adff077dec11ec281..f409dfaba6ad18ea8ab8428dd3b367618a4f37ad 100755
--- a/cfg/IriJointsClient.cfg
+++ b/cfg/IriJointsClient.cfg
@@ -51,9 +51,9 @@ point_to.add("send_point_to",    bool_t,    0,                               "Ca
 point_to.add("cancel_point_to",  bool_t,    0,                               "Cancel point_to goal",         False)
 point_to.add("ik_tol",           double_t,  0,                               "Inverse kinematics solver tolerance",      0.001,      0.0, 1.0)
 point_to.add("send_ik_tol",      bool_t,    0,                               "Send inverse kinematic tolerance",         False)
-point_to.add("base_frame",       str_t,     0,                               "Kinematic chain base frame",                          "servo2")
+point_to.add("base_frame",       str_t,     0,                               "Kinematic chain base frame",                          "pan_frame")
 point_to.add("send_base_frame",  bool_t,    0,                               "Send Kinematic chain base frame",         False)
-point_to.add("urdf_param",       str_t,     0,                               "ROS param where the urdf model is stored",            "robot_description")
+point_to.add("urdf_param",       str_t,     0,                               "ROS param where the urdf model is stored",            "/robot_description")
 point_to.add("send_urdf_param",  bool_t,    0,                               "Send the ROS param where the urdf model is stored",         False)
 
 tracker.add("start_tracking",   bool_t,    0,                          "Start tracking action",         False)
diff --git a/cfg/IriJointsModule.cfg b/cfg/IriJointsModule.cfg
index bd1bebbfa4e7665a744df39cca9be442ee41dd5a..f21e0eb95853fd29a03fb81f4ddd802b4f9297d8 100755
--- a/cfg/IriJointsModule.cfg
+++ b/cfg/IriJointsModule.cfg
@@ -49,7 +49,7 @@ axis_enum = gen.enum([ gen.const("x", int_t, 0, "Pointing axis x"),
 
 #       Name                       Type                     Reconfiguration level  Description                       Default   Min   Max
 #gen.add("velocity_scale_factor",  double_t,                0,                     "Maximum velocity scale factor",  0.5,      0.0,  1.0)
-gen.add("rate_hz",                 double_t,                0,                     "Navigation module rate in Hz",   1.0,      1.0,  100.0)
+gen.add("rate_hz",                 double_t,                0,                     "Joints module rate in Hz",   1.0,      1.0,  100.0)
 gen.add("frame_id",                str_t,                   0,                     "Joints module frame id",   "joints_module")
 
 joint_states.add("joint_states_watchdog_time_s",   double_t,                0,                     "Maximum time between JointStates messages",1,   0.01,    50)
@@ -70,7 +70,11 @@ tracker.add("tracker_feedback_watchdog_time_s",   double_t,             0,
 tracker.add("tracker_enable_timeout",             bool_t,               0,                      "Enable action timeout",          True)
 tracker.add("tracker_timeout_s",                  double_t,             0,                      "Maximum time allowed to complete the action",    5,   0.1,    6000)
 
-move_to_angles.add("move_to_angles_time_from_start",            int_t,                0,                      "Max time in seconds to finish the action",    10,   0,    100)
+move_to_angles.add("move_to_angles_time_from_start",            int_t,    0, "Max time in seconds to finish the action",    10,   0,    100)
+move_to_angles.add("move_to_angles_pos_tol",                    double_t, 0, "Path and goal position tolerance in rad or m", 0.002, -1)
+move_to_angles.add("move_to_angles_vel_tol",                    double_t, 0, "Path and goal velocity tolerance in rad/s or m/s", 0.0, -1)
+move_to_angles.add("move_to_angles_acc_tol",                    double_t, 0, "Path and goal acceleration tolerance in rad/s2 or m/s2", 0.0, -1)
+move_to_angles.add("move_to_angles_time_tol",                   double_t, 0, "Path and goal time tolerance in percentaje", 0.1, 0.0, 1.0)
 
 move_to_angles.add("move_to_angles_max_retries",                int_t,                0,                      "Maximum number of retries for the action start",    1,   1,    10)
 move_to_angles.add("move_to_angles_enable_watchdog",            bool_t,               0,                      "Enable action watchdog",         True)
diff --git a/config/joints_module_default.yaml b/config/joints_module_default.yaml
index 8d762fdb6dc6a98d45788a4299fb8102c1c9d94a..c4d12368b591727c124357c18be46b9a54a61885 100644
--- a/config/joints_module_default.yaml
+++ b/config/joints_module_default.yaml
@@ -3,7 +3,7 @@ frame_id: joints_module
 
 joint_states_watchdog_time_s: 1.0
 
-pointing_frame: "load_pointing"
+pointing_frame: "camera_optical_frame"
 pointing_axis: 2
 point_frame: "world"
 
@@ -19,7 +19,11 @@ tracker_feedback_watchdog_time_s: 1.0
 tracker_enable_timeout: True
 tracker_timeout_s: 10
 
-move_to_angles_time_from_start: 10
+move_to_angles_time_from_start: 0
+move_to_angles_pos_tol: 0.02
+move_to_angles_vel_tol: -1.0
+move_to_angles_acc_tol: -1.0
+move_to_angles_time_tol: 0.0
 
 move_to_angles_max_retries: 1
 move_to_angles_enable_watchdog: True
diff --git a/launch/joints_client.launch b/launch/joints_client.launch
index 6eaadfcc652314df372055f030f51aa46f2e4948..24b5773bc8fc5d047be9a00241b9336fbb1d0135 100644
--- a/launch/joints_client.launch
+++ b/launch/joints_client.launch
@@ -2,13 +2,13 @@
 
 <launch>
 
-  <arg name="config_file" default="$(find iri_joints_module)/config/joint_module_ccyt_demo_config.yaml" />
+  <arg name="config_file" default="$(find iri_joints_module)/config/joints_module_default.yaml" />
   <arg name="node_name" default="joints_module"/>
   <arg name="output" default="screen"/>
   <arg name="launch_prefix" default=""/>
   <arg name="point_to_node_name" default="point_to"/>
-  <arg name="device_ns" default="ana"/>
-  <arg name="ns" default="ana"/>
+  <arg name="device_ns" default="two_servos"/>
+  <arg name="ns" default="two_servos"/>
 
   <node name="$(arg node_name)"
         pkg="iri_joints_module"
@@ -26,7 +26,7 @@
     <remap from="~/$(arg node_name)/tracker_point_to"
              to="/$(arg ns)/$(arg point_to_node_name)/tracker_point_to"/>
     <remap from="~/$(arg node_name)/move_to_angle"
-             to="/$(arg device_ns)/effort_controller/follow_joint_trajectory"/>
+             to="/$(arg device_ns)/joint_trajectory_gazebo/joint_motion"/>
     <rosparam file="$(arg config_file)" command="load" ns="joints_module" />
   </node>
 
diff --git a/launch/joints_client_bt.launch b/launch/joints_client_bt.launch
index 3e94189187709f1620dbdd317c4af7366cb630c7..66c5dd5f4e8383460ad8c56bb1bac49084388363 100644
--- a/launch/joints_client_bt.launch
+++ b/launch/joints_client_bt.launch
@@ -2,16 +2,16 @@
 
 <launch>
 
-  <arg name="config_file" default="$(find iri_joints_module)/config/joint_module_ccyt_demo_config.yaml" />
+  <arg name="config_file" default="$(find iri_joints_module)/config/joints_module_default.yaml" />
   <arg name="node_name" default="joints_module"/>
   <arg name="output" default="screen"/>
   <arg name="launch_prefix" default=""/>
   <arg name="point_to_node_name" default="point_to"/>
-  <arg name="device_ns" default="ana"/>
-  <arg name="ns" default="ana"/>
+  <arg name="device_ns" default="two_servos"/>
+  <arg name="ns" default="two_servos"/>
 
   <arg name="xml_path"           default="$(find iri_joints_module)/src/xml"/>
-  <arg name="xml_file"           default="bt_point_tracker_test.xml"/>
+  <arg name="xml_file"           default="bt_move_to_angles_test.xml"/>
   <arg name="send_ticks"     default="False"/>
 
   <node name="$(arg node_name)"
@@ -30,7 +30,7 @@
     <remap from="~/$(arg node_name)/tracker_point_to"
              to="/$(arg ns)/$(arg point_to_node_name)/tracker_point_to"/>
     <remap from="~/$(arg node_name)/move_to_angle"
-             to="/$(arg device_ns)/effort_controller/follow_joint_trajectory"/>
+             to="/$(arg device_ns)/joint_trajectory_gazebo/joint_motion"/>
     <param name="xml_path" value="$(arg xml_path)"/>
     <param name="xml_file" value="$(arg xml_file)"/>
     <param name="send_ticks" value="$(arg send_ticks)"/>
@@ -38,8 +38,8 @@
   </node>
 
   <!-- launch dynamic reconfigure -->
-<!--  <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" respawn="false"
-    output="screen"/>-->
+  <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" respawn="false"
+    output="screen"/>
 
 </launch>
 
diff --git a/src/iri_joints_module.cpp b/src/iri_joints_module.cpp
index eeb5484361a9290e54fc91784a46727d07432326..684285cebddf65827fc51e396612a27f7b50d698 100644
--- a/src/iri_joints_module.cpp
+++ b/src/iri_joints_module.cpp
@@ -536,8 +536,22 @@ bool CIriJointsModule::move_to_angles(std::vector<std::string> joint_names, std:
   std::vector<std::string>().swap(this->move_to_angle_goal.trajectory.joint_names);
   this->move_to_angle_goal.trajectory.joint_names = joint_names;
 
+  control_msgs::JointTolerance tol;
+  tol.position = this->config.move_to_angles_pos_tol;
+  tol.velocity = this->config.move_to_angles_vel_tol;
+  tol.acceleration = this->config.move_to_angles_acc_tol;
+  std::vector<control_msgs::JointTolerance>().swap(this->move_to_angle_goal.path_tolerance);
+  std::vector<control_msgs::JointTolerance>().swap(this->move_to_angle_goal.goal_tolerance);
+  for (uint8_t i; i < this->move_to_angle_goal.trajectory.joint_names.size(); i++)
+  {
+    tol.name = this->move_to_angle_goal.trajectory.joint_names[i];
+    this->move_to_angle_goal.path_tolerance.push_back(tol);
+    this->move_to_angle_goal.goal_tolerance.push_back(tol);
+  }
+
   trajectory_msgs::JointTrajectoryPoint point;
   point.time_from_start = ros::Duration(this->config.move_to_angles_time_from_start);
+  this->move_to_angle_goal.goal_time_tolerance = point.time_from_start*this->config.move_to_angles_time_tol;
   // ROS_INFO_STREAM("Max time " << point.time_from_start);
   for (uint8_t i = 0; i < angles.size(); i++)
   {