diff --git a/cartesian_controller_base/CMakeLists.txt b/cartesian_controller_base/CMakeLists.txt
index cb23fe3d..0da87be1 100644
--- a/cartesian_controller_base/CMakeLists.txt
+++ b/cartesian_controller_base/CMakeLists.txt
@@ -22,9 +22,11 @@ find_package(catkin REQUIRED COMPONENTS
   roscpp
   controller_interface
   kdl_parser
+  geometry_msgs
   trajectory_msgs
   control_toolbox
   eigen_conversions
+  kdl_conversions
   dynamic_reconfigure
   pluginlib
 )
@@ -70,11 +72,9 @@ find_package(catkin REQUIRED COMPONENTS
 # )
 
 ## Generate services in the 'srv' folder
-# add_service_files(
-#   FILES
-#   Service1.srv
-#   Service2.srv
-# )
+add_service_files(DIRECTORY srv FILES
+  SetKinematicChainOffsets.srv
+)
 
 ## Generate actions in the 'action' folder
 # add_action_files(
@@ -84,10 +84,10 @@ find_package(catkin REQUIRED COMPONENTS
 # )
 
 ## Generate added messages and services with any dependencies listed here
-# generate_messages(
-#   DEPENDENCIES
-#   std_msgs  # Or other packages containing msgs
-# )
+generate_messages(
+   DEPENDENCIES
+   geometry_msgs
+)
 
 ################################################
 ## Declare ROS dynamic reconfigure parameters ##
@@ -123,7 +123,7 @@ generate_dynamic_reconfigure_options(
 catkin_package(
   INCLUDE_DIRS include
   LIBRARIES cartesian_controller_base ik_solvers
-  CATKIN_DEPENDS roscpp controller_interface kdl_parser trajectory_msgs control_toolbox eigen_conversions dynamic_reconfigure pluginlib
+  CATKIN_DEPENDS roscpp controller_interface kdl_parser geometry_msgs trajectory_msgs control_toolbox kdl_conversions eigen_conversions dynamic_reconfigure pluginlib
 #  DEPENDS system_lib
 )
 
diff --git a/cartesian_controller_base/include/cartesian_controller_base/DampedLeastSquaresSolver.h b/cartesian_controller_base/include/cartesian_controller_base/DampedLeastSquaresSolver.h
index 4449af96..7db1a324 100644
--- a/cartesian_controller_base/include/cartesian_controller_base/DampedLeastSquaresSolver.h
+++ b/cartesian_controller_base/include/cartesian_controller_base/DampedLeastSquaresSolver.h
@@ -102,6 +102,11 @@ class DampedLeastSquaresSolver : public IKSolver
               const KDL::JntArray& upper_pos_limits,
               const KDL::JntArray& lower_pos_limits);
 
+    /**
+     * @brief Update the robot kinematic chain of the solver
+     */
+    virtual bool updateChain(const KDL::Chain& chain);
+
   private:
     std::shared_ptr<KDL::ChainJntToJacSolver> m_jnt_jacobian_solver;
     KDL::Jacobian m_jnt_jacobian;
diff --git a/cartesian_controller_base/include/cartesian_controller_base/ForwardDynamicsSolver.h b/cartesian_controller_base/include/cartesian_controller_base/ForwardDynamicsSolver.h
index fd287ab7..34fb95f7 100644
--- a/cartesian_controller_base/include/cartesian_controller_base/ForwardDynamicsSolver.h
+++ b/cartesian_controller_base/include/cartesian_controller_base/ForwardDynamicsSolver.h
@@ -123,6 +123,11 @@ class ForwardDynamicsSolver : public IKSolver
               const KDL::JntArray& upper_pos_limits,
               const KDL::JntArray& lower_pos_limits);
 
+    /**
+     * @brief Update the robot kinematic chain of the solver
+     */
+    bool updateChain(const KDL::Chain& chain);
+
   private:
 
     //! Build a generic robot model for control
diff --git a/cartesian_controller_base/include/cartesian_controller_base/IKSolver.h b/cartesian_controller_base/include/cartesian_controller_base/IKSolver.h
index 6a0262c9..0901b517 100644
--- a/cartesian_controller_base/include/cartesian_controller_base/IKSolver.h
+++ b/cartesian_controller_base/include/cartesian_controller_base/IKSolver.h
@@ -151,6 +151,11 @@ class IKSolver
                       const KDL::JntArray& upper_pos_limits,
                       const KDL::JntArray& lower_pos_limits);
 
