Add optionnal end effector transform offset for the cartesian force controller

cartesian_compliance_controller/include/cartesian_compliance_controller/cartesian_compliance_controller.hpp

@@ -85,7 +85,7 @@ init(HardwareInterface* hw, ros::NodeHandle& nh)
   }
 
   // Make sure sensor wrenches are interpreted correctly
-  ForceBase::setFtSensorReferenceFrame(m_compliance_ref_link);
+  ForceBase::setFtSensorReferenceFrame(m_compliance_ref_link, Base::m_identity_transform_kdl);
 
   // Connect dynamic reconfigure and overwrite the default values with values
   // on the parameter server. This is done automatically if parameters with
@@ -126,6 +126,15 @@ update(const ros::Time& time, const ros::Duration& period)
   // Synchronize the internal model and the real robot
   Base::m_ik_solver->synchronizeJointPositions(Base::m_joint_handles);
 
+  // Update end-effector offset
+  Base::updateEndEffectorOffset();
+
+  // Reset FTS Reference frame when end effector offset has been updated
+  if(Base::m_end_effector_offset_updated)
+  {
+    ForceBase::setFtSensorReferenceFrame(Base::m_end_effector_link,Base::m_end_effector_offset);
+  }
+
   // Control the robot motion in such a way that the resulting net force
   // vanishes. This internal control needs some simulation time steps.
   for (int i = 0; i < Base::m_iterations; ++i)

cartesian_controller_base/CMakeLists.txt

@@ -109,6 +109,7 @@ generate_dynamic_reconfigure_options(
         cfg/PDGains.cfg
         cfg/DampedLeastSquaresSolver.cfg
         cfg/ForwardDynamicsSolver.cfg
+        cfg/SpatialPose.cfg
 )
 
 ###################################
