diff --git a/include/tiago_modules/arm_module.h b/include/tiago_modules/arm_module.h
index 0b3826f06dd5b65a18d0ba3c587553f34d14f193..c8587cdc467369861608a7c161a4606b435b0dfd 100644
--- a/include/tiago_modules/arm_module.h
+++ b/include/tiago_modules/arm_module.h
@@ -9,7 +9,6 @@
 //Action
 #include <moveit_msgs/MoveGroupAction.h>
 #include <moveit_msgs/GetPlanningScene.h>
-#include <moveit/move_group_interface/move_group_interface.h>
 #include <geometry_msgs/PoseStamped.h>
 #include <geometry_msgs/QuaternionStamped.h>
 #include <sensor_msgs/JointState.h>
@@ -68,8 +67,8 @@ class CArmModule : public CModule<arm_module::ArmModuleConfig>
     // planning objects
     std::vector<TVisualObject> objects;
     ros::Publisher scene_publisher;
-	CModuleService<moveit_msgs::GetPlanningScene> get_scene;
-	moveit_msgs::PlanningScene current_pscene;
+    CModuleService<moveit_msgs::GetPlanningScene> get_scene;
+    moveit_msgs::PlanningScene current_pscene;
 
     // path constraints
     moveit_msgs::Constraints path_constraints;
@@ -109,6 +108,7 @@ class CArmModule : public CModule<arm_module::ArmModuleConfig>
     void add_orientation_path_constraint(geometry_msgs::QuaternionStamped &quat,double orientation_tol=0.1);
     void add_plane_path_constraint(path_const_plane_t plane,std::string &frame_id,double offset=0.0,double position_tol=0.1);
     void add_plane_path_constraint(std::vector<double> &normal,std::string &frame_id,double offset=0.0,double position_tol=0.1);
+    void add_joint_goal_constraint(std::vector<std::string> &joint_names,std::vector<double> &position, std::vector<double> &tol);
     // joint motion
     bool set_workspace(std::string &frame_id,double x_min,double y_min, double z_min, double x_max, double y_max, double z_max);
     bool move_to(sensor_msgs::JointState &target_joints,double joint_tol=0.1);
diff --git a/src/arm_module.cpp b/src/arm_module.cpp
index 49488f233a2555ae8074ed835774d3287a7a342d..a84bcfec602352232bd6bd894cda99002155cba9 100644
--- a/src/arm_module.cpp
+++ b/src/arm_module.cpp
@@ -138,12 +138,6 @@ void CArmModule::reconfigure_callback(arm_module::ArmModuleConfig &config, uint3
   this->set_velocity_scale_factor(config.max_vel_scale);
   // workspace params
   this->set_workspace(config.ws_frame_id,config.ws_x_min,config.ws_y_min,config.ws_z_min,config.ws_x_max,config.ws_y_max,config.ws_z_max);
-  // constraints params
-  if (config.set_constraints) {
-    //constraint the arm_joint and torso_joint planning (defined at the .h)
-    this->reconfigure_constraints();
-    config.set_constraints = false;
-  }
 }
 
 void CArmModule::joint_states_callback(const sensor_msgs::JointState::ConstPtr& msg)
@@ -470,6 +464,7 @@ bool CArmModule::get_current_planning_scene(int component) {
 // constraint functions
 void CArmModule::clear_path_constraints(void)
 {
+  this->goal.request.goal_constraints[0].joint_constraints.clear();
   this->path_constraints.joint_constraints.clear();
   this->path_constraints.position_constraints.clear();
   this->path_constraints.orientation_constraints.clear();
@@ -557,6 +552,21 @@ void CArmModule::add_plane_path_constraint(std::vector<double> &normal,std::stri
 
 }
 
+void CArmModule::add_joint_goal_constraint(std::vector<std::string> &joint_names,std::vector<double> &position, std::vector<double> &tol)
+{
+  moveit_msgs::JointConstraint joint_const;
+
+  for(unsigned int i=0;i<joint_names.size();i++)
+  {
+    joint_const.joint_name=joint_names[i];
+    joint_const.position=position[i];
+    joint_const.tolerance_above=tol[i]/2.0;;
+    joint_const.tolerance_below=tol[i]/2.0;;
+    joint_const.weight=10.0;
+    this->goal.request.goal_constraints[0].joint_constraints.push_back(joint_const);
+  }
+}
+
 // joint motion
 bool CArmModule::set_workspace(std::string &frame_id,double x_min,double y_min, double z_min, double x_max, double y_max, double z_max)
 {
@@ -633,7 +643,6 @@ void CArmModule::move_to(geometry_msgs::PoseStamped &pose,double position_tol,do
   this->lock();
   if(!this->is_finished())
     this->stop();
-  this->goal.request.goal_constraints[0].joint_constraints.clear();
   this->goal.request.goal_constraints[0].position_constraints.resize(1);
   this->goal.request.goal_constraints[0].position_constraints[0].header=pose.header;
   this->goal.request.goal_constraints[0].position_constraints[0].link_name=this->config.end_effector_tool;
@@ -691,40 +700,6 @@ bool CArmModule::is_finished(void)
     return false;
 }
 
-void CArmModule::reconfigure_constraints() {
-
-    moveit::planning_interface::MoveGroupInterface group_arm_torso(PLANNING_GROUP);
-    moveit_msgs::Constraints constraints;
-
-    group_arm_torso.setStartStateToCurrentState();
-    //group_arm_torso.setMaxVelocityScalingFactor(1.0); //TODO REMOVE THIS; IS NOT RECOMMENDED TO BE USED (ONLY FOR SIM)
-
-    //Create joint constraint object, first we proceed with the arm_joint
-    moveit_msgs::JointConstraint jcm;
-    jcm.joint_name      = this->arm_joint;
-    jcm.position        = config.arm_joint_position;
-    jcm.tolerance_above = config.arm_tolerance_above;
-    jcm.tolerance_below = config.arm_tolerance_below;
-    jcm.weight = 1.0;
-
-    constraints.joint_constraints.push_back(jcm);
-
-    ROS_INFO("Constraining joint %s to position: %f , t_above: %f , t_below: %f", this->arm_joint.c_str(), config.arm_joint_position, config.arm_tolerance_above, config.arm_tolerance_below);
-   
-    jcm.joint_name      = this->torso_joint;
-    jcm.position        = config.torso_joint_position;
-    jcm.tolerance_above = config.torso_tolerance_above;
-    jcm.tolerance_below = config.torso_tolerance_below;
-    jcm.weight = 1.0;
-
-    constraints.joint_constraints.push_back(jcm);
-    
-    ROS_INFO("Constraining joint %s to position: %f , t_above: %f , t_below: %f", this->torso_joint.c_str(), config.torso_joint_position, config.torso_tolerance_above, config.torso_tolerance_below); 
-
-    group_arm_torso.setPathConstraints(constraints);
-    //Enables the defined constraints 
-}
-
 arm_module_status_t CArmModule::get_status(void)
 {
   return this->status;