+    /**
+     * @brief Update the robot kinematic chain of the solver
+     */
+    virtual bool updateChain(const KDL::Chain& chain);
+
     /**
      * @brief Update the robot kinematics of the solver
      *
diff --git a/cartesian_controller_base/include/cartesian_controller_base/JacobianTransposeSolver.h b/cartesian_controller_base/include/cartesian_controller_base/JacobianTransposeSolver.h
index db765370..05a6d71d 100644
--- a/cartesian_controller_base/include/cartesian_controller_base/JacobianTransposeSolver.h
+++ b/cartesian_controller_base/include/cartesian_controller_base/JacobianTransposeSolver.h
@@ -95,6 +95,11 @@ class JacobianTransposeSolver : public IKSolver
               const KDL::JntArray& upper_pos_limits,
               const KDL::JntArray& lower_pos_limits);
 
+    /**
+     * @brief Update the robot kinematic chain of the solver
+     */
+    virtual bool updateChain(const KDL::Chain& chain);
+
   private:
     std::shared_ptr<KDL::ChainJntToJacSolver> m_jnt_jacobian_solver;
     KDL::Jacobian m_jnt_jacobian;
diff --git a/cartesian_controller_base/include/cartesian_controller_base/SelectivelyDampedLeastSquaresSolver.h b/cartesian_controller_base/include/cartesian_controller_base/SelectivelyDampedLeastSquaresSolver.h
index 07ec0bcf..5f82b49f 100644
--- a/cartesian_controller_base/include/cartesian_controller_base/SelectivelyDampedLeastSquaresSolver.h
+++ b/cartesian_controller_base/include/cartesian_controller_base/SelectivelyDampedLeastSquaresSolver.h
@@ -101,6 +101,11 @@ class SelectivelyDampedLeastSquaresSolver : public IKSolver
               const KDL::JntArray& upper_pos_limits,
               const KDL::JntArray& lower_pos_limits);
 
+    /**
+     * @brief Update the robot kinematic chain of the solver
+     */
+    bool updateChain(const KDL::Chain& chain);
+
   private:
     /**
      * @brief Helper function to clamp a column vector
diff --git a/cartesian_controller_base/include/cartesian_controller_base/cartesian_controller_base.h b/cartesian_controller_base/include/cartesian_controller_base/cartesian_controller_base.h
index 59186d39..96b48844 100644
--- a/cartesian_controller_base/include/cartesian_controller_base/cartesian_controller_base.h
+++ b/cartesian_controller_base/include/cartesian_controller_base/cartesian_controller_base.h
@@ -59,6 +59,7 @@
 #include <cartesian_controller_base/IKSolver.h>
 #include <cartesian_controller_base/SpatialPDController.h>
 #include <cartesian_controller_base/Utility.h>
+#include <cartesian_controller_base/SetKinematicChainOffsets.h>
 
 // Dynamic reconfigure
 #include <dynamic_reconfigure/server.h>
@@ -200,6 +201,20 @@ class CartesianControllerBase : public controller_interface::Controller<Hardware
     void publishStateFeedback();
 
   private:
+    /**
+     * @brief Allow users to add a chain segment before and/or after the robot chain
+     *
+     * Before:
+     * Empty segment if frame_id is empty, otherwise consist of a fixed segment that will
+     * become the parent of the robot base link.
+     *
+     * After:
+     * Empty segment if frame_id is empty,otherwise consist of a fixed segment that will
+     * be attached to the end effector link.
+     */
+    bool signalChainOffsetsCallback(SetKinematicChainOffsets::Request& req, SetKinematicChainOffsets::Response& res);
+
+    ros::ServiceServer    m_signal_chain_offsets_server;
     std::vector<std::string>                          m_joint_names;
     trajectory_msgs::JointTrajectoryPoint             m_simulated_joint_motion;
     SpatialPDController                              m_spatial_controller;
