addressed review (#220 (review))

lbr_fri_ros2/include/lbr_fri_ros2/control.hpp

@@ -3,10 +3,13 @@
 
 #include <algorithm>
 #include <memory>
+#include <stdexcept>
 #include <string>
 
 #include "eigen3/Eigen/Core"
 #include "geometry_msgs/msg/twist.hpp"
+#include "rclcpp/logger.hpp"
+#include "rclcpp/logging.hpp"
 
 #include "lbr_fri_ros2/kinematics.hpp"
 #include "lbr_fri_ros2/pinv.hpp"
@@ -23,6 +26,9 @@ struct InvJacCtrlParameters {
 };
 
 class InvJacCtrlImpl {
+protected:
+  static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::InvJacCtrlImpl";
+
 public:
   InvJacCtrlImpl(const std::string &robot_description, const InvJacCtrlParameters &parameters);
 
@@ -34,6 +40,8 @@ class InvJacCtrlImpl {
 
   inline const std::unique_ptr<Kinematics> &get_kinematics_ptr() const { return kinematics_ptr_; };
 
+  void log_info() const;
+
 protected:
   void compute_impl_(const_jnt_array_t_ref q, jnt_array_t_ref dq);
 
@@ -44,5 +52,43 @@ class InvJacCtrlImpl {
   Eigen::Matrix<double, N_JNTS, CARTESIAN_DOF> jacobian_inv_;
   Eigen::Matrix<double, CARTESIAN_DOF, 1> twist_target_;
 };
+
+struct AdmittanceParameters {
+  AdmittanceParameters() = delete;
+  AdmittanceParameters(const double &m = 1.0, const double &b = 0.1, const double &k = 0.0)
+      : m(m), b(b), k(k) {
+    if (m <= 0.0) {
+      throw std::runtime_error("Mass must be positive and greater zero.");
+    }
+    if (b < 0.0) {
+      throw std::runtime_error("Damping must be positive.");
+    }
+    if (k < 0.0) {
+      throw std::runtime_error("Stiffness must be positive.");
+    }
+  }
+
+  double m = 1.0;
+  double b = 0.1;
+  double k = 0.0;
+};
+
+class AdmittanceImpl {
+protected:
+  static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::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);
+
+  void log_info() const;
+
+protected:
+  AdmittanceParameters parameters_;
+};
 } // namespace lbr_fri_ros2
 #endif // LBR_FRI_ROS2__CONTROL_HPP_

lbr_fri_ros2/src/control.cpp

@@ -44,6 +44,20 @@ void InvJacCtrlImpl::compute(const Eigen::Matrix<double, CARTESIAN_DOF, 1> &twis
   compute_impl_(q, dq);
 }
 
+void InvJacCtrlImpl::log_info() const {
+  RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "*** Parameters:");
+  RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "*   Chain root: %s",
+              parameters_.chain_root.c_str());
+  RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "*   Chain tip: %s", parameters_.chain_tip.c_str());
+  RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "*   Twist in tip frame: %s",
+              parameters_.twist_in_tip_frame ? "true" : "false");
+  RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "*   Damping: %.3f", parameters_.damping);
+  RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "*   Max linear velocity: %.3f",
+              parameters_.max_linear_velocity);
+  RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "*   Max angular velocity: %.3f",
+              parameters_.max_angular_velocity);
+}
+
 void InvJacCtrlImpl::compute_impl_(const_jnt_array_t_ref q, jnt_array_t_ref dq) {
   // clip velocity
   twist_target_.head(3).unaryExpr([&](double v) {
@@ -69,4 +83,23 @@ void InvJacCtrlImpl::compute_impl_(const_jnt_array_t_ref q, jnt_array_t_ref dq)
   // compute target joint veloctiy and map it to dq
   Eigen::Map<Eigen::Matrix<double, N_JNTS, 1>>(dq.data()) = jacobian_inv_ * twist_target_;
 }
+
+void AdmittanceImpl::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) {
+  if (parameters_.m <= 0.0) {
+    throw std::runtime_error("Mass must be positive and greater zero.");
+  }
+  ddx = (f_ext - parameters_.b * dx - parameters_.k * x) / parameters_.m;
+}
+
+void AdmittanceImpl::log_info() const {
+  {
+    RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "*** Parameters:");
+    RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "*   Mass: %.3f", parameters_.m);
+    RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "*   Damping: %.3f", parameters_.b);
+    RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "*   Stiffness: %.3f", parameters_.k);
+  }
+}
 } // namespace lbr_fri_ros2

