added a kinematics class

lbr_fri_ros2/CMakeLists.txt

@@ -35,6 +35,7 @@ add_library(lbr_fri_ros2
     src/command_guard.cpp
     src/filters.cpp
     src/ft_estimator.cpp
+    src/kinematics.cpp
 )
 
 target_include_directories(lbr_fri_ros2

lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp

@@ -4,20 +4,17 @@
 #include <algorithm>
 #include <array>
 #include <cmath>
+#include <memory>
 #include <string>
 
 #include "eigen3/Eigen/Core"
-#include "kdl/chain.hpp"
-#include "kdl/chainfksolverpos_recursive.hpp"
-#include "kdl/chainjnttojacsolver.hpp"
-#include "kdl/tree.hpp"
-#include "kdl_parser/kdl_parser.hpp"
 #include "rclcpp/logger.hpp"
 #include "rclcpp/logging.hpp"
 
 #include "friLBRState.h"
 
 #include "lbr_fri_idl/msg/lbr_state.hpp"
+#include "lbr_fri_ros2/kinematics.hpp"
 #include "lbr_fri_ros2/pinv.hpp"
 
 namespace lbr_fri_ros2 {
@@ -30,8 +27,7 @@ class FTEstimator {
   using const_ext_tau_array_t_ref = const ext_tau_array_t &;
 
 public:
-  static constexpr uint8_t CARTESIAN_DOF = 6;
-  using cart_array_t = std::array<double, CARTESIAN_DOF>;
+  using cart_array_t = std::array<double, Kinematics::CARTESIAN_DOF>;
   using cart_array_t_ref = cart_array_t &;
   using const_cart_array_t_ref = const cart_array_t &;
 
@@ -47,24 +43,14 @@ class FTEstimator {
   // force threshold
   cart_array_t f_ext_th_;
 
-  KDL::Tree tree_;
-  KDL::Chain chain_;
-
-  // solvers
-  std::unique_ptr<KDL::ChainJntToJacSolver> jacobian_solver_;
-  std::unique_ptr<KDL::ChainFkSolverPos_recursive> fk_solver_;
-
-  // robot state
-  KDL::JntArray q_;
-
-  // forward kinematics
-  KDL::Frame chain_tip_frame_;
+  // kinematics
+  std::unique_ptr<Kinematics> kinematics_;
 
   // force estimation
-  KDL::Jacobian jacobian_;
-  Eigen::Matrix<double, KUKA::FRI::LBRState::NUMBER_OF_JOINTS, CARTESIAN_DOF> jacobian_inv_;
+  Eigen::Matrix<double, KUKA::FRI::LBRState::NUMBER_OF_JOINTS, Kinematics::CARTESIAN_DOF>
+      jacobian_inv_;
   Eigen::Matrix<double, KUKA::FRI::LBRState::NUMBER_OF_JOINTS, 1> tau_ext_;
-  Eigen::Matrix<double, CARTESIAN_DOF, 1> f_ext_;
+  Eigen::Matrix<double, Kinematics::CARTESIAN_DOF, 1> f_ext_;
 };
 } // namespace lbr_fri_ros2
 #endif // LBR_FRI_ROS2__FT_ESTIMATOR_HPP_

lbr_fri_ros2/include/lbr_fri_ros2/kinematics.hpp

@@ -0,0 +1,58 @@
+#ifndef LBR_FRI_ROS2__KINEMATICS_HPP_
+#define LBR_FRI_ROS2__KINEMATICS_HPP_
+
+#include <string>
+
+#include "eigen3/Eigen/Core"
+#include "kdl/chain.hpp"
+#include "kdl/chainfksolverpos_recursive.hpp"
+#include "kdl/chainjnttojacsolver.hpp"
+#include "kdl/tree.hpp"
+#include "kdl_parser/kdl_parser.hpp"
+#include "rclcpp/logger.hpp"
+#include "rclcpp/logging.hpp"
+
+#include "friLBRState.h"
+
+#include "lbr_fri_idl/msg/lbr_state.hpp"
+
+namespace lbr_fri_ros2 {
+class Kinematics {
+protected:
+  static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::Kinematics";
+
+public:
+  static constexpr uint8_t CARTESIAN_DOF = 6;
+  using jnt_pos_array_t = lbr_fri_idl::msg::LBRState::_measured_joint_position_type;
+  using const_jnt_pos_array_t_ref = const jnt_pos_array_t &;
+
+  Kinematics(const std::string &robot_description, const std::string &chain_root = "lbr_link_0",
+             const std::string &chain_tip = "lbr_link_ee");
+
+  // internally computes the jacobian and return a reference to it
+  const KDL::Jacobian &compute_jacobian(const_jnt_pos_array_t_ref q);
+  const KDL::Jacobian &compute_jacobian(const std::vector<double> &q);
+
+  // forward kinematics
+  const KDL::Frame &compute_fk(const_jnt_pos_array_t_ref q);
+  const KDL::Frame &compute_fk(const std::vector<double> &q);
+
+protected:
+  KDL::Tree tree_;
+  KDL::Chain chain_;
+
+  // solvers
+  std::unique_ptr<KDL::ChainJntToJacSolver> jacobian_solver_;
+  std::unique_ptr<KDL::ChainFkSolverPos_recursive> fk_solver_;
+
+  // robot state
+  KDL::JntArray q_;
+
+  // forward kinematics
+  KDL::Frame chain_tip_frame_;
+
+  // jacobian
+  KDL::Jacobian jacobian_;
+};
+} // namespace lbr_fri_ros2
+#endif // LBR_FRI_ROS2__KINEMATICS_HPP_

lbr_fri_ros2/src/ft_estimator.cpp

@@ -4,41 +4,25 @@ namespace lbr_fri_ros2 {
 FTEstimator::FTEstimator(const std::string &robot_description, const std::string &chain_root,
                          const std::string &chain_tip, const_cart_array_t_ref f_ext_th)
     : f_ext_th_(f_ext_th) {
-  if (!kdl_parser::treeFromString(robot_description, tree_)) {
-    std::string err = "Failed to construct kdl tree from robot description.";
-    RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str());
-    throw std::runtime_error(err);
-  }
-  if (!tree_.getChain(chain_root, chain_tip, chain_)) {
-    std::string err = "Failed to construct kdl chain from robot description.";
-    RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str());
-    throw std::runtime_error(err);
-  }
-  jacobian_solver_ = std::make_unique<KDL::ChainJntToJacSolver>(chain_);
-  fk_solver_ = std::make_unique<KDL::ChainFkSolverPos_recursive>(chain_);
-  jacobian_.resize(KUKA::FRI::LBRState::NUMBER_OF_JOINTS);
-  q_.resize(KUKA::FRI::LBRState::NUMBER_OF_JOINTS);
-
+  kinematics_ = std::make_unique<Kinematics>(robot_description, chain_root, chain_tip);
   reset();
 }
 
 void FTEstimator::compute(const_jnt_pos_array_t_ref measured_joint_position,
                           const_ext_tau_array_t_ref external_torque, cart_array_t_ref f_ext,
                           const double &damping) {
-  q_.data = Eigen::Map<const Eigen::Matrix<double, KUKA::FRI::LBRState::NUMBER_OF_JOINTS, 1>>(
-      measured_joint_position.data());
   tau_ext_ = Eigen::Map<const Eigen::Matrix<double, KUKA::FRI::LBRState::NUMBER_OF_JOINTS, 1>>(
       external_torque.data());
-  jacobian_solver_->JntToJac(q_, jacobian_);
-  jacobian_inv_ = pinv(jacobian_.data, damping);
+  auto jacobian = kinematics_->compute_jacobian(measured_joint_position);
+  jacobian_inv_ = pinv(jacobian.data, damping);
   f_ext_ = jacobian_inv_.transpose() * tau_ext_;
 
   // rotate into chain tip frame
-  fk_solver_->JntToCart(q_, chain_tip_frame_);
-  f_ext_.topRows(3) = Eigen::Matrix3d::Map(chain_tip_frame_.M.data) * f_ext_.topRows(3);
-  f_ext_.bottomRows(3) = Eigen::Matrix3d::Map(chain_tip_frame_.M.data) * f_ext_.bottomRows(3);
+  auto chain_tip_frame = kinematics_->compute_fk(measured_joint_position);
+  f_ext_.topRows(3) = Eigen::Matrix3d::Map(chain_tip_frame.M.data) * f_ext_.topRows(3);
+  f_ext_.bottomRows(3) = Eigen::Matrix3d::Map(chain_tip_frame.M.data) * f_ext_.bottomRows(3);
 
-  Eigen::Map<Eigen::Matrix<double, CARTESIAN_DOF, 1>>(f_ext.data()) = f_ext_;
+  Eigen::Map<Eigen::Matrix<double, Kinematics::CARTESIAN_DOF, 1>>(f_ext.data()) = f_ext_;
 
   // threshold force-torque
   std::transform(f_ext.begin(), f_ext.end(), f_ext_th_.begin(), f_ext.begin(),
@@ -52,7 +36,6 @@ void FTEstimator::compute(const_jnt_pos_array_t_ref measured_joint_position,
 }
 
 void FTEstimator::reset() {
-  q_.data.setZero();
   tau_ext_.setZero();
   f_ext_.setZero();
 }

lbr_fri_ros2/src/kinematics.cpp

@@ -0,0 +1,65 @@
+#include "lbr_fri_ros2/kinematics.hpp"
+
+namespace lbr_fri_ros2 {
+Kinematics::Kinematics(const std::string &robot_description, const std::string &chain_root,
+                       const std::string &chain_tip) {
+  if (!kdl_parser::treeFromString(robot_description, tree_)) {
+    std::string err = "Failed to construct kdl tree from robot description.";
+    RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str());
+    throw std::runtime_error(err);
+  }
+  if (!tree_.getChain(chain_root, chain_tip, chain_)) {
+    std::string err = "Failed to construct kdl chain from robot description.";
+    RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str());
+    throw std::runtime_error(err);
+  }
+  if (chain_.getNrOfJoints() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) {
+    std::string err = "Invalid number of joints in chain.";
+    RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str());
+    throw std::runtime_error(err);
+  }
+  jacobian_solver_ = std::make_unique<KDL::ChainJntToJacSolver>(chain_);
+  fk_solver_ = std::make_unique<KDL::ChainFkSolverPos_recursive>(chain_);
+  jacobian_.resize(KUKA::FRI::LBRState::NUMBER_OF_JOINTS);
+  q_.resize(KUKA::FRI::LBRState::NUMBER_OF_JOINTS);
+  q_.data.setZero();
+}
+
+const KDL::Jacobian &Kinematics::compute_jacobian(const_jnt_pos_array_t_ref q) {
+  q_.data =
+      Eigen::Map<const Eigen::Matrix<double, KUKA::FRI::LBRState::NUMBER_OF_JOINTS, 1>>(q.data());
+  jacobian_solver_->JntToJac(q_, jacobian_);
+  return jacobian_;
+}
+
+const KDL::Jacobian &Kinematics::compute_jacobian(const std::vector<double> &q) {
+  if (q.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) {
+    std::string err = "Invalid number of joint positions.";
+    RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str());
+    throw std::runtime_error(err);
+  }
+  q_.data =
+      Eigen::Map<const Eigen::Matrix<double, KUKA::FRI::LBRState::NUMBER_OF_JOINTS, 1>>(q.data());
+  jacobian_solver_->JntToJac(q_, jacobian_);
+  return jacobian_;
+}
+
+const KDL::Frame &Kinematics::compute_fk(const_jnt_pos_array_t_ref q) {
+  q_.data =
+      Eigen::Map<const Eigen::Matrix<double, KUKA::FRI::LBRState::NUMBER_OF_JOINTS, 1>>(q.data());
+  fk_solver_->JntToCart(q_, chain_tip_frame_);
+  return chain_tip_frame_;
+}
+
+const KDL::Frame &Kinematics::compute_fk(const std::vector<double> &q) {
+  if (q.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) {
+    std::string err = "Invalid number of joint positions.";
+    RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str());
+    throw std::runtime_error(err);
+  }
+  q_.data =
+      Eigen::Map<const Eigen::Matrix<double, KUKA::FRI::LBRState::NUMBER_OF_JOINTS, 1>>(q.data());
+  fk_solver_->JntToCart(q_, chain_tip_frame_);
+  return chain_tip_frame_;
+}
+} // namespace lbr_fri_ros2