diff --git a/cartesian_controller_base/include/cartesian_controller_base/cartesian_controller_base.hpp b/cartesian_controller_base/include/cartesian_controller_base/cartesian_controller_base.hpp
index 1dabbc6b..3b3c0629 100644
--- a/cartesian_controller_base/include/cartesian_controller_base/cartesian_controller_base.hpp
+++ b/cartesian_controller_base/include/cartesian_controller_base/cartesian_controller_base.hpp
@@ -48,6 +48,7 @@
 #include <kdl/tree.hpp>
 #include <kdl_parser/kdl_parser.hpp>
 #include <kdl/jntarray.hpp>
+#include <kdl_conversions/kdl_msg.h>
 
 // URDF
 #include <urdf/model.h>
@@ -185,6 +186,9 @@ init(HardwareInterface* hw, ros::NodeHandle& nh)
   // Initialize Cartesian pd controllers
   m_spatial_controller.init(nh);
 
+  // Update chain offsets service
+  m_signal_chain_offsets_server = nh.advertiseService("signal_chain_offsets",&CartesianControllerBase<HardwareInterface>::signalChainOffsetsCallback,this);
+
   // Controller-internal state publishing
   m_feedback_pose_publisher =
     std::make_shared<realtime_tools::RealtimePublisher<geometry_msgs::PoseStamped> >(
@@ -365,6 +369,61 @@ displayInTipLink(const ctrl::Vector6D& vector, const std::string& to)
   return out;
 }
 
+template <class HardwareInterface>
+bool CartesianControllerBase<HardwareInterface>::
+signalChainOffsetsCallback(SetKinematicChainOffsets::Request& req, SetKinematicChainOffsets::Response& res)
+{
+  KDL::Chain new_chain;
+
+  // Add pre chain segment
+  if(!req.pre_robot_chain_offset.header.frame_id.empty())
+  {
+    KDL::Frame pre_chain_frame;
+    tf::poseMsgToKDL(req.pre_robot_chain_offset.pose, pre_chain_frame);
+
+    KDL::Segment pre_segment = KDL::Segment(req.pre_robot_chain_offset.header.frame_id,
+                                            KDL::Joint(KDL::Joint::None),
+                                            pre_chain_frame);
+
+    new_chain.addSegment(pre_segment);
+  }
+
+  // Add robot chain
+  new_chain.addChain(m_robot_chain);
+
+  // Add post chain segment
+  if(!req.post_robot_chain_offset.header.frame_id.empty())
+  {
+    KDL::Frame post_chain_frame;
+    tf::poseMsgToKDL(req.post_robot_chain_offset.pose, post_chain_frame);
+
+    KDL::Segment post_segment = KDL::Segment(req.post_robot_chain_offset.header.frame_id,
+                                            KDL::Joint(KDL::Joint::None),
+                                            post_chain_frame);
+
+    new_chain.addSegment(post_segment);
+  }
+
+  // Update solvers with new chain
+  if(!m_ik_solver->updateChain(new_chain))
+  {
+    res.message = "Failed to update the kinematic chain in the ik solver.";
+    res.success = false;
+
+    return false;
+  }
+
+  KDL::Tree tmp("not_relevant");
+  tmp.addChain(new_chain,"not_relevant");
+  m_forward_kinematics_solver.reset(new KDL::TreeFkSolverPos_recursive(tmp));
+
+  // Fill service result
+  res.message = "Got it.";
+  res.success = true;
+
+  return true;
+}
+
 template <class HardwareInterface>
 void CartesianControllerBase<HardwareInterface>::
 publishStateFeedback()
