Admittance with filters

admittance_controller/CMakeLists.txt

@@ -11,6 +11,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   control_toolbox
   controller_interface
   Eigen3
+  filters
   generate_parameter_library
   geometry_msgs
   hardware_interface

admittance_controller/include/admittance_controller/admittance_controller.hpp

@@ -29,6 +29,7 @@
 #include "admittance_controller/visibility_control.h"
 #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"
@@ -146,6 +147,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_;
+
   // ROS messages
   std::shared_ptr<trajectory_msgs::msg::JointTrajectoryPoint> joint_command_msg_;
 

admittance_controller/include/admittance_controller/admittance_rule.hpp

@@ -25,8 +25,8 @@
 #include <vector>
 
 #include "control_msgs/msg/admittance_controller_state.hpp"
-#include "control_toolbox/filters.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"
@@ -52,11 +52,6 @@ struct AdmittanceTransforms
   // transformation from end effector frame to base link frame at reference + admittance offset
   // joint angles
   Eigen::Isometry3d base_tip_;
-  // transformation from center of gravity frame to base link frame at reference + admittance offset
-  // joint angles
-  Eigen::Isometry3d base_cog_;
-  // transformation from world frame to base link frame
-  Eigen::Isometry3d world_base_;
 };
 
 struct AdmittanceState
@@ -98,9 +93,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_);
@@ -117,9 +114,10 @@ class AdmittanceRule
   /**
    * Calculate all transforms needed for admittance control using the loader kinematics plugin. If
    * the transform does not exist in the kinematics model, then TF will be used for lookup. The
-   * return value is true if all transformation are calculated without an error \param[in]
-   * current_joint_state current joint state of the robot \param[in] reference_joint_state input
-   * joint state reference \param[out] success true if no calls to the kinematics interface fail
+   * return value is true if all transformation are calculated without an error
+   * \param[in] current_joint_state current joint state of the robot
+   * \param[in] reference_joint_state input joint state reference
+   * \param[out] success true if no calls to the kinematics interface fail
    */
   bool get_all_transforms(
     const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state,
@@ -162,29 +160,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);
@@ -197,21 +195,20 @@ 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};
 
   // transforms needed for admittance update
   AdmittanceTransforms admittance_transforms_;
 
-  // 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

@@ -22,8 +22,11 @@
 #include <memory>
 #include <vector>
 
+#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
@@ -103,8 +106,7 @@ controller_interface::return_type AdmittanceRule::reset(const size_t num_joints)
   admittance_transforms_ = AdmittanceTransforms();
 
   // reset forces
-  wrench_world_.setZero();
-  end_effector_weight_.setZero();
+  wrench_base_.setZero();
 
   // load/initialize Eigen types from parameters
   apply_parameters_update();
@@ -119,8 +121,6 @@ void AdmittanceRule::apply_parameters_update()
     parameters_ = parameter_handler_->get_params();
   }
   // 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);
@@ -147,12 +147,6 @@ bool AdmittanceRule::get_all_transforms(
     current_joint_state.positions, parameters_.ft_sensor.frame.id, admittance_transforms_.base_ft_);
   success &= kinematics_->calculate_link_transform(
     current_joint_state.positions, parameters_.kinematics.tip, admittance_transforms_.base_tip_);
-  success &= kinematics_->calculate_link_transform(
-    current_joint_state.positions, parameters_.fixed_world_frame.frame.id,
-    admittance_transforms_.world_base_);
-  success &= kinematics_->calculate_link_transform(
-    current_joint_state.positions, parameters_.gravity_compensation.frame.id,
-    admittance_transforms_.base_cog_);
   success &= kinematics_->calculate_link_transform(
     current_joint_state.positions, parameters_.control.frame.id,
     admittance_transforms_.base_control_);
@@ -176,18 +170,25 @@ controller_interface::return_type AdmittanceRule::update(
 
   bool success = get_all_transforms(current_joint_state, reference_joint_state);
 
-  // apply filter and update wrench_world_ vector
-  Eigen::Matrix<double, 3, 3> rot_world_sensor =
-    admittance_transforms_.world_base_.rotation() * admittance_transforms_.base_ft_.rotation();
-  Eigen::Matrix<double, 3, 3> rot_world_cog =
-    admittance_transforms_.world_base_.rotation() * admittance_transforms_.base_cog_.rotation();
-  process_wrench_measurements(measured_wrench, rot_world_sensor, rot_world_cog);
-
-  // transform wrench_world_ into 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) =
-    admittance_transforms_.world_base_.rotation().transpose() * wrench_world_.block<3, 1>(0, 0);
+    admittance_transforms_.base_ft_.rotation() * wrench_filtered_ft_.block<3, 1>(0, 0);
   admittance_state_.wrench_base.block<3, 1>(3, 0) =
-    admittance_transforms_.world_base_.rotation().transpose() * wrench_world_.block<3, 1>(3, 0);
+    admittance_transforms_.base_ft_.rotation() * wrench_filtered_ft_.block<3, 1>(3, 0);
 
   // Compute admittance control law
   vec_to_eigen(current_joint_state.positions, admittance_state_.current_joint_pos);
@@ -228,8 +229,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();
@@ -269,9 +270,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);
+
+  // select the axis to apply the zeroing to
   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);
 
@@ -304,32 +310,28 @@ 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 (size_t i = 0; i < 6; ++i)
+  // pass the measured_wrench in its ft_sensor meas_frame
+  measured_wrench_.header.frame_id = parameters_.ft_sensor.meas_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.meas_frame, kinematics has no idea about "
+      "ft_sensor.meas_frame.\n"
+      "Fix the frames or provide a filter that transforms the wrench from ft_sensor.meas_frame"
+      " to ft_sensor.frame");
+    return false;
   }
+  return ret;
 }
 
 const control_msgs::msg::AdmittanceControllerState & AdmittanceRule::get_controller_state()

admittance_controller/src/admittance_controller.cpp

@@ -24,7 +24,9 @@
 #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 "rcutils/logging_macros.h"
 #include "tf2_ros/buffer.h"
 #include "trajectory_msgs/msg/joint_trajectory_point.hpp"
@@ -37,7 +39,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)
   {
@@ -148,6 +153,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;
@@ -262,6 +268,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)