lbr_ros2_control/include/lbr_ros2_control/controllers/admittance_controller.hpp

@@ -21,27 +21,6 @@
 #include "lbr_ros2_control/system_interface_type_values.hpp"
 
 namespace lbr_ros2_control {
-struct AdmittanceParameters {
-  double m = 1.0;
-  double b = 0.1;
-  double k = 0.0;
-};
-
-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();
@@ -71,10 +50,11 @@ class AdmittanceController : public controller_interface::ControllerInterface {
   void configure_admittance_impl_();
   void configure_inv_jac_ctrl_impl_();
   void zero_all_values_();
+  void log_info_() const;
 
   // admittance
   bool initialized_ = false;
-  std::unique_ptr<AdmittanceImpl> admittance_impl_ptr_;
+  std::unique_ptr<lbr_fri_ros2::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_;
 

lbr_ros2_control/include/lbr_ros2_control/controllers/twist_controller.hpp

@@ -52,6 +52,7 @@ class TwistController : public controller_interface::ControllerInterface {
   void reset_command_buffer_();
   void configure_joint_names_();
   void configure_inv_jac_ctrl_impl_();
+  void log_info_() const;
 
   // some safety checks
   std::atomic<int> updates_since_last_command_ = 0;

lbr_ros2_control/src/controllers/admittance_controller.cpp

@@ -50,6 +50,7 @@ controller_interface::CallbackReturn AdmittanceController::on_init() {
     configure_joint_names_();
     configure_admittance_impl_();
     configure_inv_jac_ctrl_impl_();
+    log_info_();
   } catch (const std::exception &e) {
     RCLCPP_ERROR(this->get_node()->get_logger(),
                  "Failed to initialize admittance controller with: %s.", e.what());
@@ -76,7 +77,7 @@ controller_interface::return_type AdmittanceController::update(const rclcpp::Tim
   // compute forward kinematics
   auto chain_tip_frame = inv_jac_ctrl_impl_ptr_->get_kinematics_ptr()->compute_fk(q_);
   x_.head(3) = Eigen::Map<Eigen::Matrix<double, 3, 1>>(chain_tip_frame.p.data);
-  chain_tip_frame.M.GetRPY(x_init_[3], x_init_[4], x_init_[5]);
+  x_.tail(3) = Eigen::Map<Eigen::Matrix<double, 3, 1>>(chain_tip_frame.M.GetRot().data);
 
   // compute steady state position and orientation
   if (!initialized_) {
@@ -85,15 +86,17 @@ controller_interface::return_type AdmittanceController::update(const rclcpp::Tim
     initialized_ = true;
   }
 
-  // compute velocity
+  // compute velocity & update previous position
   dx_ = (x_ - x_prev_) / period.seconds();
+  x_prev_ = x_;
+
+  // convert f_ext_ back to root frame
+  f_ext_.head(3) = Eigen::Matrix3d::Map(chain_tip_frame.M.data).transpose() * f_ext_.head(3);
+  f_ext_.tail(3) = Eigen::Matrix3d::Map(chain_tip_frame.M.data).transpose() * f_ext_.tail(3);
 
   // compute admittance
   admittance_impl_ptr_->compute(f_ext_, x_ - x_init_, dx_, ddx_);
 
-  // update previous position
-  x_prev_ = x_;
-
   // integrate ddx_ to command velocity
   twist_command_ = ddx_ * period.seconds();
 
@@ -198,17 +201,11 @@ void AdmittanceController::configure_joint_names_() {
 }
 
 void AdmittanceController::configure_admittance_impl_() {
-  admittance_impl_ptr_ = std::make_unique<AdmittanceImpl>(
-      AdmittanceParameters{this->get_node()->get_parameter("admittance.mass").as_double(),
-                           this->get_node()->get_parameter("admittance.damping").as_double(),
-                           this->get_node()->get_parameter("admittance.stiffness").as_double()});
-  RCLCPP_INFO(this->get_node()->get_logger(), "Admittance controller initialized.");
-  RCLCPP_INFO(this->get_node()->get_logger(), "Mass: %f",
-              this->get_node()->get_parameter("admittance.mass").as_double());
-  RCLCPP_INFO(this->get_node()->get_logger(), "Damping: %f",
-              this->get_node()->get_parameter("admittance.damping").as_double());
-  RCLCPP_INFO(this->get_node()->get_logger(), "Stiffness: %f",
-              this->get_node()->get_parameter("admittance.stiffness").as_double());
+  admittance_impl_ptr_ =
+      std::make_unique<lbr_fri_ros2::AdmittanceImpl>(lbr_fri_ros2::AdmittanceParameters{
+          this->get_node()->get_parameter("admittance.mass").as_double(),
+          this->get_node()->get_parameter("admittance.damping").as_double(),
+          this->get_node()->get_parameter("admittance.stiffness").as_double()});
 }
 
 void AdmittanceController::configure_inv_jac_ctrl_impl_() {
@@ -217,7 +214,7 @@ void AdmittanceController::configure_inv_jac_ctrl_impl_() {
       lbr_fri_ros2::InvJacCtrlParameters{
           this->get_node()->get_parameter("inv_jac_ctrl.chain_root").as_string(),
           this->get_node()->get_parameter("inv_jac_ctrl.chain_tip").as_string(),
-          true, // always assume twist in tip frame, since force-torque is estimated in tip frame
+          false, // always assume twist in root frame
           this->get_node()->get_parameter("inv_jac_ctrl.damping").as_double(),
           this->get_node()->get_parameter("inv_jac_ctrl.max_linear_velocity").as_double(),
           this->get_node()->get_parameter("inv_jac_ctrl.max_angular_velocity").as_double()});
@@ -231,6 +228,11 @@ void AdmittanceController::zero_all_values_() {
   std::fill(dq_.begin(), dq_.end(), 0.0);
   twist_command_.setZero();
 }
+
+void AdmittanceController::log_info_() const {
+  admittance_impl_ptr_->log_info();
+  inv_jac_ctrl_impl_ptr_->log_info();
+}
 } // namespace lbr_ros2_control
 
 #include "pluginlib/class_list_macros.hpp"

lbr_ros2_control/src/controllers/twist_controller.cpp

@@ -44,6 +44,7 @@ controller_interface::CallbackReturn TwistController::on_init() {
     this->get_node()->declare_parameter("timeout", 0.2);
     configure_joint_names_();
     configure_inv_jac_ctrl_impl_();
+    log_info_();
     timeout_ = this->get_node()->get_parameter("timeout").as_double();
   } catch (const std::exception &e) {
     RCLCPP_ERROR(this->get_node()->get_logger(), "Failed to initialize twist controller with: %s.",
@@ -175,6 +176,8 @@ void TwistController::configure_inv_jac_ctrl_impl_() {
           this->get_node()->get_parameter("inv_jac_ctrl.max_linear_velocity").as_double(),
           this->get_node()->get_parameter("inv_jac_ctrl.max_angular_velocity").as_double()});
 }
+
+void TwistController::log_info_() const { inv_jac_ctrl_impl_ptr_->log_info(); }
 } // namespace lbr_ros2_control
 
 #include "pluginlib/class_list_macros.hpp"