diff --git a/cartesian_controller_base/package.xml b/cartesian_controller_base/package.xml
index 82ce7800..a66df31b 100644
--- a/cartesian_controller_base/package.xml
+++ b/cartesian_controller_base/package.xml
@@ -42,18 +42,22 @@
   <build_depend>roscpp</build_depend>
   <build_depend>controller_interface</build_depend>
   <build_depend>kdl_parser</build_depend>
+  <build_depend>geometry_msgs</build_depend>
   <build_depend>trajectory_msgs</build_depend>
   <build_depend>control_toolbox</build_depend>
   <build_depend>eigen_conversions</build_depend>
+  <build_depend>kdl_conversions</build_depend>
   <build_depend>dynamic_reconfigure</build_depend>
   <build_depend>pluginlib</build_depend>
 
   <run_depend>roscpp</run_depend>
   <run_depend>controller_interface</run_depend>
   <run_depend>kdl_parser</run_depend>
+  <run_depend>geometry_msgs</run_depend>
   <run_depend>trajectory_msgs</run_depend>
   <run_depend>control_toolbox</run_depend>
   <run_depend>eigen_conversions</run_depend>
+  <run_depend>kdl_conversions</run_depend>
   <run_depend>dynamic_reconfigure</run_depend>
   <run_depend>pluginlib</run_depend>
 
