Fix build issue (#8)

* use new realtime_tools headers * fix str format error * lint * add jacobian_inverse to track kinematics interface changes * fix clang format * change private DynamicsInterfaceKDL stuff to protected * use DynamicsInterfaceKDL as a base class for DynamicsInterfaceFd * fix KDL_plugin_no_robot_description test * fix KDL_plugin_no_gravity test * add calculate_jacobian_inverse test * add missing alpha initialization * fix deleted fd_inertia_topic_name initialization * fix DynamicsInterfaceFd::initialize() * fix "double free or corruption (!prev)" error * fix broken tests * linting --------- Co-authored-by: Thibault Poignonec <[email protected]>
tpoignonec · Jan 18, 2025 · 4248b79 · 4248b79
1 parent 94ab06a
commit 4248b79
Show file tree

Hide file tree

Showing 9 changed files with 165 additions and 561 deletions.
diff --git a/.clang-format b/.clang-format
diff --git a/dynamics_interface_fd/CMakeLists.txt b/dynamics_interface_fd/CMakeLists.txt
@@ -7,7 +7,7 @@ endif()
 
 set(THIS_PACKAGE_INCLUDE_DEPENDS
   kdl_parser
-  dynamics_interface
+  dynamics_interface_kdl
   pluginlib
   tf2_eigen_kdl
   realtime_tools

diff --git a/dynamics_interface_fd/include/dynamics_interface_fd/dynamics_interface_fd.hpp b/dynamics_interface_fd/include/dynamics_interface_fd/dynamics_interface_fd.hpp
@@ -24,29 +24,18 @@
 #include <unordered_map>
 #include <vector>
 
-#include "dynamics_interface/dynamics_interface.hpp"
+#include "dynamics_interface_kdl/dynamics_interface_kdl.hpp"
 
-#include "eigen3/Eigen/Core"
-#include "eigen3/Eigen/LU"
-
-#include "kdl/chaindynparam.hpp"
-#include "kdl/chainfksolverpos_recursive.hpp"
-#include "kdl/chainfksolvervel_recursive.hpp"
-#include "kdl/chainjnttojacdotsolver.hpp"
-#include "kdl/chainjnttojacsolver.hpp"
-#include "kdl/treejnttojacsolver.hpp"
-#include "kdl_parser/kdl_parser.hpp"
-#include "tf2_eigen_kdl/tf2_eigen_kdl.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 #include "rclcpp/node_interfaces/node_parameters_interface.hpp"
-#include "realtime_tools/realtime_buffer.h"
+#include "realtime_tools/realtime_buffer.hpp"
 
 #include "std_msgs/msg/float64_multi_array.hpp"
 
 namespace dynamics_interface_fd
 {
-class DynamicsInterfaceFd : public dynamics_interface::DynamicsInterface
+class DynamicsInterfaceFd : public dynamics_interface_kdl::DynamicsInterfaceKDL
 {
 public:
   DynamicsInterfaceFd();
@@ -65,19 +54,6 @@ class DynamicsInterfaceFd : public dynamics_interface::DynamicsInterface
     std::shared_ptr<rclcpp::node_interfaces::NodeParametersInterface> parameters_interface,
     const std::string & param_namespace) override;
 
-  bool calculate_link_transform(
-    const Eigen::VectorXd & joint_pos, const std::string & link_name,
-    Eigen::Isometry3d & transform) override;
-
-  bool calculate_jacobian(
-    const Eigen::VectorXd & joint_pos, const std::string & link_name,
-    Eigen::Matrix<double, 6, Eigen::Dynamic> & jacobian) override;
-
-  bool calculate_jacobian_derivative(
-    const Eigen::VectorXd & joint_pos, const Eigen::VectorXd & joint_vel,
-    const std::string & link_name,
-    Eigen::Matrix<double, 6, Eigen::Dynamic> & jacobian_derivative) override;
-
   bool calculate_inertia(
     const Eigen::VectorXd & joint_pos,
     Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> & inertia) override;
@@ -88,37 +64,12 @@ class DynamicsInterfaceFd : public dynamics_interface::DynamicsInterface
 
   bool calculate_gravity(const Eigen::VectorXd & joint_pos, Eigen::VectorXd & gravity) override;
 
-  bool convert_cartesian_deltas_to_joint_deltas(
-    const Eigen::VectorXd & joint_pos, const Eigen::Matrix<double, 6, 1> & delta_x,
-    const std::string & link_name, Eigen::VectorXd & delta_theta) override;
-
-  bool convert_joint_deltas_to_cartesian_deltas(
-    const Eigen::VectorXd & joint_pos, const Eigen::VectorXd & delta_theta,
-    const std::string & link_name, Eigen::Matrix<double, 6, 1> & delta_x) override;
-
-private:
+protected:
   // methods
   bool fill_joints_if_missing(
     const Eigen::VectorXd & joint_values, Eigen::VectorXd & joint_values_filled);
 
-  // verification methods
-  bool verify_initialized();
-  bool verify_link_name(const std::string & link_name);
-  bool verify_joint_vector(const Eigen::VectorXd & joint_vector);
-  bool verify_jacobian(const Eigen::Matrix<double, 6, Eigen::Dynamic> & jacobian);
-  bool verify_inertia(const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> & inertia);
-  bool verify_coriolis(const Eigen::VectorXd & coriolis);
-  bool verify_gravity(const Eigen::VectorXd & gravity);
-
-  bool initialized = false;
-  std::string root_name_;
-  size_t num_joints_;
-
-  std::shared_ptr<rclcpp::node_interfaces::NodeParametersInterface> parameters_interface_;
-  std::unordered_map<std::string, int> link_name_map_;
-  double alpha;  // damping term for Jacobian inverse
   std::string fd_inertia_topic_name_;
-  Eigen::MatrixXd I;
 
   // async node for inertia
   rclcpp::Node::SharedPtr async_node_;
@@ -130,17 +81,7 @@ class DynamicsInterfaceFd : public dynamics_interface::DynamicsInterface
 
   // Incoming fd_inertia data
   realtime_tools::RealtimeBuffer<std::shared_ptr<std_msgs::msg::Float64MultiArray>>
-    rt_fd_inertia_subscriber_ptr_;
-
-  // KDL
-  KDL::Chain chain_;
-  std::shared_ptr<KDL::ChainFkSolverPos_recursive> fk_pos_solver_;
-  KDL::JntArray q_, q_dot_;
-  KDL::JntArrayVel q_array_vel_;
-  KDL::Frame frame_;
-  std::shared_ptr<KDL::Jacobian> jacobian_, jacobian_derivative_;
-  std::shared_ptr<KDL::ChainJntToJacSolver> jac_solver_;
-  std::shared_ptr<KDL::ChainJntToJacDotSolver> jac_dot_solver_;
+  rt_fd_inertia_subscriber_ptr_;
 };
 
 }  // namespace dynamics_interface_fd

diff --git a/dynamics_interface_fd/package.xml b/dynamics_interface_fd/package.xml
@@ -13,7 +13,7 @@
 
   <depend>eigen</depend>
   <depend>kdl_parser</depend>
-  <depend>dynamics_interface</depend>
+  <depend>dynamics_interface_kdl</depend>
   <depend>pluginlib</depend>
   <depend>tf2_eigen_kdl</depend>
   <depend>realtime_tools</depend>