diff --git a/include/tiago_arm_module/tiago_arm_bt_module.h b/include/tiago_arm_module/tiago_arm_bt_module.h
index f2a62b66c1fd8f1d68d1f40655acb46846849ad8..4b05272e1d5278781847cdd9fa3da80c9d70e1dc 100644
--- a/include/tiago_arm_module/tiago_arm_bt_module.h
+++ b/include/tiago_arm_module/tiago_arm_bt_module.h
@@ -418,6 +418,280 @@ class CTiagoArmModuleBT
       */
     BT::NodeStatus get_tiago_arm_velocity_scale_factor(BT::TreeNode& self);
 
+    /**
+      * \brief Synchronized add_object TIAGo arm function.
+      *
+      * This function calls add_object of tiago_arm_module.
+      *
+      * It has the following input ports:
+      *
+      *   name (std::string): The object name.
+      *
+      *   pose (geometry_msgs::PoseStamped): The object pose.
+      *
+      *   width (double): The object width.
+      *
+      *   length (double): The object length.
+      *
+      *   height (double): The object height.
+      *
+      * \return a BT:NodeStatus indicating whether the request has been
+      * successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
+      * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
+      *
+      */
+    BT::NodeStatus tiago_arm_add_object(BT::TreeNode& self);
+
+    /**
+      * \brief Synchronized update_object_pose TIAGo arm function.
+      *
+      * This function calls update_object_pose of tiago_arm_module.
+      *
+      * It has the following input ports:
+      *
+      *   name (std::string): The object name.
+      *
+      *   pose (geometry_msgs::PoseStamped): The new object pose.
+      *
+      * \return a BT:NodeStatus indicating whether the request has been
+      * successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
+      * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
+      *
+      */
+    BT::NodeStatus tiago_arm_update_object_pose(BT::TreeNode& self);
+
+    /**
+      * \brief Synchronized update_object_size TIAGo arm function.
+      *
+      * This function calls update_object_size of tiago_arm_module.
+      *
+      * It has the following input ports:
+      *
+      *   name (std::string): The object name.
+      *
+      *   width (double): The object width.
+      *
+      *   length (double): The object length.
+      *
+      *   height (double): The object height.
+      *
+      * \return a BT:NodeStatus indicating whether the request has been
+      * successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
+      * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
+      *
+      */
+    BT::NodeStatus tiago_arm_update_object_size(BT::TreeNode& self);
+
+    /**
+      * \brief Synchronized remove_object TIAGo arm function.
+      *
+      * This function calls remove_object of tiago_arm_module.
+      *
+      * It has the following input ports:
+      *
+      *   name (std::string): The object name.
+      *
+      * \return a BT:NodeStatus indicating whether the request has been
+      * successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
+      * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
+      *
+      */
+    BT::NodeStatus tiago_arm_remove_object(BT::TreeNode& self);
+
+    /**
+      * \brief Synchronized add_object_to_environment TIAGo arm function.
+      *
+      * This function calls add_object_to_environment of tiago_arm_module.
+      *
+      * It has the following input ports:
+      *
+      *   name (std::string): The object name.
+      *
+      * \return a BT:NodeStatus indicating whether the request has been
+      * successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
+      * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
+      *
+      */
+    BT::NodeStatus tiago_arm_add_object_to_environment(BT::TreeNode& self);
+
+    /**
+      * \brief Synchronized remove_object_from_environment TIAGo arm function.
+      *
+      * This function calls remove_object_from_environment of tiago_arm_module.
+      *
+      * It has the following input ports:
+      *
+      *   name (std::string): The object name.
+      *
+      * \return a BT:NodeStatus indicating whether the request has been
+      * successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
+      * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
+      *
+      */
+    BT::NodeStatus tiago_arm_remove_object_from_environment(BT::TreeNode& self);
+
+    /**
+      * \brief Synchronized attach_object_to_robot TIAGo arm function.
+      *
+      * This function calls attach_object_to_robot of tiago_arm_module.
+      *
+      * It has the following input ports:
+      *
+      *   name (std::string): The object name.
+      *
+      *   link (std::string): [Optional] The gripper link.
+      *
+      * \return a BT:NodeStatus indicating whether the request has been
+      * successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
+      * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
+      *
+      */
+    BT::NodeStatus tiago_arm_attach_object_to_robot(BT::TreeNode& self);
+
+    /**
+      * \brief Synchronized dettach_object_from_robot TIAGo arm function.
+      *
+      * This function calls dettach_object_from_robot of tiago_arm_module.
+      *
+      * It has the following input ports:
+      *
+      *   name (std::string): The object name.
+      *
+      * \return a BT:NodeStatus indicating whether the request has been
+      * successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
+      * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
+      *
+      */
+    BT::NodeStatus tiago_arm_dettach_object_from_robot(BT::TreeNode& self);
+
+    /**
+      * \brief Synchronized disable_collision TIAGo arm function.
+      *
+      * This function calls disable_collision of tiago_arm_module.
+      *
+      * It has the following input ports:
+      *
+      *   name (std::string): The object name.
+      *
+      * \return a BT:NodeStatus indicating whether the request has been
+      * successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
+      * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
+      *
+      */
+    BT::NodeStatus tiago_arm_disable_collision(BT::TreeNode& self);
+
+    /**
+      * \brief Synchronized get_current_planning_scene TIAGo arm function.
+      *
+      * This function calls get_current_planning_scene of tiago_arm_module.
+      *
+      * It has the following input ports:
+      *
+      *   component (int): Planing scene component.
+      *
+      * \return a BT:NodeStatus indicating whether the request has been
+      * successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
+      * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
+      *
+      */
+    BT::NodeStatus tiago_arm_get_current_planning_scene(BT::TreeNode& self);
+
+    /**
+      * \brief Synchronized clear_path_constraints TIAGo arm function.
+      *
+      * This function calls clear_path_constraints of tiago_arm_module. As this is a
+      * synchronous action is_tiago_arm_finished() must be used to know when the action
+      * has actually finished.
+      *
+      * \return a BT:NodeStatus indicating whether the request has been
+      * successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
+      * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
+      *
+      */
+    BT::NodeStatus tiago_arm_clear_path_constraints(void);
+
+    /**
+      * \brief Synchronized add_orientation_path_constraint TIAGo arm function.
+      *
+      * This function calls add_orientation_path_constraint of tiago_arm_module.
+      *
+      * It has the following input ports:
+      *
+      *   quat (geometry_msgs::QuaternionStamped): The orientation constrain.
+      *
+      *   orientation_tol (double): [Optional] The orientation constrain tolerance.
+      *
+      * \return a BT:NodeStatus indicating whether the request has been
+      * successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
+      * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
+      *
+      */
+    BT::NodeStatus tiago_arm_add_orientation_path_constraint(BT::TreeNode& self);
+
+    /**
+      * \brief Synchronized add_plane_path_constraint TIAGo arm function.
+      *
+      * This function calls add_plane_path_constraint of tiago_arm_module.
+      *
+      * It has the following input ports:
+      *
+      *   plane (path_const_plane_t): The plane definition.
+      *
+      *   frame_id (std::string): The plane frame_id.
+      *
+      *   offset (double): [Optional] The plane offset.
+      *
+      *   position_tol (double): [Optional] The position tolerance.
+      *
+      * \return a BT:NodeStatus indicating whether the request has been
+      * successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
+      * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
+      *
+      */
+    BT::NodeStatus tiago_arm_add_plane_path_constraint(BT::TreeNode& self);
+
+    /**
+      * \brief Synchronized add_plane_path_constraint TIAGo arm function.
+      *
+      * This function calls add_plane_path_constraint of tiago_arm_module.
+      *
+      * It has the following input ports:
+      *
+      *   normal (std::vector<double>): The plane definition.
+      *
+      *   frame_id (std::string): The plane frame_id.
+      *
+      *   offset (double): [Optional] The plane offset.
+      *
+      *   position_tol (double): [Optional] The position tolerance.
+      *
+      * \return a BT:NodeStatus indicating whether the request has been
+      * successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
+      * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
+      *
+      */
+    BT::NodeStatus tiago_arm_add_plane_path_constraint_normal(BT::TreeNode& self);
+
+    /**
+      * \brief Synchronized add_joint_goal_constraint TIAGo arm function.
+      *
+      * This function calls add_joint_goal_constraint of tiago_arm_module.
+      *
+      * It has the following input ports:
+      *
+      *   joint_names (std::vector<std::string>): The joint names.
+      *
+      *   position (std::vector<double>): The joint positions.
+      *
+      *   tol (std::vector<double>): The joints tolerance.
+      *
+      * \return a BT:NodeStatus indicating whether the request has been
+      * successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
+      * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
+      *
+      */
+    BT::NodeStatus tiago_arm_add_joint_goal_constraint(BT::TreeNode& self);
+
     /**
       * \brief Synchronized stop TIAGo arm function.
       *
diff --git a/src/tiago_arm_bt_module.cpp b/src/tiago_arm_bt_module.cpp
index 8270a5b01046324aece51c7f74aa7aaec3f2a8df..ac6b40d5d5c41d5e6ac30d9aaad493ad920fb731 100644
--- a/src/tiago_arm_bt_module.cpp
+++ b/src/tiago_arm_bt_module.cpp
@@ -45,6 +45,16 @@ void CTiagoArmModuleBT::init(IriBehaviorTreeFactory &factory)
   BT::PortsList set_velocity_scale_factor_ports = {BT::InputPort<double>("scale")};
   BT::PortsList get_velocity_scale_factor_ports = {BT::OutputPort<double>("scale")};
 
+  BT::PortsList add_object_ports = {BT::InputPort<std::string>("name"), BT::InputPort<geometry_msgs::PoseStamped>("pose"), BT::InputPort<double>("width"), BT::InputPort<double>("length"), BT::InputPort<double>("height")};
+  BT::PortsList update_object_pose_ports = {BT::InputPort<std::string>("name"), BT::InputPort<geometry_msgs::PoseStamped>("pose")};
+  BT::PortsList update_object_size_ports = {BT::InputPort<std::string>("name"), BT::InputPort<double>("width"), BT::InputPort<double>("length"), BT::InputPort<double>("height")};
+  BT::PortsList name_port = {BT::InputPort<std::string>("name")};
+  BT::PortsList attach_object_to_robot_ports = {BT::InputPort<std::string>("name"), BT::InputPort<std::string>("link")};
+  BT::PortsList get_current_scene_ports = {BT::InputPort<int>("component")};
+  BT::PortsList add_orientation_path_constraints_ports = {BT::InputPort<geometry_msgs::QuaternionStamped>("quat"), BT::InputPort<double>("orientation_tol")};
+  BT::PortsList add_plane_path_constraints_ports = {BT::InputPort<path_const_plane_t>("plane"), BT::InputPort<std::string>("frame_id"), BT::InputPort<double>("offset"), BT::InputPort<double>("position_tol")};
+  BT::PortsList add_plane_path_constraints_normal_ports = {BT::InputPort<std::vector<double> >("normal"), BT::InputPort<std::string>("frame_id"), BT::InputPort<double>("offset"), BT::InputPort<double>("position_tol")};
+  BT::PortsList tiago_arm_add_joint_goal_constraint_ports = {BT::InputPort<std::vector<std::string> >("joint_names"), BT::InputPort<std::vector<double> >("position"), BT::InputPort<std::vector<double> >("tol")};
 
 
   //Registration of conditions
@@ -95,6 +105,20 @@ void CTiagoArmModuleBT::init(IriBehaviorTreeFactory &factory)
   factory.registerSimpleAction("set_tiago_arm_velocity_scale_factor",  std::bind(&CTiagoArmModuleBT::set_tiago_arm_velocity_scale_factor, this, std::placeholders::_1), set_velocity_scale_factor_ports);
   factory.registerSimpleAction("get_tiago_arm_velocity_scale_factor",  std::bind(&CTiagoArmModuleBT::get_tiago_arm_velocity_scale_factor, this, std::placeholders::_1), get_velocity_scale_factor_ports);
 
+  factory.registerSimpleAction("tiago_arm_add_object",  std::bind(&CTiagoArmModuleBT::tiago_arm_add_object, this, std::placeholders::_1), add_object_ports);
+  factory.registerSimpleAction("tiago_arm_update_object_pose",  std::bind(&CTiagoArmModuleBT::tiago_arm_update_object_pose, this, std::placeholders::_1), update_object_pose_ports);
+  factory.registerSimpleAction("tiago_arm_update_object_size",  std::bind(&CTiagoArmModuleBT::tiago_arm_update_object_size, this, std::placeholders::_1), update_object_size_ports);
+  factory.registerSimpleAction("tiago_arm_remove_object",  std::bind(&CTiagoArmModuleBT::tiago_arm_remove_object, this, std::placeholders::_1), name_port);
+  factory.registerSimpleAction("tiago_arm_add_object_to_environment",  std::bind(&CTiagoArmModuleBT::tiago_arm_add_object_to_environment, this, std::placeholders::_1), name_port);
+  factory.registerSimpleAction("tiago_arm_remove_object_from_environment",  std::bind(&CTiagoArmModuleBT::tiago_arm_remove_object_from_environment, this, std::placeholders::_1), name_port);
+  factory.registerSimpleAction("tiago_arm_attach_object_to_robot",  std::bind(&CTiagoArmModuleBT::tiago_arm_attach_object_to_robot, this, std::placeholders::_1), attach_object_to_robot_ports);
+  factory.registerSimpleAction("tiago_arm_dettach_object_from_robot",  std::bind(&CTiagoArmModuleBT::tiago_arm_dettach_object_from_robot, this, std::placeholders::_1), name_port);
+  factory.registerSimpleAction("tiago_arm_disable_collision",  std::bind(&CTiagoArmModuleBT::tiago_arm_disable_collision, this, std::placeholders::_1), name_port);
+  factory.registerSimpleAction("tiago_arm_get_current_planning_scene",  std::bind(&CTiagoArmModuleBT::tiago_arm_get_current_planning_scene, this, std::placeholders::_1), get_current_scene_ports);
+  factory.registerSimpleAction("tiago_arm_add_orientation_path_constraint",  std::bind(&CTiagoArmModuleBT::tiago_arm_add_orientation_path_constraint, this, std::placeholders::_1), add_orientation_path_constraints_ports);
+  factory.registerSimpleAction("tiago_arm_add_plane_path_constraint",  std::bind(&CTiagoArmModuleBT::tiago_arm_add_plane_path_constraint, this, std::placeholders::_1), add_plane_path_constraints_ports);
+  factory.registerSimpleAction("tiago_arm_add_plane_path_constraint_normal",  std::bind(&CTiagoArmModuleBT::tiago_arm_add_plane_path_constraint_normal, this, std::placeholders::_1), add_plane_path_constraints_normal_ports);
+  factory.registerSimpleAction("tiago_arm_add_joint_goal_constraint",  std::bind(&CTiagoArmModuleBT::tiago_arm_add_joint_goal_constraint, this, std::placeholders::_1), tiago_arm_add_joint_goal_constraint_ports);
 
 }
 
@@ -487,6 +511,354 @@ BT::NodeStatus CTiagoArmModuleBT::get_tiago_arm_velocity_scale_factor(BT::TreeNo
   return BT::NodeStatus::SUCCESS;
 }
 
+BT::NodeStatus CTiagoArmModuleBT::tiago_arm_add_object(BT::TreeNode& self)
+{
+  ROS_DEBUG("CTiagoArmModuleBT::tiago_arm_add_object-> tiago_arm_add_object");
+  BT::Optional<std::string> name = self.getInput<std::string>("name");
+  BT::Optional<geometry_msgs::PoseStamped> pose = self.getInput<geometry_msgs::PoseStamped>("pose");
+  BT::Optional<double> width = self.getInput<double>("width");
+  BT::Optional<double> length = self.getInput<double>("length");
+  BT::Optional<double> height = self.getInput<double>("height");
+
+  double width_goal, length_goal, height_goal;
+  std::string name_goal;
+  geometry_msgs::PoseStamped pose_goal;
+  if (!name || !pose || !width || !length || !height)
+  {
+    ROS_ERROR("CTiagoArmModuleBT::tiago_arm_add_object-> Incorrect or missing input. It needs the following input ports: name(std::string), pose(geometry_msgs::PoseStamped), width(double), length(double), height(double)");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  name_goal = name.value();
+  pose_goal = pose.value();
+  width_goal = width.value();
+  length_goal = length.value();
+  height_goal = height.value();
+
+  this->tiago_arm_module.add_object(name_goal, pose_goal, width_goal, length_goal, height_goal);
+  return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus CTiagoArmModuleBT::tiago_arm_update_object_pose(BT::TreeNode& self)
+{
+  ROS_DEBUG("CTiagoArmModuleBT::tiago_arm_update_object_pose-> tiago_arm_update_object_pose");
+  BT::Optional<std::string> name = self.getInput<std::string>("name");
+  BT::Optional<geometry_msgs::PoseStamped> pose = self.getInput<geometry_msgs::PoseStamped>("pose");
+
+  std::string name_goal;
+  geometry_msgs::PoseStamped pose_goal;
+  if (!name || !pose)
+  {
+    ROS_ERROR("CTiagoArmModuleBT::tiago_arm_update_object_pose-> Incorrect or missing input. It needs the following input ports: name(std::string), pose(geometry_msgs::PoseStamped)");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  name_goal = name.value();
+  pose_goal = pose.value();
+
+  this->tiago_arm_module.update_object_pose(name_goal, pose_goal);
+  return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus CTiagoArmModuleBT::tiago_arm_update_object_size(BT::TreeNode& self)
+{
+  ROS_DEBUG("CTiagoArmModuleBT::tiago_arm_update_object_size-> tiago_arm_update_object_size");
+  BT::Optional<std::string> name = self.getInput<std::string>("name");
+  BT::Optional<double> width = self.getInput<double>("width");
+  BT::Optional<double> length = self.getInput<double>("length");
+  BT::Optional<double> height = self.getInput<double>("height");
+
+  double width_goal, length_goal, height_goal;
+  std::string name_goal;
+  if (!name || !width || !length || !height)
+  {
+    ROS_ERROR("CTiagoArmModuleBT::tiago_arm_update_object_size-> Incorrect or missing input. It needs the following input ports: name(std::string), width(double), length(double), height(double)");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  name_goal = name.value();
+  width_goal = width.value();
+  length_goal = length.value();
+  height_goal = height.value();
+
+  this->tiago_arm_module.update_object_size(name_goal, width_goal, length_goal, height_goal);
+  return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus CTiagoArmModuleBT::tiago_arm_remove_object(BT::TreeNode& self)
+{
+  ROS_DEBUG("CTiagoArmModuleBT::tiago_arm_remove_object-> tiago_arm_remove_object");
+  BT::Optional<std::string> name = self.getInput<std::string>("name");
+
+  std::string name_goal;
+  if (!name)
+  {
+    ROS_ERROR("CTiagoArmModuleBT::tiago_arm_remove_object-> Incorrect or missing input. It needs the following input ports: name(std::string)");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  name_goal = name.value();
+
+  if (this->tiago_arm_module.remove_object(name_goal))
+    return BT::NodeStatus::SUCCESS;
+  else
+    return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CTiagoArmModuleBT::tiago_arm_add_object_to_environment(BT::TreeNode& self)
+{
+  ROS_DEBUG("CTiagoArmModuleBT::tiago_arm_add_object_to_enviroment-> tiago_arm_add_object_to_enviroment");
+  BT::Optional<std::string> name = self.getInput<std::string>("name");
+
+  std::string name_goal;
+  if (!name)
+  {
+    ROS_ERROR("CTiagoArmModuleBT::tiago_arm_add_object_to_enviroment-> Incorrect or missing input. It needs the following input ports: name(std::string)");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  name_goal = name.value();
+
+  this->tiago_arm_module.add_object_to_environment(name_goal);
+  return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus CTiagoArmModuleBT::tiago_arm_remove_object_from_environment(BT::TreeNode& self)
+{
+  ROS_DEBUG("CTiagoArmModuleBT::tiago_arm_remove_object_to_enviroment-> tiago_arm_remove_object_to_enviroment");
+  BT::Optional<std::string> name = self.getInput<std::string>("name");
+
+  std::string name_goal;
+  if (!name)
+  {
+    ROS_ERROR("CTiagoArmModuleBT::tiago_arm_remove_object_to_enviroment-> Incorrect or missing input. It needs the following input ports: name(std::string)");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  name_goal = name.value();
+
+  if (this->tiago_arm_module.remove_object_from_environment(name_goal))
+    return BT::NodeStatus::SUCCESS;
+  else
+    return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CTiagoArmModuleBT::tiago_arm_attach_object_to_robot(BT::TreeNode& self)
+{
+  ROS_DEBUG("CTiagoArmModuleBT::tiago_arm_attach_object_to_robot-> tiago_arm_attach_object_to_robot");
+  BT::Optional<std::string> name = self.getInput<std::string>("name");
+  BT::Optional<std::string> link = self.getInput<std::string>("link");
+
+  std::string name_goal;
+  if (!name)
+  {
+    ROS_ERROR("CTiagoArmModuleBT::tiago_arm_attach_object_to_robot-> Incorrect or missing input. It needs the following input ports: name(std::string) and [Optional] link(std::string)");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  name_goal = name.value();
+
+  if (!link)
+    this->tiago_arm_module.attach_object_to_robot(name_goal);
+  else
+  {
+    std::string link_goal = link.value();
+    this->tiago_arm_module.attach_object_to_robot(name_goal, link_goal);
+  }
+  return BT::NodeStatus::SUCCESS;
+}
+
+
+BT::NodeStatus CTiagoArmModuleBT::tiago_arm_dettach_object_from_robot(BT::TreeNode& self)
+{
+  ROS_DEBUG("CTiagoArmModuleBT::tiago_arm_dettach_object_from_robot-> tiago_arm_dettach_object_from_robot");
+  BT::Optional<std::string> name = self.getInput<std::string>("name");
+
+  std::string name_goal;
+  if (!name)
+  {
+    ROS_ERROR("CTiagoArmModuleBT::tiago_arm_dettach_object_from_robot-> Incorrect or missing input. It needs the following input ports: name(std::string)");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  name_goal = name.value();
+
+  if (this->tiago_arm_module.dettach_object_from_robot(name_goal))
+    return BT::NodeStatus::SUCCESS;
+  else
+    return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CTiagoArmModuleBT::tiago_arm_disable_collision(BT::TreeNode& self)
+{
+  ROS_DEBUG("CTiagoArmModuleBT::tiago_arm_disable_collision-> tiago_arm_disable_collision");
+  BT::Optional<std::string> name = self.getInput<std::string>("name");
+
+  std::string name_goal;
+  if (!name)
+  {
+    ROS_ERROR("CTiagoArmModuleBT::tiago_arm_disable_collision-> Incorrect or missing input. It needs the following input ports: name(std::string)");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  name_goal = name.value();
+
+  if (this->tiago_arm_module.disable_collision(name_goal))
+    return BT::NodeStatus::SUCCESS;
+  else
+    return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CTiagoArmModuleBT::tiago_arm_get_current_planning_scene(BT::TreeNode& self)
+{
+  ROS_DEBUG("CTiagoArmModuleBT::tiago_arm_get_current_planning_scene-> tiago_arm_get_current_planning_scene");
+  BT::Optional<int> component = self.getInput<int>("component");
+
+  int component_goal;
+  if (!component)
+  {
+    ROS_ERROR("CTiagoArmModuleBT::tiago_arm_get_current_planning_scene-> Incorrect or missing input. It needs the following input ports: component(int)");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  component_goal = component.value();
+
+  if (this->tiago_arm_module.get_current_planning_scene(component_goal))
+    return BT::NodeStatus::SUCCESS;
+  else
+    return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus CTiagoArmModuleBT::tiago_arm_clear_path_constraints(void)
+{
+  ROS_DEBUG("CTiagoArmModuleBT::tiago_arm_clear_path_constraints-> tiago_arm_clear_path_constraints");
+  this->tiago_arm_module.clear_path_constraints();
+  return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus CTiagoArmModuleBT::tiago_arm_add_orientation_path_constraint(BT::TreeNode& self)
+{
+  ROS_DEBUG("CTiagoArmModuleBT::tiago_arm_add_orientation_path_constraint-> tiago_arm_add_orientation_path_constraint");
+  BT::Optional<geometry_msgs::QuaternionStamped> quat = self.getInput<geometry_msgs::QuaternionStamped>("quat");
+  BT::Optional<double> orientation_tol = self.getInput<double>("orientation_tol");
+
+  geometry_msgs::QuaternionStamped quat_goal;
+  if (!quat)
+  {
+    ROS_ERROR("CTiagoArmModuleBT::tiago_arm_add_orientation_path_constraint-> Incorrect or missing input. It needs the following input ports: quat(geometry_msgs::QuaternionStamped) and [Optional] orientation_tol(double)");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  quat_goal = quat.value();
+  if (!orientation_tol)
+    this->tiago_arm_module.add_orientation_path_constraint(quat_goal);
+  else
+  {
+    double orientation_tol_goal = orientation_tol.value();
+    this->tiago_arm_module.add_orientation_path_constraint(quat_goal, orientation_tol_goal);
+  }
+  return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus CTiagoArmModuleBT::tiago_arm_add_plane_path_constraint(BT::TreeNode& self)
+{
+  ROS_DEBUG("CTiagoArmModuleBT::tiago_arm_add_orientation_path_constraint-> tiago_arm_add_orientation_path_constraint");
+  BT::Optional<path_const_plane_t> plane = self.getInput<path_const_plane_t>("plane");
+  BT::Optional<std::string> frame_id = self.getInput<std::string>("frame_id");
+  BT::Optional<double> offset = self.getInput<double>("offset");
+  BT::Optional<double> position_tol = self.getInput<double>("position_tol");
+
+  path_const_plane_t plane_goal;
+  std::string frame_id_goal;
+  double offset_goal;
+  if (!plane || !frame_id || (!offset && position_tol))
+  {
+    ROS_ERROR("CTiagoArmModuleBT::tiago_arm_add_orientation_path_constraint-> Incorrect or missing input. It needs the following input ports: plane(geometry_msgs::QuaternionStamped), frame_id(geometry_msgs::QuaternionStamped), [Optional] offset(double) and [Optional] position_tol(double)");
+    ROS_ERROR("CTiagoArmModuleBT::tiago_arm_add_orientation_path_constraint-> The correct input options are: plane + frame_id, plane + frame_id + offset or plane + frame_id + offset + position_tol");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  plane_goal = plane.value();
+  frame_id_goal = frame_id.value();
+  if (!position_tol)
+  {
+    if (!offset)
+      this->tiago_arm_module.add_plane_path_constraint(plane_goal, frame_id_goal);
+    else
+    {
+      offset_goal = offset.value();
+      this->tiago_arm_module.add_plane_path_constraint(plane_goal, frame_id_goal, offset_goal);
+    }
+  }
+  else
+  {
+    double position_tol_goal = position_tol.value();
+    this->tiago_arm_module.add_plane_path_constraint(plane_goal, frame_id_goal, offset_goal, position_tol_goal);
+  }
+  return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus CTiagoArmModuleBT::tiago_arm_add_plane_path_constraint_normal(BT::TreeNode& self)
+{
+  ROS_DEBUG("CTiagoArmModuleBT::tiago_arm_add_plane_path_constraint_normal-> tiago_arm_add_plane_path_constraint_normal");
+  BT::Optional<std::vector<double> > normal = self.getInput<std::vector<double> >("normal");
+  BT::Optional<std::string> frame_id = self.getInput<std::string>("frame_id");
+  BT::Optional<double> offset = self.getInput<double>("offset");
+  BT::Optional<double> position_tol = self.getInput<double>("position_tol");
+
+  std::vector<double> normal_goal;
+  std::string frame_id_goal;
+  double offset_goal;
+  if (!normal || !frame_id || (!offset && position_tol))
+  {
+    ROS_ERROR("CTiagoArmModuleBT::tiago_arm_add_plane_path_constraint_normal-> Incorrect or missing input. It needs the following input ports: normal(geometry_msgs::QuaternionStamped), frame_id(geometry_msgs::QuaternionStamped), [Optional] offset(double) and [Optional] position_tol(double)");
+    ROS_ERROR("CTiagoArmModuleBT::tiago_arm_add_plane_path_constraint_normal-> The correct input options are: normal + frame_id, normal + frame_id + offset or normal + frame_id + offset + position_tol");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  normal_goal = normal.value();
+  frame_id_goal = frame_id.value();
+  if (!position_tol)
+  {
+    if (!offset)
+      this->tiago_arm_module.add_plane_path_constraint(normal_goal, frame_id_goal);
+    else
+    {
+      offset_goal = offset.value();
+      this->tiago_arm_module.add_plane_path_constraint(normal_goal, frame_id_goal, offset_goal);
+    }
+  }
+  else
+  {
+    double position_tol_goal = position_tol.value();
+    this->tiago_arm_module.add_plane_path_constraint(normal_goal, frame_id_goal, offset_goal, position_tol_goal);
+  }
+  return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus CTiagoArmModuleBT::tiago_arm_add_joint_goal_constraint(BT::TreeNode& self)
+{
+  ROS_DEBUG("CTiagoArmModuleBT::tiago_arm_add_joint_goal_constraint-> tiago_arm_add_joint_goal_constraint");
+  BT::Optional<std::vector<std::string> > joint_names = self.getInput<std::vector<std::string> >("joint_names");
+  BT::Optional<std::vector<double> > position = self.getInput<std::vector<double> >("position");
+  BT::Optional<std::vector<double> > tol = self.getInput<std::vector<double> >("tol");
+
+  std::vector<std::string> joint_names_goal;
+  std::vector<double> position_goal, tol_goal;
+  if (!joint_names || !position || !tol)
+  {
+    ROS_ERROR("CTiagoArmModuleBT::tiago_arm_add_joint_goal_constraint-> Incorrect or missing input. It needs the following input ports: joint_names(std::vector<std::string>), position(std::vector<double>) and tol(std::vector<double>)");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  joint_names_goal = joint_names.value();
+  position_goal = position.value();
+  tol_goal = tol.value();
+
+  this->tiago_arm_module.add_joint_goal_constraint(joint_names_goal, position_goal, tol_goal);
+  return BT::NodeStatus::SUCCESS;
+}
+
 BT::NodeStatus CTiagoArmModuleBT::sync_cancel_tiago_arm_action(void)
 {
   ROS_DEBUG("CTiagoArmModuleBT::sync_cancel_tiago_arm_action-> sync_cancel_tiago_arm_action");