Ensure future forcedimension compatibility

cartesian_vic_controller/CMakeLists.txt

@@ -49,6 +49,8 @@ add_library(cartesian_vic_controller SHARED
   # Vanilla VIC rules (basic admittance / impedance control laws)
   src/rules/vanilla_cartesian_admittance_rule.cpp
   src/rules/vanilla_cartesian_impedance_rule.cpp
+  # Impedance control for Force Dimension Haptic Interfaces
+  src/rules/fd_impedance_rule.cpp
 )
 target_compile_features(cartesian_vic_controller PUBLIC cxx_std_17)
 target_include_directories(cartesian_vic_controller PUBLIC

cartesian_vic_controller/cartesian_vic_rules.xml

@@ -13,4 +13,11 @@
       Classical impedance control law.
     </description>
   </class>
+  <class name="cartesian_vic_controller/FdImpedanceRule"
+         type="cartesian_vic_controller::FdImpedanceRule"
+         base_class_type="cartesian_vic_controller::CartesianVicRule">
+    <description>
+      Impedance control law for Force Dimension haptic devices (and ONLY those!!!).
+    </description>
+  </class>
 </library>

cartesian_vic_controller/config/example_admittance_controller_config.yaml

@@ -29,6 +29,7 @@ cartesian_vic_controller:
       gravity: [0.0, 0.0, -9.81]
 
     ft_sensor:
+      is_enabled: true
       name: ftsensor
       frame:
         id: ft_sensor  # Wrench measurements are in this frame

cartesian_vic_controller/include/cartesian_vic_controller/cartesian_vic_controller.hpp

@@ -23,6 +23,7 @@
 #include <vector>
 
 #include "cartesian_vic_controller/visibility_control.h"
+#include "cartesian_vic_controller/measurement_data.hpp"
 #include "cartesian_vic_controller/cartesian_vic_rule.hpp"
 #include "cartesian_vic_controller/external_torque_sensor.hpp"
 
@@ -133,16 +134,13 @@ class CartesianVicController : public controller_interface::ControllerInterface
    * @brief Read values from hardware interfaces and set corresponding fields of joint_state and
    * ft_values
    */
-  bool read_state_from_hardware(
-    trajectory_msgs::msg::JointTrajectoryPoint & joint_state,
-    geometry_msgs::msg::Wrench & ft_values,
-    std::vector<double> & external_torques);
+  bool read_state_from_hardware(MeasurementData & measurement_data);
 
 
   /**
    * @brief Initialize the vic rule
    */
-  bool initialize_vic_rule(const trajectory_msgs::msg::JointTrajectoryPoint & joint_state);
+  bool initialize_vic_rule(const MeasurementData & measurement_data);
 
   /**
    * @brief Try to retrieve the URDF from the parameter server. If this fails, it will
@@ -224,13 +222,18 @@ class CartesianVicController : public controller_interface::ControllerInterface
   cartesian_control_msgs::msg::CompliantFrameTrajectory cartesian_state_;
   cartesian_control_msgs::msg::CompliantFrameTrajectory cartesian_reference_,
     last_cartesian_reference_;
-  // joint_state_: current joint readings from the hardware
   // joint_command_: joint reference value computed by the controller
-  trajectory_msgs::msg::JointTrajectoryPoint joint_state_;
   trajectory_msgs::msg::JointTrajectoryPoint joint_command_, last_commanded_joint_state_;
-  // ft_values_: values read from the force torque sensor
+
+  // measurement data used by the vic controller
+  //   - joint state: current joint readings from the hardware
+  //   - measured wrench: values read from the ft sensor (optional)
+  //   - external torques: values read from the external torque sensor (optional)
+  MeasurementData measurement_data_{0};
+
+  // ft_values_: values read from the force torque sensor (temporary storage)
   geometry_msgs::msg::Wrench ft_values_;
-  // ext_torque_values_ : values read from the external torque sensor (optional, zero if disabled)
+  // ext_torque_values_ : values read from the external torque sensor (temporary storage)
   std::vector<double> ext_torque_values_;
 };
 

cartesian_vic_controller/include/cartesian_vic_controller/cartesian_vic_rule.hpp

@@ -43,6 +43,7 @@
 #include "cartesian_vic_controller_parameters.hpp"
 
 // include data structures
+#include "cartesian_vic_controller/measurement_data.hpp"
 #include "cartesian_vic_controller/cartesian_vic_state.hpp"
 #include "cartesian_vic_controller/compliance_frame_trajectory.hpp"
 
@@ -84,51 +85,55 @@ class CartesianVicRule
   * Compute joint command from the current cartesian tracking errors
   * and the desired interaction parameters (M, K, D). This function is
   * to be used when no external torque sensor is available.
+  * Internally calls update_input_data() and then compute_controls().
+  *
+  * Note: call only ONE of the update functions.
   *
   * \param[in] period time in seconds since last controller update
-  * \param[in] current_joint_state current joint state of the robot
-  * \param[in] measured_wrench most recent measured wrench from force torque sensor
+  * \param[in] measurement_data most recent measurement data, including at
+  * least the joint position and velocity
   * \param[out] joint_state_command computed joint state command
   */
   controller_interface::return_type update(
     const rclcpp::Duration & period,
-    const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state,
-    const geometry_msgs::msg::Wrench & measured_wrench,
+    const MeasurementData & measurement_data,
     trajectory_msgs::msg::JointTrajectoryPoint & joint_state_command
   );
 
+  /**
+  * Update VIC input data (i.e., kinematics and parameters)
+  *
+  * Note: call only ONE of the update functions.
+  *
+  * \param[in] period time in seconds since last controller update
+  * \param[in] measurement_data most recent measurement data, including at
+  * least the joint position and velocity
+  */
+  controller_interface::return_type update_input_data(
+    const rclcpp::Duration & period,
+    const MeasurementData & measurement_data
+  );
+
   /**
   * Compute joint command from the current cartesian tracking errors
   * and the desired interaction parameters (M, K, D). This function is
-  * to be used when an external torque sensor is available.
+  * to be used when no external torque sensor is available.
   *
   * \param[in] period time in seconds since last controller update
-  * \param[in] current_joint_state current joint state of the robot
-  * \param[in] measured_wrench most recent measured wrench from force torque sensor
-  * \param[in] measured_external_torques most recent measured external torques
   * \param[out] joint_state_command computed joint state command
   */
-  controller_interface::return_type update(
+  controller_interface::return_type compute_controls(
     const rclcpp::Duration & period,
-    const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state,
-    const geometry_msgs::msg::Wrench & measured_wrench,
-    const std::vector<double> & measured_external_torques,
     trajectory_msgs::msg::JointTrajectoryPoint & joint_state_command
   );
 
   /// Get current control mode (ADMITTANCE / IMPEDANCE / INVALID)
   ControlMode get_control_mode() const {return control_mode_;}
 
-protected:
-  /// Internal wrapper for the VIC logic
-  controller_interface::return_type internal_update(
-    const rclcpp::Duration & period,
-    const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state,
-    const geometry_msgs::msg::Wrench & measured_wrench,
-    trajectory_msgs::msg::JointTrajectoryPoint & joint_state_command,
-    bool use_external_torques = false
-  );
+  /// Get current input data (call update_input_data() first)
+  const VicInputData & get_input_data() const {return vic_state_.input_data;}
 
+protected:
   /// Manual setting of inertia, damping, and stiffness (diagonal matrices)
   void set_interaction_parameters(
     const Eigen::Matrix<double, 6, 1> & desired_inertia,
@@ -159,9 +164,11 @@ class CartesianVicRule
   std::shared_ptr<cartesian_vic_controller::ParamListener> parameter_handler_;
   cartesian_vic_controller::Params parameters_;
 
+protected:
+  // ROS2 logging
+  rclcpp::Logger logger_;
   rclcpp::Clock internal_clock_;
 
-protected:
   template<typename T1, typename T2>
   void vec_to_eigen(const std::vector<T1> & data, T2 & matrix);
 
@@ -193,6 +200,8 @@ class CartesianVicRule
 private:
   /// Filtered wrench expressed in world frame
   Eigen::Matrix<double, 6, 1> wrench_world_;
+  /// Filtered wrench expressed in robot base frame
+  Eigen::Matrix<double, 6, 1> wrench_base_;
 
   /// Filtered external torques
   Eigen::VectorXd filtered_external_torques_;

cartesian_vic_controller/include/cartesian_vic_controller/cartesian_vic_state.hpp

@@ -63,6 +63,9 @@ class VicInputData
     natural_joint_space_inertia = \
       Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>::Zero(num_joints, num_joints);
     */
+    // Reset data availability
+    has_ft_sensor_ = false;
+    has_external_torque_sensor_ = false;
   }
   bool set_joint_state_external_torques(const Eigen::VectorXd & joint_state_external_torques)
   {
@@ -75,28 +78,42 @@ class VicInputData
     return true;
   }
 
-  bool get_joint_state_external_torques(Eigen::VectorXd & joint_state_external_torques) const
+  bool set_ft_sensor_wrench(const Eigen::Matrix<double, 6, 1> & ft_sensor_wrench)
   {
-    if (!has_external_torque_sensor_) {
-      joint_state_external_torques.setZero();
-      return false;
-    }
-    if (joint_state_external_torques.size() != joint_state_external_torques_.size()) {
-      joint_state_external_torques.setZero();
-      return false;
-    }
-    joint_state_external_torques = joint_state_external_torques_;
+    robot_current_wrench_at_ft_frame_ = ft_sensor_wrench;
+    has_ft_sensor_ = true;
     return true;
   }
+
+  const Eigen::VectorXd & get_joint_state_external_torques() const
+  {
+    return joint_state_external_torques_;
+  }
+
+  const Eigen::Matrix<double, 6, 1> & get_ft_sensor_wrench() const
+  {
+    return robot_current_wrench_at_ft_frame_;
+  }
+
   bool reset_joint_state_external_torques()
   {
     has_external_torque_sensor_ = false;
     return true;
   }
+
+  bool reset_ft_sensor_wrench()
+  {
+    has_ft_sensor_ = false;
+    return true;
+  }
   bool has_external_torque_sensor() const
   {
     return has_external_torque_sensor_;
   }
+  bool has_ft_sensor() const
+  {
+    return has_ft_sensor_;
+  }
 
 public:
   // General parameters
@@ -133,14 +150,15 @@ class VicInputData
   // Cartesian state (control frame w.r.t. robot base frame)
   Eigen::Isometry3d robot_current_pose;
   Eigen::Matrix<double, 6, 1> robot_current_velocity;
-  Eigen::Matrix<double, 6, 1> robot_current_wrench_at_ft_frame;  // ft_frame w.r.t. base
 
   /// Natural inertia matrix (from dynamic model)
   // Eigen::Matrix<double, 6, 6> natural_cartesian_inertia;
   // Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> natural_joint_space_inertia;
 
 protected:
   Eigen::VectorXd joint_state_external_torques_;
+  Eigen::Matrix<double, 6, 1> robot_current_wrench_at_ft_frame_;  // ft_frame w.r.t. base
+  bool has_ft_sensor_ = false;
   bool has_external_torque_sensor_ = false;
 };
 

cartesian_vic_controller/include/cartesian_vic_controller/measurement_data.hpp

@@ -0,0 +1,123 @@
+// Copyright 2024 ICUBE Laboratory, University of Strasbourg
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+/// \authors: Thibault Poignonec <thibault.poignonec@gmail.com>
+
+#ifndef CARTESIAN_VIC_CONTROLLER__MEASUREMENT_DATA_HPP_
+#define CARTESIAN_VIC_CONTROLLER__MEASUREMENT_DATA_HPP_
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include <map>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "trajectory_msgs/msg/joint_trajectory_point.hpp"
+#include "geometry_msgs/msg/wrench_stamped.hpp"
+
+namespace cartesian_vic_controller
+{
+
+/** @brief Class used to store the measurement data
+ *
+ * Used to pass the data to the Vic rule. The measurement data is
+ * at least the joint position and velocity. Optionally, the external
+ * joint torques and the ft sensor wrench can be passed.
+ *
+ */
+class MeasurementData
+{
+private:
+  size_t num_joints_;
+  bool has_external_torques_data_;
+  bool has_ft_sensor_data_;
+
+  geometry_msgs::msg::Wrench measured_wrench_;
+  std::vector<double> external_torques_;
+
+public:
+  trajectory_msgs::msg::JointTrajectoryPoint joint_state;
+
+public:
+  explicit MeasurementData(size_t num_joints)
+  {
+    num_joints_ = num_joints;
+    joint_state.positions.assign(num_joints_, 0.0);  // std::nan);
+    joint_state.velocities.assign(num_joints_, 0.0);  //  std::nan);
+    joint_state.accelerations.assign(num_joints_, 0.0);  //  std::nan);
+    joint_state.effort.reserve(num_joints_);  // not used
+    external_torques_.assign(num_joints_, 0.0);
+    reset_data_availability();
+  }
+  ~MeasurementData() = default;
+
+  const trajectory_msgs::msg::JointTrajectoryPoint & get_joint_state() const
+  {
+    return joint_state;
+  }
+  const geometry_msgs::msg::Wrench & get_ft_sensor_wrench() const
+  {
+    return measured_wrench_;
+  }
+  const std::vector<double> & get_external_torques() const
+  {
+    return external_torques_;
+  }
+  bool has_ft_sensor_data() const
+  {
+    return has_ft_sensor_data_;
+  }
+  bool has_external_torques_data() const
+  {
+    return has_external_torques_data_;
+  }
+
+    /// Reset all values back to default
+  bool reset_data_availability()
+  {
+    has_external_torques_data_ = false;
+    has_ft_sensor_data_ = false;
+    return true;
+  }
+
+  bool update_joint_state(const trajectory_msgs::msg::JointTrajectoryPoint & joint_state_values)
+  {
+    if (joint_state_values.positions.size() != num_joints_ ||
+      joint_state_values.velocities.size() != num_joints_)
+    {
+      return false;
+    }
+    joint_state = joint_state_values;
+    return true;
+  }
+  bool update_ft_sensor_wrench(const geometry_msgs::msg::Wrench & measured_wrench)
+  {
+    measured_wrench_ = measured_wrench;
+    has_ft_sensor_data_ = true;
+    return true;
+  }
+  bool update_external_torques(const std::vector<double> & external_torques)
+  {
+    external_torques_ = external_torques;
+    has_external_torques_data_ = true;
+    return true;
+  }
+};
+
+}  // namespace cartesian_vic_controller
+
+
+#endif  // CARTESIAN_VIC_CONTROLLER__MEASUREMENT_DATA_HPP_