@@ -143,10 +144,12 @@ include_directories(
 add_library(${PROJECT_NAME}
   src/SpatialPDController.cpp
   src/PDController.cpp
+  src/PoseParameterHandle.cpp
   include/cartesian_controller_base/cartesian_controller_base.h
   include/cartesian_controller_base/cartesian_controller_base.hpp
   include/cartesian_controller_base/SpatialPDController.h
   include/cartesian_controller_base/PDController.h
+  include/cartesian_controller_base/PoseParameterHandle.h
   include/cartesian_controller_base/Utility.h
   src/IKSolver.cpp
   include/cartesian_controller_base/IKSolver.h

cartesian_controller_base/cfg/SpatialPose.cfg

@@ -0,0 +1,21 @@
+#!/usr/bin/env python
+PACKAGE = "cartesian_controller_base"
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+position = gen.add_group("Position")
+
+position.add("px", double_t, 0, "Position of a point in free space X axis", 0.0)
+position.add("py", double_t, 0, "Position of a point in free space Y axis", 0.0)
+position.add("pz", double_t, 0, "Position of a point in free space Z axis", 0.0)
+
+orientation = gen.add_group("Orientation")
+
+orientation.add("qx", double_t, 0, "Quaternion X parameter", 0.0, -1.0, 1.0)
+orientation.add("qy", double_t, 0, "Quaternion Y parameter", 0.0, -1.0, 1.0)
+orientation.add("qz", double_t, 0, "Quaternion Z parameter", 0.0, -1.0, 1.0)
+orientation.add("qw", double_t, 0, "Quaternion W parameter", 1.0, -1.0, 1.0)
+
+exit(gen.generate(PACKAGE, "cartesian_controller_base", "SpatialPose"))

cartesian_controller_base/include/cartesian_controller_base/IKSolver.h

@@ -157,8 +157,7 @@ class IKSolver
      * Call this periodically to update the internal simulation's forward
      * kinematics.
      */
-    void updateKinematics();
-
+    void updateKinematics(const KDL::Frame& offset);
   protected:
 
     /**

cartesian_controller_base/include/cartesian_controller_base/PoseParameterHandle.h

@@ -0,0 +1,102 @@
+////////////////////////////////////////////////////////////////////////////////
+// Copyright 2019 FZI Research Center for Information Technology
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// 1. Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+//
+// 2. Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+//
+// 3. Neither the name of the copyright holder nor the names of its
+// contributors may be used to endorse or promote products derived from this
+// software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+////////////////////////////////////////////////////////////////////////////////
+
+
+//-----------------------------------------------------------------------------
+/*!\file    PoseParameterHandle.h
+ *
+ * \author  Captain Yoshi <[email protected]>
+ * \date    2023/12/17
+ *
+ */
+//-----------------------------------------------------------------------------
+
+#ifndef POSE_PARAMETER_HANDLE_H_INCLUDED
+#define POSE_PARAMETER_HANDLE_H_INCLUDED
+
+// STD
+#include <atomic>
+
+// ROS
+#include <ros/ros.h>
+
+// ros_control
+#include <realtime_tools/realtime_buffer.h>
+#include <realtime_tools/realtime_box.h>
+
+// KDL
+#include <kdl/frames.hpp>
+
+// Dynamic reconfigure
+#include <dynamic_reconfigure/server.h>
+#include <cartesian_controller_base/SpatialPoseConfig.h>
+
+namespace cartesian_controller_base
+{
+
+/**
+ * @brief A pose parameter getter utility
+ *
+ * Motivation for this custom implementation:
+ *
+ *  - Retrieve the pose parameter from dynamic reconfigure
+ *
+ *  - Check if the parameter has been updated atomically (atomically)
+ */
+class PoseParameterHandle
+{
+  public:
+    PoseParameterHandle();
+    PoseParameterHandle(const PoseParameterHandle& other); ///< RealtimeBuffer needs special treatment
+    ~PoseParameterHandle();
+
+    void init(const std::string& name_space);
+
+    bool has_new_pose();
+
+    KDL::Frame get_pose();
+
+  private:
+    std::atomic<bool> m_pose_updated{false};
+
+    realtime_tools::RealtimeBuffer<KDL::Frame> m_transform_kdl;
+
+    // Dynamic reconfigure
+    typedef cartesian_controller_base::SpatialPoseConfig Config;
+    void dynamicReconfigureCallback(Config& config, uint32_t level);
+
+    std::shared_ptr<dynamic_reconfigure::Server<Config> > m_dyn_conf_server;
+    dynamic_reconfigure::Server<Config>::CallbackType m_callback_type;
+
+};
+
+}
+
+#endif

cartesian_controller_base/include/cartesian_controller_base/cartesian_controller_base.h

@@ -57,6 +57,7 @@
 
 // Project
 #include <cartesian_controller_base/IKSolver.h>
+#include <cartesian_controller_base/PoseParameterHandle.h>
 #include <cartesian_controller_base/SpatialPDController.h>
 #include <cartesian_controller_base/Utility.h>
 
@@ -95,8 +96,10 @@ class CartesianControllerBase : public controller_interface::Controller<Hardware
     virtual bool init(HardwareInterface* hw, ros::NodeHandle& nh);
 
     virtual void starting(const ros::Time& time);
-
   protected:
+
+    const KDL::Frame m_identity_transform_kdl;
+
     /**
      * @brief Write joint control commands to the real hardware
      *
@@ -120,10 +123,14 @@ class CartesianControllerBase : public controller_interface::Controller<Hardware
      *
      * @param vector The quantity to transform
      * @param from The reference frame where the quantity was formulated
+     * @param from_offset Optionnal offset
      *
      * @return The quantity in the robot base frame
      */
     ctrl::Vector6D displayInBaseLink(const ctrl::Vector6D& vector, const std::string& from);
+    ctrl::Vector6D displayInBaseLink(const ctrl::Vector6D& vector, const KDL::Frame& from);
+    ctrl::Vector6D displayInBaseLink(const ctrl::Vector6D& vector, const std::string& from, const KDL::Frame& from_offset);
+    ctrl::Vector6D displayInBaseLink(const ctrl::Vector6D& vector, const KDL::Frame& from, const KDL::Frame& from_offset);
 
     /**
      * @brief Display the given tensor in the robot base frame
@@ -142,10 +149,14 @@ class CartesianControllerBase : public controller_interface::Controller<Hardware
      *
      * @param vector The quantity to transform
      * @param to The reference frame in which to formulate the quantity
+     * @param to_offset Optionnal offset
      *
      * @return The quantity in the new frame
      */
     ctrl::Vector6D displayInTipLink(const ctrl::Vector6D& vector, const std::string& to);
+    ctrl::Vector6D displayInTipLink(const ctrl::Vector6D& vector, const KDL::Frame& to);
+    ctrl::Vector6D displayInTipLink(const ctrl::Vector6D& vector, const std::string& to, const KDL::Frame& to_offset);
+    ctrl::Vector6D displayInTipLink(const ctrl::Vector6D& vector, const KDL::Frame& to, const KDL::Frame& to_offset);
 
     /**
      * @brief Check if specified links are part of the robot chain
@@ -182,6 +193,14 @@ class CartesianControllerBase : public controller_interface::Controller<Hardware
     int m_iterations;
     std::vector<hardware_interface::JointHandle>      m_joint_handles;
 
+    /**
+     * Allow users to specify a transform offset from the end-effector frame.
+     */
+    void updateEndEffectorOffset();
+
+    KDL::Frame m_end_effector_offset;
+    std::atomic<bool> m_end_effector_offset_updated = false;
+
     /**
      * Whether or not to publish the controller's current end-effector pose and
      * twist.
@@ -209,6 +228,9 @@ class CartesianControllerBase : public controller_interface::Controller<Hardware
     // Against multi initialization in multi inheritance scenarios
     bool m_already_initialized;
 
+    // Handles end effector offset pose, configurable using dynamic reconfigure
+    PoseParameterHandle m_end_effector_offset_handle;
+
     // Dynamic reconfigure
     typedef cartesian_controller_base::CartesianControllerConfig ControllerConfig;