initial admittance controller impl (#163)

lbr_fri_ros2/CMakeLists.txt

@@ -15,6 +15,7 @@ find_package(control_toolbox REQUIRED)
 find_package(eigen3_cmake_module REQUIRED)
 find_package(Eigen3)
 find_package(FRIClient REQUIRED)
+find_package(geometry_msgs REQUIRED)
 find_package(kdl_parser REQUIRED)
 find_package(lbr_fri_idl REQUIRED)
 find_package(orocos_kdl_vendor REQUIRED)
@@ -33,6 +34,7 @@ add_library(lbr_fri_ros2
     src/app.cpp
     src/async_client.cpp
     src/command_guard.cpp
+    src/control.cpp
     src/filters.cpp
     src/ft_estimator.cpp
     src/kinematics.cpp
@@ -48,6 +50,7 @@ target_include_directories(lbr_fri_ros2
 ament_target_dependencies(lbr_fri_ros2
   control_toolbox
   Eigen3
+  geometry_msgs
   kdl_parser
   lbr_fri_idl
   orocos_kdl_vendor
@@ -65,6 +68,7 @@ ament_export_dependencies(
   eigen3_cmake_module
   Eigen3
   FRIClient
+  geometry_msgs
   kdl_parser
   lbr_fri_idl
   orocos_kdl_vendor

lbr_fri_ros2/include/lbr_fri_ros2/control.hpp

@@ -0,0 +1,48 @@
+#ifndef LBR_FRI_ROS2__CONTROL_HPP_
+#define LBR_FRI_ROS2__CONTROL_HPP_
+
+#include <algorithm>
+#include <memory>
+#include <string>
+
+#include "eigen3/Eigen/Core"
+#include "geometry_msgs/msg/twist.hpp"
+
+#include "lbr_fri_ros2/kinematics.hpp"
+#include "lbr_fri_ros2/pinv.hpp"
+#include "lbr_fri_ros2/types.hpp"
+
+namespace lbr_fri_ros2 {
+struct InvJacCtrlParameters {
+  std::string chain_root;
+  std::string chain_tip;
+  bool twist_in_tip_frame;
+  double damping;
+  double max_linear_velocity;
+  double max_angular_velocity;
+};
+
+class InvJacCtrlImpl {
+public:
+  InvJacCtrlImpl(const std::string &robot_description, const InvJacCtrlParameters &parameters);
+
+  void compute(const geometry_msgs::msg::Twist::SharedPtr &twist_target, const_jnt_array_t_ref q,
+               jnt_array_t_ref dq);
+  void compute(const_cart_array_t_ref twist_target, const_jnt_array_t_ref q, jnt_array_t_ref dq);
+  void compute(const Eigen::Matrix<double, CARTESIAN_DOF, 1> &twist_target, const_jnt_array_t_ref q,
+               jnt_array_t_ref dq);
+
+  inline const std::unique_ptr<Kinematics> &get_kinematics_ptr() const { return kinematics_ptr_; };
+
+protected:
+  void compute_impl_(const_jnt_array_t_ref q, jnt_array_t_ref dq);
+
+  InvJacCtrlParameters parameters_;
+
+  jnt_array_t q_;
+  std::unique_ptr<Kinematics> kinematics_ptr_;
+  Eigen::Matrix<double, N_JNTS, CARTESIAN_DOF> jacobian_inv_;
+  Eigen::Matrix<double, CARTESIAN_DOF, 1> twist_target_;
+};
+} // namespace lbr_fri_ros2
+#endif // LBR_FRI_ROS2__CONTROL_HPP_

lbr_fri_ros2/package.xml

@@ -15,6 +15,7 @@
 
   <depend>control_toolbox</depend>
   <depend>fri_client_sdk</depend>
+  <depend>geometry_msgs</depend>
   <depend>kdl_parser</depend>
   <depend>lbr_fri_idl</depend>
   <depend>orocos_kdl_vendor</depend>

lbr_fri_ros2/src/control.cpp

@@ -0,0 +1,72 @@
+#include "lbr_fri_ros2/control.hpp"
+
+namespace lbr_fri_ros2 {
+InvJacCtrlImpl::InvJacCtrlImpl(const std::string &robot_description,
+                               const InvJacCtrlParameters &parameters)
+    : parameters_(parameters) {
+  kinematics_ptr_ = std::make_unique<Kinematics>(robot_description, parameters_.chain_root,
+                                                 parameters_.chain_tip);
+}
+
+void InvJacCtrlImpl::compute(const geometry_msgs::msg::Twist::SharedPtr &twist_target,
+                             const_jnt_array_t_ref q, jnt_array_t_ref dq) {
+  // twist to Eigen
+  twist_target_[0] = twist_target->linear.x;
+  twist_target_[1] = twist_target->linear.y;
+  twist_target_[2] = twist_target->linear.z;
+  twist_target_[3] = twist_target->angular.x;
+  twist_target_[4] = twist_target->angular.y;
+  twist_target_[5] = twist_target->angular.z;
+
+  // compute
+  compute_impl_(q, dq);
+}
+
+void InvJacCtrlImpl::compute(const_cart_array_t_ref twist_target, const_jnt_array_t_ref q,
+                             jnt_array_t_ref dq) {
+  // twist to Eigen
+  twist_target_[0] = twist_target[0];
+  twist_target_[1] = twist_target[1];
+  twist_target_[2] = twist_target[2];
+  twist_target_[3] = twist_target[3];
+  twist_target_[4] = twist_target[4];
+  twist_target_[5] = twist_target[5];
+
+  // compute
+  compute_impl_(q, dq);
+}
+
+void InvJacCtrlImpl::compute(const Eigen::Matrix<double, CARTESIAN_DOF, 1> &twist_target,
+                             const_jnt_array_t_ref q, jnt_array_t_ref dq) {
+  twist_target_ = twist_target;
+
+  // compute
+  compute_impl_(q, dq);
+}
+
+void InvJacCtrlImpl::compute_impl_(const_jnt_array_t_ref q, jnt_array_t_ref dq) {
+  // clip velocity
+  twist_target_.head(3).unaryExpr([&](double v) {
+    return std::clamp(v, -parameters_.max_linear_velocity, parameters_.max_linear_velocity);
+  });
+  twist_target_.tail(3).unaryExpr([&](double v) {
+    return std::clamp(v, -parameters_.max_angular_velocity, parameters_.max_angular_velocity);
+  });
+
+  // if desired, transform to tip frame
+  if (parameters_.twist_in_tip_frame) {
+    auto chain_tip_frame = kinematics_ptr_->compute_fk(q);
+    twist_target_.topRows(3) =
+        Eigen::Matrix3d::Map(chain_tip_frame.M.data).transpose() * twist_target_.topRows(3);
+    twist_target_.bottomRows(3) =
+        Eigen::Matrix3d::Map(chain_tip_frame.M.data).transpose() * twist_target_.bottomRows(3);
+  }
+
+  // compute jacobian
+  auto jacobian = kinematics_ptr_->compute_jacobian(q);
+  jacobian_inv_ = pinv(jacobian.data, parameters_.damping);
+
+  // compute target joint veloctiy and map it to dq
+  Eigen::Map<Eigen::Matrix<double, N_JNTS, 1>>(dq.data()) = jacobian_inv_ * twist_target_;
+}
+} // namespace lbr_fri_ros2

lbr_ros2_control/CMakeLists.txt

@@ -31,6 +31,7 @@ find_package(realtime_tools REQUIRED)
 add_library(
   ${PROJECT_NAME}
   SHARED
+  src/controllers/admittance_controller.cpp
   src/controllers/lbr_joint_position_command_controller.cpp
   src/controllers/lbr_torque_command_controller.cpp
   src/controllers/lbr_wrench_command_controller.cpp

lbr_ros2_control/config/lbr_controllers.yaml

@@ -93,13 +93,28 @@
   ros__parameters:
     robot_name: lbr
 
+/**/admittance_controller:
+  ros__parameters:
+    robot_name: lbr
+    admittance:
+      mass: 0.2
+      damping: 0.1
+      stiffness: 0.0
+    inv_jac_ctrl:
+      chain_root: lbr_link_0
+      chain_tip: lbr_link_ee
+      damping: 0.2 # damping factor for the pseudo-inverse of the Jacobian
+      max_linear_velocity: 0.1 # maximum linear velocity
+      max_angular_velocity: 0.1 # maximum linear acceleration
+
 /**/twist_controller:
   ros__parameters:
     robot_name: lbr
-    chain_root: lbr_link_0
-    chain_tip: lbr_link_ee
-    twist_in_tip_frame: true # if true, the twist command is expressed in the tip frame, otherwise in the root frame
-    damping: 0.2 # damping factor for the pseudo-inverse of the Jacobian
-    max_linear_velocity: 0.1 # maximum linear velocity
-    max_angular_velocity: 0.1 # maximum linear acceleration
+    inv_jac_ctrl:
+      chain_root: lbr_link_0
+      chain_tip: lbr_link_ee
+      twist_in_tip_frame: true # if true, the twist command is expressed in the tip frame, otherwise in the root frame
+      damping: 0.2 # damping factor for the pseudo-inverse of the Jacobian
+      max_linear_velocity: 0.1 # maximum linear velocity
+      max_angular_velocity: 0.1 # maximum linear acceleration
     timeout: 0.2 # stop controller if no command is received within this time [s]

lbr_ros2_control/include/lbr_ros2_control/controllers/admittance_controller.hpp

@@ -9,22 +9,40 @@
 #include <vector>
 
 #include "controller_interface/controller_interface.hpp"
-#include "hardware_interface/loaned_command_interface.hpp"
+#include "eigen3/Eigen/Core"
+#include "hardware_interface/loaned_state_interface.hpp"
 #include "hardware_interface/types/hardware_interface_type_values.hpp"
 #include "rclcpp/rclcpp.hpp"
+#include "semantic_components/force_torque_sensor.hpp"
 
 #include "friLBRState.h"
+#include "lbr_fri_ros2/control.hpp"
 #include "lbr_fri_ros2/types.hpp"
 #include "lbr_ros2_control/system_interface_type_values.hpp"
 
 namespace lbr_ros2_control {
-class Admittance {
-  // implement an addmittance...
+struct AdmittanceParameters {
+  double m = 1.0;
+  double b = 0.1;
+  double k = 0.0;
 };
 
-class AdmittanceController : public controller_interface::ControllerInterface {
-  static constexpr uint8_t CARTESIAN_DOF = 6;
+class AdmittanceImpl {
+public:
+  AdmittanceImpl(const AdmittanceParameters &parameters) : parameters_(parameters) {}
+
+  void compute(const Eigen::Matrix<double, lbr_fri_ros2::CARTESIAN_DOF, 1> &f_ext,
+               const Eigen::Matrix<double, lbr_fri_ros2::CARTESIAN_DOF, 1> &x,
+               const Eigen::Matrix<double, lbr_fri_ros2::CARTESIAN_DOF, 1> &dx,
+               Eigen::Matrix<double, lbr_fri_ros2::CARTESIAN_DOF, 1> &ddx) {
+    ddx = (f_ext - parameters_.b * dx - parameters_.k * x) / parameters_.m;
+  }
 
+protected:
+  AdmittanceParameters parameters_;
+};
+
+class AdmittanceController : public controller_interface::ControllerInterface {
 public:
   AdmittanceController();
 
@@ -47,18 +65,33 @@ class AdmittanceController : public controller_interface::ControllerInterface {
   on_deactivate(const rclcpp_lifecycle::State &previous_state) override;
 
 protected:
-  bool reference_command_interfaces_();
   bool reference_state_interfaces_();
-  void clear_command_interfaces_();
   void clear_state_interfaces_();
   void configure_joint_names_();
-
+  void configure_admittance_impl_();
+  void configure_inv_jac_ctrl_impl_();
+  void zero_all_values_();
+
+  // admittance
+  bool initialized_ = false;
+  std::unique_ptr<AdmittanceImpl> admittance_impl_ptr_;
+  Eigen::Matrix<double, lbr_fri_ros2::CARTESIAN_DOF, 1> x_init_, x_prev_;
+  Eigen::Matrix<double, lbr_fri_ros2::CARTESIAN_DOF, 1> f_ext_, x_, dx_, ddx_;
+
+  // joint veloctiy computation
+  std::unique_ptr<lbr_fri_ros2::InvJacCtrlImpl> inv_jac_ctrl_impl_ptr_;
+  lbr_fri_ros2::jnt_array_t q_, dq_;
+  Eigen::Matrix<double, lbr_fri_ros2::CARTESIAN_DOF, 1> twist_command_;
+
+  // interfaces
   lbr_fri_ros2::jnt_name_array_t joint_names_;
-
-  std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
-      joint_position_command_interfaces_;
   std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface>>
-      estimated_ft_sensor_state_interface_;
+      joint_position_state_interfaces_;
+  std::unique_ptr<std::reference_wrapper<hardware_interface::LoanedStateInterface>>
+      sample_time_state_interface_ptr_;
+  std::unique_ptr<std::reference_wrapper<hardware_interface::LoanedStateInterface>>
+      session_state_interface_ptr_;
+  std::unique_ptr<semantic_components::ForceTorqueSensor> estimated_ft_sensor_ptr_;
 };
 } // namespace lbr_ros2_control
 #endif // LBR_ROS2_CONTROL__ADMITTANCE_CONTROLLER_HPP_

lbr_ros2_control/include/lbr_ros2_control/controllers/twist_controller.hpp

@@ -4,14 +4,12 @@
 #include <algorithm>
 #include <array>
 #include <atomic>
-#include <functional>
 #include <memory>
 #include <stdexcept>
 #include <string>
 #include <vector>
 
 #include "controller_interface/controller_interface.hpp"
-#include "eigen3/Eigen/Core"
 #include "geometry_msgs/msg/twist.hpp"
 #include "hardware_interface/loaned_state_interface.hpp"
 #include "hardware_interface/types/hardware_interface_type_values.hpp"
@@ -20,37 +18,12 @@
 
 #include "friLBRState.h"
 
+#include "lbr_fri_ros2/control.hpp"
 #include "lbr_fri_ros2/kinematics.hpp"
-#include "lbr_fri_ros2/pinv.hpp"
 #include "lbr_fri_ros2/types.hpp"
 #include "lbr_ros2_control/system_interface_type_values.hpp"
 
 namespace lbr_ros2_control {
-struct TwistParameters {
-  std::string chain_root;
-  std::string chain_tip;
-  bool twist_in_tip_frame;
-  double damping;
-  double max_linear_velocity;
-  double max_angular_velocity;
-};
-
-class TwistImpl {
-public:
-  TwistImpl(const std::string &robot_description, const TwistParameters &parameters);
-
-  void compute(const geometry_msgs::msg::Twist::SharedPtr &twist_target,
-               lbr_fri_ros2::const_jnt_array_t_ref q, lbr_fri_ros2::jnt_array_t_ref dq);
-
-protected:
-  TwistParameters parameters_;
-
-  lbr_fri_ros2::jnt_array_t q_;
-  std::unique_ptr<lbr_fri_ros2::Kinematics> kinematics_ptr_;
-  Eigen::Matrix<double, lbr_fri_ros2::N_JNTS, lbr_fri_ros2::CARTESIAN_DOF> jacobian_inv_;
-  Eigen::Matrix<double, lbr_fri_ros2::CARTESIAN_DOF, 1> twist_target_;
-};
-
 class TwistController : public controller_interface::ControllerInterface {
 public:
   TwistController();
@@ -78,24 +51,24 @@ class TwistController : public controller_interface::ControllerInterface {
   void clear_state_interfaces_();
   void reset_command_buffer_();
   void configure_joint_names_();
-  void configure_twist_impl_();
+  void configure_inv_jac_ctrl_impl_();
 
   // some safety checks
   std::atomic<int> updates_since_last_command_ = 0;
   double timeout_ = 0.2;
 
   // joint veloctiy computation
-  std::unique_ptr<TwistImpl> twist_impl_ptr_;
+  std::unique_ptr<lbr_fri_ros2::InvJacCtrlImpl> inv_jac_ctrl_impl_ptr_;
   lbr_fri_ros2::jnt_array_t q_, dq_;
 
   // interfaces
   lbr_fri_ros2::jnt_name_array_t joint_names_;
   std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface>>
       joint_position_state_interfaces_;
   std::unique_ptr<std::reference_wrapper<hardware_interface::LoanedStateInterface>>
-      sample_time_state_interface_;
+      sample_time_state_interface_ptr_;
   std::unique_ptr<std::reference_wrapper<hardware_interface::LoanedStateInterface>>
-      session_state_interface_;
+      session_state_interface_ptr_;
 
   // real-time twist command topic
   realtime_tools::RealtimeBuffer<geometry_msgs::msg::Twist::SharedPtr> rt_twist_ptr_;