From 25144271af629c5c23d595113e8a0de7d3d19847 Mon Sep 17 00:00:00 2001
From: Captain Yoshi <captain.yoshisaur@gmail.com>
Date: Fri, 15 Dec 2023 21:38:32 -0500
Subject: [PATCH] Add optional transform offset to the cartesian force
 controller

Only update the internal end-effector transform offset when a new pose
has been been received from the dynamic reconfigure server.

The offset affects:
- The Ft sensor reference frame
- The target wrench, only when given from the end-effector link coordinates
---
 .../cartesian_force_controller.h              |  9 ++++++++
 .../cartesian_force_controller.hpp            | 21 ++++++++++++++++---
 2 files changed, 27 insertions(+), 3 deletions(-)

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 7ca0ee58..82a24950 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
@@ -42,6 +42,7 @@
 
 // Project
 #include <cartesian_controller_base/cartesian_controller_base.h>
+#include <cartesian_controller_base/PoseParameterHandle.h>
 
 // ROS
 #include <std_srvs/Trigger.h>
@@ -122,6 +123,14 @@ class CartesianForceController : public virtual cartesian_controller_base::Carte
     std::string           m_ft_sensor_ref_link;
     KDL::Frame            m_ft_sensor_transform;
 
+    /**
+     * Allow users to specify a transform offset from the end-effector frame. Affects
+     * FT Sensor reference frame as well as the target wrench (m_hand_frame_control = True)
+     */
+    KDL::Frame m_end_effector_transform_offset; // only used for target wrench when m_hand_frame_control = True
+
+    cartesian_controller_base::PoseParameterHandle m_pose_parameter_handle;
+
     /**
      * Allow users to choose whether to specify their target wrenches in the
      * end-effector frame (= True) or the base frame (= False). The first one
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 a3fee14a..d62b8b2a 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
@@ -79,7 +79,7 @@ init(HardwareInterface* hw, ros::NodeHandle& nh)
   }
 
   // Make sure sensor wrenches are interpreted correctly
-  setFtSensorReferenceFrame(Base::m_end_effector_link);
+  setFtSensorReferenceFrame(Base::m_end_effector_link,m_end_effector_transform_offset);
 
   m_signal_taring_server = nh.advertiseService("signal_taring",&CartesianForceController<HardwareInterface>::signalTaringCallback,this);
   m_target_wrench_subscriber = nh.subscribe("target_wrench",2,&CartesianForceController<HardwareInterface>::targetWrenchCallback,this);
@@ -113,6 +113,12 @@ init(HardwareInterface* hw, ros::NodeHandle& nh)
   m_target_wrench.setZero();
   m_ft_sensor_wrench.setZero();
 
+
+  // Initialize spatial pose for end effector transform offset
+  std::string spatial_pose_config = nh.getNamespace() + "/end_effector_transform_offset";
+  m_pose_parameter_handle.init(spatial_pose_config);
+
+
   // 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.
@@ -175,10 +181,19 @@ template <class HardwareInterface>
 ctrl::Vector6D CartesianForceController<HardwareInterface>::
 computeForceError()
 {
+  // Update end effector transform offset is updated through dynamic reconfigure
+  if(m_pose_parameter_handle.has_new_pose())
+  {
+    m_end_effector_transform_offset = m_pose_parameter_handle.get_pose();
+
+    // Reset FTS Reference frame
+    setFtSensorReferenceFrame(Base::m_end_effector_link,m_end_effector_transform_offset);
+  }
+
   ctrl::Vector6D target_wrench;
   if (m_hand_frame_control) // Assume end-effector frame by convention
   {
-    target_wrench = Base::displayInBaseLink(m_target_wrench,Base::m_end_effector_link);
+    target_wrench = Base::displayInBaseLink(m_target_wrench,Base::m_end_effector_link,m_end_effector_transform_offset);
   }
   else // Default to robot base frame
   {
@@ -186,7 +201,7 @@ computeForceError()
   }
 
   // Superimpose target wrench and sensor wrench in base frame
-  return Base::displayInBaseLink(m_ft_sensor_wrench,m_new_ft_sensor_ref)
+  return Base::displayInBaseLink(m_ft_sensor_wrench,m_new_ft_sensor_ref,m_end_effector_transform_offset)
     + target_wrench
     + compensateGravity();
 }