diff --git a/cartesian_controller_base/src/DampedLeastSquaresSolver.cpp b/cartesian_controller_base/src/DampedLeastSquaresSolver.cpp
index fe52d75d..38d304cb 100644
--- a/cartesian_controller_base/src/DampedLeastSquaresSolver.cpp
+++ b/cartesian_controller_base/src/DampedLeastSquaresSolver.cpp
@@ -125,8 +125,7 @@ namespace cartesian_controller_base{
   {
     IKSolver::init(nh, chain, upper_pos_limits, lower_pos_limits);
 
-    m_jnt_jacobian_solver.reset(new KDL::ChainJntToJacSolver(m_chain));
-    m_jnt_jacobian.resize(m_number_joints);
+    updateChain(m_chain);
 
     // Connect dynamic reconfigure and overwrite the default values with values
     // on the parameter server. This is done automatically if parameters with
@@ -143,6 +142,17 @@ namespace cartesian_controller_base{
     return true;
   }
 
+  bool DampedLeastSquaresSolver::updateChain(const KDL::Chain& chain)
+  {
+    IKSolver::updateChain(chain);
+
+    m_jnt_jacobian_solver.reset(new KDL::ChainJntToJacSolver(m_chain));
+    m_jnt_jacobian.resize(m_number_joints);
+
+    return true;
+  }
+
+
     void DampedLeastSquaresSolver::dynamicReconfigureCallback(IKConfig& config, uint32_t level)
     {
       m_alpha = config.alpha;
diff --git a/cartesian_controller_base/src/ForwardDynamicsSolver.cpp b/cartesian_controller_base/src/ForwardDynamicsSolver.cpp
index 83277519..d155b20e 100644
--- a/cartesian_controller_base/src/ForwardDynamicsSolver.cpp
+++ b/cartesian_controller_base/src/ForwardDynamicsSolver.cpp
@@ -138,17 +138,8 @@ namespace cartesian_controller_base{
   {
     IKSolver::init(nh, chain, upper_pos_limits, lower_pos_limits);
 
-    if (!buildGenericModel())
-    {
-      ROS_ERROR("ForwardDynamicsSolver: Something went wrong in setting up the internal model.");
+    if(!updateChain(m_chain))
       return false;
-    }
-
-    // Forward dynamics
-    m_jnt_jacobian_solver.reset(new KDL::ChainJntToJacSolver(m_chain));
-    m_jnt_space_inertia_solver.reset(new KDL::ChainDynParam(m_chain,KDL::Vector::Zero()));
-    m_jnt_jacobian.resize(m_number_joints);
-    m_jnt_space_inertia.resize(m_number_joints);
 
     // Connect dynamic reconfigure and overwrite the default values with values
     // on the parameter server. This is done automatically if parameters with
@@ -170,6 +161,26 @@ namespace cartesian_controller_base{
     return true;
   }
 
+  bool ForwardDynamicsSolver::updateChain(const KDL::Chain& chain)
+  {
+    IKSolver::updateChain(chain);
+
+    if (!buildGenericModel())
+    {
+      ROS_ERROR("ForwardDynamicsSolver: Something went wrong in setting up the internal model.");
+      return false;
+    }
+
+    // Forward dynamics
+    m_jnt_jacobian_solver.reset(new KDL::ChainJntToJacSolver(m_chain));
+    m_jnt_space_inertia_solver.reset(new KDL::ChainDynParam(m_chain,KDL::Vector::Zero()));
+    m_jnt_jacobian.resize(m_number_joints);
+    m_jnt_space_inertia.resize(m_number_joints);
+
+    return true;
+  }
+
+
   bool ForwardDynamicsSolver::buildGenericModel()
   {
     // Set all masses and inertias to minimal (yet stable) values.
diff --git a/cartesian_controller_base/src/IKSolver.cpp b/cartesian_controller_base/src/IKSolver.cpp
index 104ea2fc..3b872418 100644
--- a/cartesian_controller_base/src/IKSolver.cpp
+++ b/cartesian_controller_base/src/IKSolver.cpp
@@ -111,6 +111,20 @@ namespace cartesian_controller_base{
                       const KDL::JntArray& lower_pos_limits)
   {
     // Initialize
+    IKSolver::updateChain(chain);
+    m_upper_pos_limits           = upper_pos_limits;
+    m_lower_pos_limits           = lower_pos_limits;
+
+    return true;
+  }
+
+  bool IKSolver::updateChain(const KDL::Chain& chain)
+  {
+    // Guard against multiple initialization
+    // Also fixes a segfault...
+    if(&chain == &m_chain)
+      return true;
+
     m_chain = chain;
     m_number_joints              = m_chain.getNrOfJoints();
     m_current_positions.data     = ctrl::VectorND::Zero(m_number_joints);
@@ -118,8 +132,6 @@ namespace cartesian_controller_base{
     m_current_accelerations.data = ctrl::VectorND::Zero(m_number_joints);
     m_last_positions.data        = ctrl::VectorND::Zero(m_number_joints);
     m_last_velocities.data       = ctrl::VectorND::Zero(m_number_joints);
-    m_upper_pos_limits           = upper_pos_limits;
-    m_lower_pos_limits           = lower_pos_limits;
 
     // Forward kinematics
     m_fk_pos_solver.reset(new KDL::ChainFkSolverPos_recursive(m_chain));
diff --git a/cartesian_controller_base/src/JacobianTransposeSolver.cpp b/cartesian_controller_base/src/JacobianTransposeSolver.cpp
index c49fb954..a0f89c50 100644
--- a/cartesian_controller_base/src/JacobianTransposeSolver.cpp
+++ b/cartesian_controller_base/src/JacobianTransposeSolver.cpp
@@ -116,6 +116,15 @@ namespace cartesian_controller_base{
   {
     IKSolver::init(nh, chain, upper_pos_limits, lower_pos_limits);
 
+    updateChain(m_chain);
+
+    return true;
+  }
+
+  bool JacobianTransposeSolver::updateChain(const KDL::Chain& chain)
+  {
+    IKSolver::updateChain(chain);
+
     m_jnt_jacobian_solver.reset(new KDL::ChainJntToJacSolver(m_chain));
     m_jnt_jacobian.resize(m_number_joints);
 
diff --git a/cartesian_controller_base/src/SelectivelyDampedLeastSquaresSolver.cpp b/cartesian_controller_base/src/SelectivelyDampedLeastSquaresSolver.cpp
index f6125e0d..77fdfc3a 100644
--- a/cartesian_controller_base/src/SelectivelyDampedLeastSquaresSolver.cpp
+++ b/cartesian_controller_base/src/SelectivelyDampedLeastSquaresSolver.cpp
@@ -148,6 +148,15 @@ namespace cartesian_controller_base{
   {
     IKSolver::init(nh, chain, upper_pos_limits, lower_pos_limits);
 
+    updateChain(m_chain);
+
+    return true;
+  }
+
+  bool SelectivelyDampedLeastSquaresSolver::updateChain(const KDL::Chain& chain)
+  {
+    IKSolver::updateChain(chain);
+
     m_jnt_jacobian_solver.reset(new KDL::ChainJntToJacSolver(m_chain));
     m_jnt_jacobian.resize(m_number_joints);
 
diff --git a/cartesian_controller_base/srv/SetKinematicChainOffsets.srv b/cartesian_controller_base/srv/SetKinematicChainOffsets.srv
new file mode 100644
index 00000000..ee3956d5
--- /dev/null
+++ b/cartesian_controller_base/srv/SetKinematicChainOffsets.srv
@@ -0,0 +1,8 @@
+# Ability to add a chain before or after the robot chain (parsed by the URDF)
+
+# Removes the chain if the frame_id is empty. Otherwise, overwrites the previous chain.
+geometry_msgs/PoseStamped pre_robot_chain_offset    # Attached before the robot base link
+geometry_msgs/PoseStamped post_robot_chain_offset   # Attached after end effector link
+---
+bool success
+string message
diff --git a/cartesian_force_controller/include/cartesian_force_controller/cartesian_force_controller.h b/cartesian_force_controller/include/cartesian_force_controller/cartesian_force_controller.h
index 0cf07481..a8b59dd0 100644
--- a/cartesian_force_controller/include/cartesian_force_controller/cartesian_force_controller.h
+++ b/cartesian_force_controller/include/cartesian_force_controller/cartesian_force_controller.h
@@ -103,6 +103,14 @@ class CartesianForceController : public virtual cartesian_controller_base::Carte
     std::string           m_new_ft_sensor_ref;
     void setFtSensorReferenceFrame(const std::string& new_ref);
 
+    /**
+     * @brief Publish the controller's wrenches (sensor, gravity, target and net force)
+     *
+     * The data are w.r.t. the specified robot base link.
+     */
+    void publishStateWrenchFeedback(realtime_tools::RealtimePublisherSharedPtr<geometry_msgs::WrenchStamped>& rt_publisher,
+                                    ctrl::Vector6D& wrench);
+
   private:
     ctrl::Vector6D        compensateGravity();
 
@@ -136,6 +144,15 @@ class CartesianForceController : public virtual cartesian_controller_base::Carte
 
     std::shared_ptr<dynamic_reconfigure::Server<Config> > m_dyn_conf_server;
     dynamic_reconfigure::Server<Config>::CallbackType m_callback_type;
+
+    realtime_tools::RealtimePublisherSharedPtr<geometry_msgs::WrenchStamped>
+      m_feedback_gravity_wrench_publisher;
+    realtime_tools::RealtimePublisherSharedPtr<geometry_msgs::WrenchStamped>
+      m_feedback_sensor_wrench_publisher;
+    realtime_tools::RealtimePublisherSharedPtr<geometry_msgs::WrenchStamped>
+      m_feedback_target_wrench_publisher;
+    realtime_tools::RealtimePublisherSharedPtr<geometry_msgs::WrenchStamped>
+      m_feedback_net_force_wrench_publisher;
 };
 
 }
diff --git a/cartesian_force_controller/include/cartesian_force_controller/cartesian_force_controller.hpp b/cartesian_force_controller/include/cartesian_force_controller/cartesian_force_controller.hpp
index 107d6499..95ebf7ea 100644
--- a/cartesian_force_controller/include/cartesian_force_controller/cartesian_force_controller.hpp
+++ b/cartesian_force_controller/include/cartesian_force_controller/cartesian_force_controller.hpp
@@ -113,6 +113,23 @@ init(HardwareInterface* hw, ros::NodeHandle& nh)
   m_target_wrench.setZero();
   m_ft_sensor_wrench.setZero();
 
+  // Controller-internal state publishing
+  m_feedback_gravity_wrench_publisher =
+    std::make_shared<realtime_tools::RealtimePublisher<geometry_msgs::WrenchStamped> >(
+      nh, "current_gravity_wrench", 3);
+
+  m_feedback_sensor_wrench_publisher =
+    std::make_shared<realtime_tools::RealtimePublisher<geometry_msgs::WrenchStamped> >(
+      nh, "current_sensor_wrench", 3);
+
+  m_feedback_target_wrench_publisher =
+    std::make_shared<realtime_tools::RealtimePublisher<geometry_msgs::WrenchStamped> >(
+      nh, "current_target_wrench", 3);
+
+  m_feedback_net_force_wrench_publisher =
+    std::make_shared<realtime_tools::RealtimePublisher<geometry_msgs::WrenchStamped> >(
+      nh, "current_net_force_wrench", 3);
+
   // Connect dynamic reconfigure and overwrite the default values with values
   // on the parameter server. This is done automatically if parameters with
   // the according names exist.
@@ -185,10 +202,28 @@ computeForceError()
     target_wrench = m_target_wrench;
   }
 
+  ctrl::Vector6D sensor_wrench;
+  sensor_wrench = Base::displayInBaseLink(m_ft_sensor_wrench,m_new_ft_sensor_ref);
+
+  ctrl::Vector6D gravity_wrench;
+  gravity_wrench = compensateGravity();
+
   // Superimpose target wrench and sensor wrench in base frame
-  return Base::displayInBaseLink(m_ft_sensor_wrench,m_new_ft_sensor_ref)
+  ctrl::Vector6D net_force_wrench;
+  net_force_wrench = sensor_wrench
     + target_wrench
-    + compensateGravity();
+    + gravity_wrench;
+
+  // Publish wrench state feedback
+  if (Base::m_publish_state_feedback)
+  {
+    publishStateWrenchFeedback(m_feedback_sensor_wrench_publisher, sensor_wrench);
+    publishStateWrenchFeedback(m_feedback_target_wrench_publisher, target_wrench);
+    publishStateWrenchFeedback(m_feedback_gravity_wrench_publisher, gravity_wrench);
+    publishStateWrenchFeedback(m_feedback_net_force_wrench_publisher, net_force_wrench);
+  }
+
+  return net_force_wrench;
 }
 
 template <class HardwareInterface>
@@ -295,6 +330,25 @@ signalTaringCallback(std_srvs::Trigger::Request& req, std_srvs::Trigger::Respons
   return true;
 }
 
+template <class HardwareInterface>
+void CartesianForceController<HardwareInterface>::
+publishStateWrenchFeedback(realtime_tools::RealtimePublisherSharedPtr<geometry_msgs::WrenchStamped>& rt_publisher,
+                           ctrl::Vector6D& wrench)
+{
+  if (rt_publisher->trylock()){
+    rt_publisher->msg_.header.stamp = ros::Time::now();
+    rt_publisher->msg_.header.frame_id = Base::m_robot_base_link;
+    rt_publisher->msg_.wrench.force.x = wrench[0];
+    rt_publisher->msg_.wrench.force.y = wrench[1];
+    rt_publisher->msg_.wrench.force.z = wrench[2];
+    rt_publisher->msg_.wrench.torque.x = wrench[3];
+    rt_publisher->msg_.wrench.torque.y = wrench[4];
+    rt_publisher->msg_.wrench.torque.z = wrench[5];
+
+    rt_publisher->unlockAndPublish();
+  }
+}
+
 template <class HardwareInterface>
 void CartesianForceController<HardwareInterface>::dynamicReconfigureCallback(Config& config,
                                                                              uint32_t level)