Add filter to AdmittanceController

admittance_controller/CMakeLists.txt

@@ -10,6 +10,8 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   control_msgs
   control_toolbox
   controller_interface
+  Eigen3
+  filters
   generate_parameter_library
   geometry_msgs
   hardware_interface
@@ -55,6 +57,7 @@ target_link_libraries(admittance_controller PUBLIC
                       pluginlib::pluginlib
                       rclcpp::rclcpp
                       angles::angles
+                      filters::filter_chain
                       rclcpp_lifecycle::rclcpp_lifecycle
                       realtime_tools::realtime_tools
                       tf2::tf2

admittance_controller/include/admittance_controller/admittance_controller.hpp

@@ -27,6 +27,10 @@
 #include "admittance_controller/admittance_rule.hpp"
 #include "control_msgs/msg/admittance_controller_state.hpp"
 #include "controller_interface/chainable_controller_interface.hpp"
+#include "filters/filter_chain.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/transform_stamped.hpp"
+#include "geometry_msgs/msg/wrench_stamped.hpp"
 #include "hardware_interface/types/hardware_interface_type_values.hpp"
 #include "rclcpp/duration.hpp"
 #include "rclcpp/time.hpp"
@@ -116,6 +120,9 @@ class AdmittanceController : public controller_interface::ChainableControllerInt
   // admittance parameters
   std::shared_ptr<admittance_controller::ParamListener> parameter_handler_;
 
+  // filter chain for Wrench data
+  std::shared_ptr<filters::FilterChain<geometry_msgs::msg::WrenchStamped>> filter_chain_;
+
   // real-time boxes
   realtime_tools::RealtimeThreadSafeBox<trajectory_msgs::msg::JointTrajectoryPoint>
     input_joint_command_;

admittance_controller/include/admittance_controller/admittance_rule.hpp

@@ -25,7 +25,9 @@
 
 #include "admittance_controller/admittance_controller_parameters.hpp"
 #include "control_msgs/msg/admittance_controller_state.hpp"
-#include "controller_interface/controller_interface_base.hpp"
+#include "controller_interface/controller_interface.hpp"
+#include "filters/filter_chain.hpp"
+#include "geometry_msgs/msg/wrench_stamped.hpp"
 #include "kinematics_interface/kinematics_interface.hpp"
 #include "pluginlib/class_loader.hpp"
 #include "trajectory_msgs/msg/joint_trajectory_point.hpp"
@@ -72,9 +74,11 @@ class AdmittanceRule
 {
 public:
   explicit AdmittanceRule(
-    const std::shared_ptr<admittance_controller::ParamListener> & parameter_handler)
+    const std::shared_ptr<admittance_controller::ParamListener> & parameter_handler,
+    const std::shared_ptr<filters::FilterChain<geometry_msgs::msg::WrenchStamped>> & filter_chain)
   {
     parameter_handler_ = parameter_handler;
+    filter_chain_ = filter_chain;
     parameters_ = parameter_handler_->get_params();
     num_joints_ = parameters_.joints.size();
     admittance_state_ = AdmittanceState(num_joints_);
@@ -130,29 +134,29 @@ class AdmittanceRule
   // admittance config parameters
   std::shared_ptr<admittance_controller::ParamListener> parameter_handler_;
   admittance_controller::Params parameters_;
+  // Filter chain for Wrench data
+  std::shared_ptr<filters::FilterChain<geometry_msgs::msg::WrenchStamped>> filter_chain_;
 
 protected:
   /**
    * Calculates the admittance rule from given the robot's current joint angles. The admittance
    * controller state input is updated with the new calculated values. A boolean value is returned
-   * indicating if any of the kinematics plugin calls failed. \param[in] admittance_state contains
-   * all the information needed to calculate the admittance offset \param[in] dt controller period
+   * indicating if any of the kinematics plugin calls failed.
+   * \param[in] admittance_state contains all the information needed to calculate
+   * the admittance offset
+   * \param[in] dt controller period
    * \param[out] success true if no calls to the kinematics interface fail
    */
   bool calculate_admittance_rule(AdmittanceState & admittance_state, double dt);
 
   /**
-   * Updates internal estimate of wrench in world frame `wrench_world_` given the new measurement
-   * `measured_wrench`, the sensor to base frame rotation `sensor_world_rot`, and the center of
-   * gravity frame to base frame rotation `cog_world_rot`. The `wrench_world_` estimate includes
-   * gravity compensation \param[in] measured_wrench  most recent measured wrench from force torque
-   * sensor \param[in] sensor_world_rot rotation matrix from world frame to sensor frame \param[in]
-   * cog_world_rot rotation matrix from world frame to center of gravity frame
+   * Updates internal estimate of wrench in ft frame `measured_wrench_filtered_`
+   * given the new measurement `measured_wrench`
+   * The `measured_wrench_filtered_` might include gravity compensation if such a filter is loaded
+   * \param[in] measured_wrench  most recent measured wrench from force torque sensor
+   * \param[out] success false if processing failed (=filter update failed)
    */
-  void process_wrench_measurements(
-    const geometry_msgs::msg::Wrench & measured_wrench,
-    const Eigen::Matrix<double, 3, 3> & sensor_world_rot,
-    const Eigen::Matrix<double, 3, 3> & cog_world_rot);
+  bool process_wrench_measurements(const geometry_msgs::msg::Wrench & measured_wrench);
 
   template <typename T1, typename T2>
   void vec_to_eigen(const std::vector<T1> & data, T2 & matrix);
@@ -165,18 +169,17 @@ class AdmittanceRule
     kinematics_loader_;
   std::unique_ptr<kinematics_interface::KinematicsInterface> kinematics_;
 
-  // filtered wrench in world frame
-  Eigen::Matrix<double, 6, 1> wrench_world_;
+  // filtered wrench in base frame
+  Eigen::Matrix<double, 6, 1> wrench_base_;
+  // input wrench in sensor frame
+  geometry_msgs::msg::WrenchStamped measured_wrench_;
+  // filtered wrench in sensor_frame
+  geometry_msgs::msg::WrenchStamped measured_wrench_filtered_;
+  Eigen::Matrix<double, 6, 1> wrench_filtered_ft_;
 
   // admittance controllers internal state
   AdmittanceState admittance_state_{0};
 
-  // position of center of gravity in cog_frame
-  Eigen::Vector3d cog_pos_;
-
-  // force applied to sensor due to weight of end effector
-  Eigen::Vector3d end_effector_weight_;
-
   // ROS
   control_msgs::msg::AdmittanceControllerState state_message_;
 };

admittance_controller/include/admittance_controller/admittance_rule_impl.hpp

@@ -23,10 +23,12 @@
 #include <string>
 #include <vector>
 
-#include <control_toolbox/filters.hpp>
-#include <tf2_eigen/tf2_eigen.hpp>
-
+#include "geometry_msgs/msg/transform_stamped.hpp"
 #include "rclcpp/duration.hpp"
+#include "rclcpp/utilities.hpp"
+#include "tf2_eigen/tf2_eigen.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "tf2_ros/transform_listener.h"
 
 namespace admittance_controller
 {
@@ -107,8 +109,7 @@ controller_interface::return_type AdmittanceRule::reset(const size_t num_joints)
   admittance_state_ = AdmittanceState(num_joints);
 
   // reset forces
-  wrench_world_.setZero();
-  end_effector_weight_.setZero();
+  wrench_base_.setZero();
 
   // load/initialize Eigen types from parameters
   apply_parameters_update();
@@ -121,8 +122,6 @@ void AdmittanceRule::apply_parameters_update()
   parameter_handler_->try_get_params(parameters_);
 
   // update param values
-  end_effector_weight_[2] = -parameters_.gravity_compensation.CoG.force;
-  vec_to_eigen(parameters_.gravity_compensation.CoG.pos, cog_pos_);
   vec_to_eigen(parameters_.admittance.mass, admittance_state_.mass);
   vec_to_eigen(parameters_.admittance.stiffness, admittance_state_.stiffness);
   vec_to_eigen(parameters_.admittance.selected_axes, admittance_state_.selected_axes);
@@ -162,35 +161,34 @@ controller_interface::return_type AdmittanceRule::update(
     reference_joint_state.positions, parameters_.ft_sensor.frame.id, tf);
   admittance_state_.ref_trans_base_ft = tf;
 
-  // --- world frame to base frame (we only need the rotation) ---
-  success &= kinematics_->calculate_link_transform(
-    current_joint_state.positions, parameters_.fixed_world_frame.frame.id, tf);
-  const Eigen::Matrix3d rot_world_base = tf.rotation();
-
   // --- control/base frame to base frame (rotation only) ---
   success &= kinematics_->calculate_link_transform(
     current_joint_state.positions, parameters_.control.frame.id, tf);
   admittance_state_.rot_base_control = tf.rotation();
 
-  success &= kinematics_->calculate_link_transform(
-    current_joint_state.positions, parameters_.gravity_compensation.frame.id, tf);
-  const Eigen::Matrix3d rot_tf_cog = tf.rotation();
-
   success &= kinematics_->calculate_link_transform(
     current_joint_state.positions, parameters_.ft_sensor.frame.id, tf);
   const Eigen::Matrix3d rot_tf_base_ft = tf.rotation();
 
-  // wrench processing (gravity + filter) in world
-  process_wrench_measurements(
-    measured_wrench,
-    // pass rotations into sensor and CoG:
-    rot_world_base * rot_tf_base_ft, rot_world_base * rot_tf_cog);
-
-  // transform filtered wrench into the robot base frame
+  // process the wrench measurements, expect the result in ft_sensor.frame (=FK-accessible frame)
+  if (!process_wrench_measurements(measured_wrench))
+  {
+    desired_joint_state = reference_joint_state;
+    return controller_interface::return_type::ERROR;
+  }
+  // fill the Wrench (there is no fromMsg for wrench)
+  wrench_filtered_ft_(0) = measured_wrench_filtered_.wrench.force.x;
+  wrench_filtered_ft_(1) = measured_wrench_filtered_.wrench.force.y;
+  wrench_filtered_ft_(2) = measured_wrench_filtered_.wrench.force.z;
+  wrench_filtered_ft_(3) = measured_wrench_filtered_.wrench.torque.x;
+  wrench_filtered_ft_(4) = measured_wrench_filtered_.wrench.torque.y;
+  wrench_filtered_ft_(5) = measured_wrench_filtered_.wrench.torque.z;
+
+  // Rotate (and not transform) to the base frame, explanation lower
   admittance_state_.wrench_base.block<3, 1>(0, 0) =
-    rot_world_base.transpose() * wrench_world_.block<3, 1>(0, 0);
+    rot_tf_base_ft * wrench_filtered_ft_.block<3, 1>(0, 0);
   admittance_state_.wrench_base.block<3, 1>(3, 0) =
-    rot_world_base.transpose() * wrench_world_.block<3, 1>(3, 0);
+    rot_tf_base_ft * wrench_filtered_ft_.block<3, 1>(3, 0);
 
   // populate current joint positions in the state
   vec_to_eigen(current_joint_state.positions, admittance_state_.current_joint_pos);
@@ -230,8 +228,8 @@ bool AdmittanceRule::calculate_admittance_rule(AdmittanceState & admittance_stat
   Eigen::Matrix<double, 3, 3> K_rot = Eigen::Matrix<double, 3, 3>::Zero();
   K_pos.diagonal() = admittance_state.stiffness.block<3, 1>(0, 0);
   K_rot.diagonal() = admittance_state.stiffness.block<3, 1>(3, 0);
-  // Transform to the control frame
-  // A reference is here:  https://users.wpi.edu/~jfu2/rbe502/files/force_control.pdf
+  // Rotate (and not transform) to the control frame, explanation can be found
+  // in the reference here:  https://users.wpi.edu/~jfu2/rbe502/files/force_control.pdf
   // Force Control by Luigi Villani and Joris De Schutter
   // Page 200
   K_pos = rot_base_control * K_pos * rot_base_control.transpose();
@@ -271,9 +269,14 @@ bool AdmittanceRule::calculate_admittance_rule(AdmittanceState & admittance_stat
 
   // zero out any forces in the control frame
   Eigen::Matrix<double, 6, 1> F_control;
+  // rotate to control frame
   F_control.block<3, 1>(0, 0) = rot_base_control.transpose() * F_base.block<3, 1>(0, 0);
   F_control.block<3, 1>(3, 0) = rot_base_control.transpose() * F_base.block<3, 1>(3, 0);
+
+  // zero out forces/torques for axes not in selected_axes
   F_control = F_control.cwiseProduct(admittance_state.selected_axes);
+
+  // rotate to base frame
   F_base.block<3, 1>(0, 0) = rot_base_control * F_control.block<3, 1>(0, 0);
   F_base.block<3, 1>(3, 0) = rot_base_control * F_control.block<3, 1>(3, 0);
 
@@ -306,32 +309,29 @@ bool AdmittanceRule::calculate_admittance_rule(AdmittanceState & admittance_stat
   return success;
 }
 
-void AdmittanceRule::process_wrench_measurements(
-  const geometry_msgs::msg::Wrench & measured_wrench,
-  const Eigen::Matrix<double, 3, 3> & sensor_world_rot,
-  const Eigen::Matrix<double, 3, 3> & cog_world_rot)
+bool AdmittanceRule::process_wrench_measurements(const geometry_msgs::msg::Wrench & measured_wrench)
 {
-  Eigen::Matrix<double, 3, 2, Eigen::ColMajor> new_wrench;
-  new_wrench(0, 0) = measured_wrench.force.x;
-  new_wrench(1, 0) = measured_wrench.force.y;
-  new_wrench(2, 0) = measured_wrench.force.z;
-  new_wrench(0, 1) = measured_wrench.torque.x;
-  new_wrench(1, 1) = measured_wrench.torque.y;
-  new_wrench(2, 1) = measured_wrench.torque.z;
-
-  // transform to world frame
-  Eigen::Matrix<double, 3, 2> new_wrench_base = sensor_world_rot * new_wrench;
-
-  // apply gravity compensation
-  new_wrench_base(2, 0) -= end_effector_weight_[2];
-  new_wrench_base.block<3, 1>(0, 1) -= (cog_world_rot * cog_pos_).cross(end_effector_weight_);
-
-  // apply smoothing filter
-  for (Eigen::Index i = 0; i < 6; ++i)
+  // pass the measured_wrench in its ft_sensor measurement_frame
+  measured_wrench_.header.frame_id = parameters_.ft_sensor.measurement_frame.id;
+  measured_wrench_.wrench = measured_wrench;
+  // output should be ft_sensor frame (because kinematics is only up to there)
+  measured_wrench_filtered_.header.frame_id = parameters_.ft_sensor.frame.id;
+  // apply the filter
+  auto ret = filter_chain_->update(measured_wrench_, measured_wrench_filtered_);
+  // check the output wrench was correctly transformed into the desired frame
+  if (measured_wrench_filtered_.header.frame_id != parameters_.ft_sensor.frame.id)
   {
-    wrench_world_(i) = filters::exponentialSmoothing(
-      new_wrench_base(i), wrench_world_(i), parameters_.ft_sensor.filter_coefficient);
+    RCLCPP_ERROR_ONCE(
+      rclcpp::get_logger("AdmittanceRule"),
+      "Wrench frame transformation missing.\n"
+      "If ft_sensor.frame != ft_sensor.measurement_frame, kinematics has no idea about "
+      "ft_sensor.measurement_frame.\n"
+      "Fix the frames or provide a filter that transforms the wrench from "
+      "ft_sensor.measurement_frame"
+      " to ft_sensor.frame");
+    return false;
   }
+  return ret;
 }
 
 const control_msgs::msg::AdmittanceControllerState & AdmittanceRule::get_controller_state()

admittance_controller/src/admittance_controller.cpp

@@ -22,7 +22,10 @@
 #include <vector>
 
 #include "admittance_controller/admittance_rule_impl.hpp"
+#include "filters/filter_chain.hpp"
 #include "geometry_msgs/msg/wrench.hpp"
+#include "geometry_msgs/msg/wrench_stamped.hpp"
+#include "tf2_ros/buffer.h"
 #include "trajectory_msgs/msg/joint_trajectory_point.hpp"
 
 namespace
@@ -71,7 +74,10 @@ controller_interface::CallbackReturn AdmittanceController::on_init()
   try
   {
     parameter_handler_ = std::make_shared<admittance_controller::ParamListener>(get_node());
-    admittance_ = std::make_unique<admittance_controller::AdmittanceRule>(parameter_handler_);
+    filter_chain_ = std::make_unique<filters::FilterChain<geometry_msgs::msg::WrenchStamped>>(
+      "geometry_msgs::msg::WrenchStamped");
+    admittance_ =
+      std::make_unique<admittance_controller::AdmittanceRule>(parameter_handler_, filter_chain_);
   }
   catch (const std::exception & e)
   {
@@ -183,6 +189,7 @@ controller_interface::CallbackReturn AdmittanceController::on_configure(
   const rclcpp_lifecycle::State & /*previous_state*/)
 {
   command_joint_names_ = admittance_->parameters_.command_joints;
+
   if (command_joint_names_.empty())
   {
     command_joint_names_ = admittance_->parameters_.joints;
@@ -299,6 +306,18 @@ controller_interface::CallbackReturn AdmittanceController::on_configure(
     get_interface_list(admittance_->parameters_.command_interfaces).c_str(),
     get_interface_list(admittance_->parameters_.state_interfaces).c_str());
 
+  // configure filters
+  if (!filter_chain_->configure(
+        "sensor_filter_chain", get_node()->get_node_logging_interface(),
+        get_node()->get_node_parameters_interface()))
+  {
+    RCLCPP_ERROR(
+      get_node()->get_logger(),
+      "Could not configure sensor filter chain, please check if the "
+      "parameters are provided correctly.");
+    return CallbackReturn::FAILURE;
+  }
+
   // setup subscribers and publishers
   auto joint_command_callback =
     [this](const std::shared_ptr<trajectory_msgs::msg::JointTrajectoryPoint> msg)

admittance_controller/src/admittance_controller_parameters.yaml

@@ -65,44 +65,17 @@ admittance_controller:
         type: string,
         description: "Specifies the frame/link name of the force torque sensor."
       }
-    filter_coefficient: {
-      type: double,
-      default_value: 0.05,
-      description: "Specifies the filter coefficient for the sensor's exponential filter."
-    }
-
-  control:
-    frame:
+    measurement_frame:
       id: {
         type: string,
-        description: "Specifies the control frame used for admittance calculation."
+        description: "Specifies the measurement frame of the force torque sensor (depends on sensor thickness)."
       }
 
-  fixed_world_frame: # Gravity points down (neg. Z) in this frame (Usually: world or base_link)
-    frame:
-      id: {
-        type: string,
-        description: "Specifies the world frame use for admittance calculation. Gravity must point down in this frame."
-      }
-
-  gravity_compensation:
+  control:
     frame:
       id: {
         type: string,
-        description: "Specifies the frame which center of gravity (CoG) is defined in. Normally, the force torque sensor frame should be used."
-      }
-    CoG:
-      pos: {
-        type: double_array,
-        description: "Specifies the position of the center of gravity (CoG) of the end effector in the gravity compensation frame.",
-        validation: {
-          fixed_size<>: 3
-        }
-      }
-      force: {
-        type: double,
-        default_value: 0.0,
-        description: "Specifies the weight of the end effector, e.g mass * 9.81."
+        description: "Specifies the control frame used for admittance calculation."
       }
 
   admittance: