diff --git a/.clang-format b/.clang-format
deleted file mode 100644
index 9c21e1e..0000000
--- a/.clang-format
+++ /dev/null
@@ -1,15 +0,0 @@
----
-Language:        Cpp
-BasedOnStyle:  Google
-
-ColumnLimit: 100
-AccessModifierOffset: -2
-AlignAfterOpenBracket: AlwaysBreak
-BreakBeforeBraces: Allman
-ConstructorInitializerIndentWidth: 0
-ContinuationIndentWidth: 2
-DerivePointerAlignment: false
-PointerAlignment: Middle
-ReflowComments: false
-IncludeBlocks: Preserve
-...
diff --git a/dynamics_interface_fd/CMakeLists.txt b/dynamics_interface_fd/CMakeLists.txt
index 04730fb..cfbc472 100644
--- 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
index d4ec6be..8f6cde4 100644
--- 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
index 6c73428..97b04a6 100644
--- 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>
diff --git a/dynamics_interface_fd/src/dynamics_interface_fd.cpp b/dynamics_interface_fd/src/dynamics_interface_fd.cpp
index bd29109..0b197e5 100644
--- a/dynamics_interface_fd/src/dynamics_interface_fd.cpp
+++ b/dynamics_interface_fd/src/dynamics_interface_fd.cpp
@@ -28,10 +28,8 @@ rclcpp::Logger LOGGER = rclcpp::get_logger("dynamics_interface_fd");
 bool fromMsg(const std_msgs::msg::Float64MultiArray & m, Eigen::Matrix<double, 6, 6> & e)
 {
   int ii = 0;
-  for (int i = 0; i < e.rows(); ++i)
-  {
-    for (int j = 0; j < e.cols(); ++j)
-    {
+  for (int i = 0; i < e.rows(); ++i) {
+    for (int j = 0; j < e.cols(); ++j) {
       e(i, j) = m.data[ii++];
     }
   }
@@ -39,9 +37,9 @@ bool fromMsg(const std_msgs::msg::Float64MultiArray & m, Eigen::Matrix<double, 6
 }
 
 DynamicsInterfaceFd::DynamicsInterfaceFd()
-: initialized(false), rt_fd_inertia_subscriber_ptr_(nullptr)
+: DynamicsInterfaceKDL::DynamicsInterfaceKDL(),
+  rt_fd_inertia_subscriber_ptr_(nullptr)
 {
-  // nothing to do
 }
 
 DynamicsInterfaceFd::~DynamicsInterfaceFd()
@@ -49,8 +47,7 @@ DynamicsInterfaceFd::~DynamicsInterfaceFd()
   RCLCPP_INFO(LOGGER, "Deactivating DynamicsInterfaceFd internal communication... please wait...");
 
   // shutdown node thread
-  if (node_thread_ != nullptr)
-  {
+  if (node_thread_ != nullptr) {
     executor_.cancel();
     node_thread_->join();
     node_thread_.reset();
@@ -64,117 +61,23 @@ bool DynamicsInterfaceFd::initialize(
   std::shared_ptr<rclcpp::node_interfaces::NodeParametersInterface> parameters_interface,
   const std::string & param_namespace)
 {
-  // track initialization plugin
-  initialized = true;
-
-  // get parameters
-  std::string ns = !param_namespace.empty() ? param_namespace + "." : "";
-
-  std::string robot_description_local;
-  if (robot_description.empty())
-  {
-    // If the robot_description input argument is empty, try to get the
-    // robot_description from the node's parameters.
-    auto robot_param = rclcpp::Parameter();
-    if (!parameters_interface->get_parameter("robot_description", robot_param))
-    {
-      RCLCPP_ERROR(LOGGER, "parameter robot_description not set in kinematics_interface_kdl");
-      return false;
-    }
-    robot_description_local = robot_param.as_string();
-  }
-  else
-  {
-    robot_description_local = robot_description;
-  }
-
-  // get end-effector name
-  auto end_effector_name_param = rclcpp::Parameter("tip");
-  if (parameters_interface->has_parameter(ns + "tip"))
-  {
-    parameters_interface->get_parameter(ns + "tip", end_effector_name_param);
-  }
-  else
-  {
-    RCLCPP_ERROR(LOGGER, "Failed to find end effector name parameter [tip].");
+  if (!DynamicsInterfaceKDL::initialize(robot_description, parameters_interface, param_namespace)) {
+    RCLCPP_ERROR(LOGGER, "Failed to initialize DynamicsInterfaceKDL");
     return false;
   }
-  std::string end_effector_name = end_effector_name_param.as_string();
-
-  // create kinematic chain
-  KDL::Tree robot_tree;
-  kdl_parser::treeFromString(robot_description_local, robot_tree);
 
-  // get root name
-  auto base_param = rclcpp::Parameter();
-  if (parameters_interface->has_parameter(ns + "base"))
-  {
-    parameters_interface->get_parameter(ns + "base", base_param);
-    root_name_ = base_param.as_string();
-  }
-  else
-  {
-    root_name_ = robot_tree.getRootSegment()->first;
-  }
-
-  if (!robot_tree.getChain(root_name_, end_effector_name, chain_))
-  {
-    RCLCPP_ERROR(
-      LOGGER, "failed to find chain from robot root %s to end effector %s", root_name_.c_str(),
-      end_effector_name.c_str());
-    return false;
-  }
+  // get parameters
+  std::string ns = !param_namespace.empty() ? param_namespace + "." : "";
 
-  // get root name
+  // get inertia topic name
   auto fd_inertia_topic_name_param = rclcpp::Parameter();
-  if (parameters_interface->has_parameter(ns + "fd_inertia_topic_name"))
-  {
+  if (parameters_interface->has_parameter(ns + "fd_inertia_topic_name")) {
     parameters_interface->get_parameter(ns + "fd_inertia_topic_name", fd_inertia_topic_name_param);
     fd_inertia_topic_name_ = fd_inertia_topic_name_param.as_string();
-  }
-  else
-  {
+  } else {
     fd_inertia_topic_name_ = "fd_inertia";
   }
 
-  if (!robot_tree.getChain(root_name_, end_effector_name, chain_))
-  {
-    RCLCPP_ERROR(
-      LOGGER, "failed to find chain from robot root %s to end effector %s", root_name_.c_str(),
-      end_effector_name.c_str());
-    return false;
-  }
-
-  // create map from link names to their index
-  for (size_t i = 0; i < chain_.getNrOfSegments(); ++i)
-  {
-    link_name_map_[chain_.getSegment(i).getName()] = i + 1;
-  }
-
-  // get alpha damping term
-  auto alpha_param = rclcpp::Parameter("alpha", 0.000005);
-  if (parameters_interface->has_parameter(ns + "alpha"))
-  {
-    parameters_interface->get_parameter(ns + "alpha", alpha_param);
-  }
-  alpha = alpha_param.as_double();
-
-  // allocate dynamics memory
-  num_joints_ = chain_.getNrOfJoints();
-  q_ = KDL::JntArray(num_joints_);
-  q_dot_ = KDL::JntArray(num_joints_);
-  q_array_vel_ = KDL::JntArrayVel(num_joints_);  // container for for q AND q_dot_
-  I = Eigen::MatrixXd(num_joints_, num_joints_);
-  I.setIdentity();
-
-  jacobian_ = std::make_shared<KDL::Jacobian>(num_joints_);
-  jacobian_derivative_ = std::make_shared<KDL::Jacobian>(num_joints_);
-
-  // create KDL solvers
-  fk_pos_solver_ = std::make_shared<KDL::ChainFkSolverPos_recursive>(chain_);
-  jac_solver_ = std::make_shared<KDL::ChainJntToJacSolver>(chain_);
-  jac_dot_solver_ = std::make_shared<KDL::ChainJntToJacDotSolver>(chain_);
-
   // Setup internal inertia subscriber
   RCLCPP_INFO(LOGGER, "Setting up internal inertia subscriber... please wait...");
   rclcpp::NodeOptions options;
@@ -187,7 +90,7 @@ bool DynamicsInterfaceFd::initialize(
   fd_inertia_subscriber_ptr_ = async_node_->create_subscription<std_msgs::msg::Float64MultiArray>(
     fd_inertia_topic_name_, rclcpp::SystemDefaultsQoS(),
     [this](const std_msgs::msg::Float64MultiArray::SharedPtr msg)
-    { rt_fd_inertia_subscriber_ptr_.writeFromNonRT(msg); });
+    {rt_fd_inertia_subscriber_ptr_.writeFromNonRT(msg);});
 
   node_thread_ = std::make_unique<std::thread>(
     [&]()
@@ -206,21 +109,18 @@ bool DynamicsInterfaceFd::initialize(
 bool DynamicsInterfaceFd::fill_joints_if_missing(
   const Eigen::VectorXd & joint_values, Eigen::VectorXd & joint_values_filled)
 {
-  if (joint_values.size() == 0 || static_cast<size_t>(joint_values.size()) > num_joints_)
-  {
+  if (joint_values.size() == 0 || static_cast<size_t>(joint_values.size()) > num_joints_) {
     // TODO(tpoignonec) add warning
     return false;
   }
 
-  if (joint_values.size() != 3 && joint_values.size() != 6)
-  {
+  if (joint_values.size() != 3 && joint_values.size() != 6) {
     // should be either 3 or 6!
     // TODO(tpoignonec) add warning
     return false;
   }
 
-  if (static_cast<size_t>(joint_values.size()) == num_joints_)
-  {
+  if (static_cast<size_t>(joint_values.size()) == num_joints_) {
     joint_values_filled = joint_values;
     return true;
   }
@@ -230,98 +130,6 @@ bool DynamicsInterfaceFd::fill_joints_if_missing(
   return true;
 }
 
-// FK, Jacobian, and Jacobian time derivative
-// --------------------------------------------------------------------
-
-bool DynamicsInterfaceFd::calculate_link_transform(
-  const Eigen::Matrix<double, Eigen::Dynamic, 1> & joint_pos, const std::string & link_name,
-  Eigen::Isometry3d & transform)
-{
-  // verify inputs
-  if (!verify_initialized() || !verify_joint_vector(joint_pos) || !verify_link_name(link_name))
-  {
-    return false;
-  }
-
-  // get joint array
-  if (!fill_joints_if_missing(joint_pos, q_.data))
-  {
-    return false;
-  }
-
-  // reset transform_vec
-  transform.setIdentity();
-
-  // special case: since the root is not in the robot tree, need to return identity transform
-  if (link_name == root_name_)
-  {
-    return true;
-  }
-
-  // create forward kinematics solver
-  fk_pos_solver_->JntToCart(q_, frame_, link_name_map_[link_name]);
-  tf2::transformKDLToEigen(frame_, transform);
-  return true;
-}
-
-bool DynamicsInterfaceFd::calculate_jacobian(
-  const Eigen::Matrix<double, Eigen::Dynamic, 1> & joint_pos, const std::string & link_name,
-  Eigen::Matrix<double, 6, Eigen::Dynamic> & jacobian)
-{
-  // verify inputs
-  if (
-    !verify_initialized() || !verify_joint_vector(joint_pos) || !verify_link_name(link_name) ||
-    !verify_jacobian(jacobian))
-  {
-    return false;
-  }
-
-  // get joint array
-  if (!fill_joints_if_missing(joint_pos, q_.data))
-  {
-    return false;
-  }
-
-  // calculate Jacobian
-  jac_solver_->JntToJac(q_, *jacobian_, link_name_map_[link_name]);
-  jacobian = jacobian_->data;
-
-  return true;
-}
-
-bool DynamicsInterfaceFd::calculate_jacobian_derivative(
-  const Eigen::Matrix<double, Eigen::Dynamic, 1> & joint_pos,
-  const Eigen::Matrix<double, Eigen::Dynamic, 1> & joint_vel, const std::string & link_name,
-  Eigen::Matrix<double, 6, Eigen::Dynamic> & jacobian_derivative)
-{
-  // verify inputs
-  if (
-    !verify_initialized() || !verify_joint_vector(joint_pos) || !verify_joint_vector(joint_vel) ||
-    !verify_link_name(link_name) || !verify_jacobian(jacobian_derivative))
-  {
-    return false;
-  }
-
-  // get joint array
-  if (!fill_joints_if_missing(joint_pos, q_.data))
-  {
-    return false;
-  }
-  if (!fill_joints_if_missing(joint_vel, q_dot_.data))
-  {
-    return false;
-  }
-
-  q_array_vel_.q = q_;
-  q_array_vel_.qdot = q_dot_;
-
-  // calculate Jacobian
-  jac_dot_solver_->JntToJacDot(q_array_vel_, *jacobian_derivative_, link_name_map_[link_name]);
-  jacobian_derivative = jacobian_derivative_->data;
-
-  return true;
-}
-
 // Dynamics
 // --------------------------------------------------------------------
 
@@ -330,8 +138,7 @@ bool DynamicsInterfaceFd::calculate_inertia(
   Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> & inertia)
 {
   // verify inputs
-  if (!verify_initialized() || !verify_joint_vector(joint_pos) || !verify_inertia(inertia))
-  {
+  if (!verify_initialized() || !verify_joint_vector(joint_pos) || !verify_inertia(inertia)) {
     return false;
   }
 
@@ -339,33 +146,27 @@ bool DynamicsInterfaceFd::calculate_inertia(
   fd_inertia_msg_ = *rt_fd_inertia_subscriber_ptr_.readFromRT();
 
   // if message exists, load values into references
-  if (!fd_inertia_msg_.get())
-  {
+  if (!fd_inertia_msg_.get()) {
     RCLCPP_ERROR(LOGGER, "No message received from fd_inertia real-time subscriber.");
     return false;
   }
 
   // check dimension of message
-  if (fd_inertia_msg_->data.size() != static_cast<size_t>(36))
-  {
+  if (fd_inertia_msg_->data.size() != static_cast<size_t>(36)) {
     RCLCPP_ERROR(
-      LOGGER, "The size of the inertia matrix msg (%zu) does not match the required size of (6x6)",
-      fd_inertia_msg_->data.size(), num_joints_ * num_joints_);
+      LOGGER,
+      "The size of the inertia matrix msg (%zu) does not match the required size of (6 x 6)",
+      fd_inertia_msg_->data.size());
     return false;
   }
   fromMsg(*fd_inertia_msg_, fd_inertia_);
 
   // load values from message
-  if (num_joints_ == 3)
-  {
+  if (num_joints_ == 3) {
     inertia = fd_inertia_.block<3, 3>(0, 0);
-  }
-  else if (num_joints_ == 6)
-  {
+  } else if (num_joints_ == 6) {
     inertia = fd_inertia_;
-  }
-  else
-  {
+  } else {
     RCLCPP_ERROR(LOGGER, "Invalid number of joints (%lu).", num_joints_);
     return false;
   }
@@ -392,8 +193,7 @@ bool DynamicsInterfaceFd::calculate_gravity(
   const Eigen::VectorXd & joint_pos, Eigen::VectorXd & gravity)
 {
   // verify inputs
-  if (!verify_initialized() || !verify_joint_vector(joint_pos) || !verify_gravity(gravity))
-  {
+  if (!verify_initialized() || !verify_joint_vector(joint_pos) || !verify_gravity(gravity)) {
     return false;
   }
 
@@ -402,168 +202,6 @@ bool DynamicsInterfaceFd::calculate_gravity(
   return true;
 }
 
-// Kinematics
-// --------------------------------------------------------------------
-
-bool DynamicsInterfaceFd::convert_joint_deltas_to_cartesian_deltas(
-  const Eigen::Matrix<double, Eigen::Dynamic, 1> & joint_pos,
-  const Eigen::Matrix<double, Eigen::Dynamic, 1> & delta_theta, const std::string & link_name,
-  Eigen::Matrix<double, 6, 1> & delta_x)
-{
-  // verify inputs
-  if (
-    !verify_initialized() || !verify_joint_vector(joint_pos) || !verify_link_name(link_name) ||
-    !verify_joint_vector(delta_theta))
-  {
-    return false;
-  }
-
-  // get joint array
-  if (!fill_joints_if_missing(joint_pos, q_.data))
-  {
-    return false;
-  }
-
-  // calculate Jacobian
-  jac_solver_->JntToJac(q_, *jacobian_, link_name_map_[link_name]);
-  delta_x = jacobian_->data * delta_theta;
-
-  return true;
-}
-
-bool DynamicsInterfaceFd::convert_cartesian_deltas_to_joint_deltas(
-  const Eigen::Matrix<double, Eigen::Dynamic, 1> & joint_pos,
-  const Eigen::Matrix<double, 6, 1> & delta_x, const std::string & link_name,
-  Eigen::Matrix<double, Eigen::Dynamic, 1> & delta_theta)
-{
-  // verify inputs
-  if (
-    !verify_initialized() || !verify_joint_vector(joint_pos) || !verify_link_name(link_name) ||
-    !verify_joint_vector(delta_theta))
-  {
-    return false;
-  }
-
-  // get joint array
-  if (!fill_joints_if_missing(joint_pos, q_.data))
-  {
-    return false;
-  }
-
-  // calculate Jacobian
-  jac_solver_->JntToJac(q_, *jacobian_, link_name_map_[link_name]);
-  // TODO(anyone): this dynamic allocation needs to be replaced
-  Eigen::Matrix<double, 6, Eigen::Dynamic> J = jacobian_->data;
-  // damped inverse
-  Eigen::Matrix<double, Eigen::Dynamic, 6> J_inverse =
-    (J.transpose() * J + alpha * I).inverse() * J.transpose();
-  delta_theta = J_inverse * delta_x;
-
-  return true;
-}
-
-// Verification functions
-// --------------------------------------------------------------------
-
-bool DynamicsInterfaceFd::verify_link_name(const std::string & link_name)
-{
-  if (link_name == root_name_)
-  {
-    return true;
-  }
-  if (link_name_map_.find(link_name) == link_name_map_.end())
-  {
-    std::string links;
-    for (size_t i = 0; i < chain_.getNrOfSegments(); ++i)
-    {
-      links += "\n" + chain_.getSegment(i).getName();
-    }
-    RCLCPP_ERROR(
-      LOGGER, "The link %s was not found in the robot chain. Available links are: %s",
-      link_name.c_str(), links.c_str());
-    return false;
-  }
-  return true;
-}
-
-bool DynamicsInterfaceFd::verify_joint_vector(const Eigen::VectorXd & joint_vector)
-{
-  if (static_cast<size_t>(joint_vector.size()) != num_joints_)
-  {
-    RCLCPP_ERROR(
-      LOGGER, "Invalid joint vector size (%zu). Expected size is %zu.", joint_vector.size(),
-      num_joints_);
-    return false;
-  }
-  return true;
-}
-
-bool DynamicsInterfaceFd::verify_initialized()
-{
-  // check if interface is initialized
-  if (!initialized)
-  {
-    RCLCPP_ERROR(
-      LOGGER,
-      "The KDL kinematics plugin was not initialized. Ensure you called the initialize method.");
-    return false;
-  }
-  return true;
-}
-
-bool DynamicsInterfaceFd::verify_jacobian(const Eigen::Matrix<double, 6, Eigen::Dynamic> & jacobian)
-{
-  if (jacobian.rows() != jacobian_->rows() || jacobian.cols() != jacobian_->columns())
-  {
-    RCLCPP_ERROR(
-      LOGGER, "The size of the jacobian (%zu, %zu) does not match the required size of (%u, %u)",
-      jacobian.rows(), jacobian.cols(), jacobian_->rows(), jacobian_->columns());
-    return false;
-  }
-  return true;
-}
-
-bool DynamicsInterfaceFd::verify_inertia(
-  const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> & inertia)
-{
-  if (
-    static_cast<size_t>(inertia.rows()) != num_joints_ ||
-    static_cast<size_t>(inertia.cols()) != num_joints_)
-  {
-    RCLCPP_ERROR(
-      LOGGER,
-      "The size of the inertia matrix (%zu, %zu) does not match the required size of (%lu, %lu)",
-      inertia.rows(), inertia.cols(), num_joints_, num_joints_);
-    return false;
-  }
-  return true;
-}
-
-bool DynamicsInterfaceFd::verify_coriolis(const Eigen::VectorXd & coriolis)
-{
-  if (static_cast<size_t>(coriolis.size()) != num_joints_)
-  {
-    RCLCPP_ERROR(
-      LOGGER, "The size of the coriolis vector (%zu) does not match the required size of (%lu)",
-      coriolis.size(), num_joints_);
-    return false;
-  }
-  return true;
-}
-
-bool DynamicsInterfaceFd::verify_gravity(const Eigen::VectorXd & gravity)
-{
-  if (static_cast<size_t>(gravity.size()) != num_joints_)
-  {
-    RCLCPP_ERROR(
-      LOGGER, "The size of the gravity vector (%zu) does not match the required size of (%lu)",
-      gravity.size(), num_joints_);
-    return false;
-  }
-
-  return true;
-}
-
 }  // namespace dynamics_interface_fd
 
 #include "pluginlib/class_list_macros.hpp"
diff --git a/dynamics_interface_fd/test/test_dynamics_interface_fd.cpp b/dynamics_interface_fd/test/test_dynamics_interface_fd.cpp
index 4b9205f..0fe5a05 100644
--- a/dynamics_interface_fd/test/test_dynamics_interface_fd.cpp
+++ b/dynamics_interface_fd/test/test_dynamics_interface_fd.cpp
@@ -75,10 +75,8 @@ class TestDynamicsFdPlugin : public ::testing::Test
   {
     // wait for subscriber
     size_t wait_count = 0;
-    while (node_->count_subscribers(fd_inertia_topic_name) == 0)
-    {
-      if (wait_count >= 5)
-      {
+    while (node_->count_subscribers(fd_inertia_topic_name) == 0) {
+      if (wait_count >= 5) {
         auto error_msg =
           std::string("publishing to ") + fd_inertia_topic_name + " but no node subscribes to it";
         throw std::runtime_error(error_msg);
@@ -90,10 +88,8 @@ class TestDynamicsFdPlugin : public ::testing::Test
     // publish
     std_msgs::msg::Float64MultiArray msg;
     msg.data.resize(36);
-    for (size_t i = 0; i < 6; i++)
-    {
-      for (size_t j = 0; j < 6; j++)
-      {
+    for (size_t i = 0; i < 6; i++) {
+      for (size_t j = 0; j < 6; j++) {
         msg.data[i * 6 + j] = inertia(i, j);
       }
     }
@@ -155,6 +151,9 @@ TEST_F(TestDynamicsFdPlugin, FD_plugin_function_with_omega6_urdf)
   // initialize the  plugin
   auto robot_param = rclcpp::Parameter();
   ASSERT_TRUE(node_->get_parameter("robot_description", robot_param));
+  auto robot_description_str = robot_param.as_string();
+  ASSERT_TRUE(
+    dyn_->initialize(robot_description_str, node_->get_node_parameters_interface(), "dynamics"));
   RCLCPP_INFO(node_->get_logger(), "Plugin instantiated successfully!");
 
   // publish inertia matrix
@@ -214,8 +213,7 @@ TEST_F(TestDynamicsFdPlugin, FD_plugin_function_with_omega6_urdf)
     dyn_->convert_joint_deltas_to_cartesian_deltas(pos, delta_theta, end_effector_, delta_x_est));
 
   // Ensure kinematics math is correct
-  for (size_t i = 0; i < static_cast<size_t>(delta_x.size()); ++i)
-  {
+  for (size_t i = 0; i < static_cast<size_t>(delta_x.size()); ++i) {
     ASSERT_NEAR(delta_x[i], delta_x_est[i], 0.02);
   }
   RCLCPP_INFO(node_->get_logger(), "All good so far...");
@@ -234,6 +232,9 @@ TEST_F(TestDynamicsFdPlugin, FD_plugin_function_with_omega3_urdf)
   // initialize the  plugin
   auto robot_param = rclcpp::Parameter();
   ASSERT_TRUE(node_->get_parameter("robot_description", robot_param));
+  auto robot_description_str = robot_param.as_string();
+  ASSERT_TRUE(
+    dyn_->initialize(robot_description_str, node_->get_node_parameters_interface(), "dynamics"));
   RCLCPP_INFO(node_->get_logger(), "Plugin instantiated successfully!");
 
   // publish inertia matrix
@@ -293,8 +294,7 @@ TEST_F(TestDynamicsFdPlugin, FD_plugin_function_with_omega3_urdf)
     dyn_->convert_joint_deltas_to_cartesian_deltas(pos, delta_theta, end_effector_, delta_x_est));
 
   // Ensure kinematics math is correct
-  for (size_t i = 0; i < static_cast<size_t>(delta_x.size()); ++i)
-  {
+  for (size_t i = 0; i < static_cast<size_t>(delta_x.size()); ++i) {
     ASSERT_NEAR(delta_x[i], delta_x_est[i], 0.02);
   }
 }
@@ -307,6 +307,10 @@ TEST_F(TestDynamicsFdPlugin, FD_plugin_function_std_vector)
   // initialize the  plugin
   auto robot_param = rclcpp::Parameter();
   ASSERT_TRUE(node_->get_parameter("robot_description", robot_param));
+  auto robot_description_str = robot_param.as_string();
+  ASSERT_TRUE(
+    dyn_->initialize(robot_description_str, node_->get_node_parameters_interface(), "dynamics"));
+  RCLCPP_INFO(node_->get_logger(), "Plugin instantiated successfully!");
 
   // publish inertia matrix
   Eigen::Matrix<double, 6, 6> mock_inertia = 2 * Eigen::Matrix<double, 6, 6>::Identity();
@@ -352,8 +356,7 @@ TEST_F(TestDynamicsFdPlugin, FD_plugin_function_std_vector)
     dyn_->convert_joint_deltas_to_cartesian_deltas(pos, delta_theta, end_effector_, delta_x_est));
 
   // Ensure kinematics math is correct
-  for (size_t i = 0; i < static_cast<size_t>(delta_x.size()); ++i)
-  {
+  for (size_t i = 0; i < static_cast<size_t>(delta_x.size()); ++i) {
     ASSERT_NEAR(delta_x[i], delta_x_est[i], 0.02);
   }
 }
@@ -368,6 +371,9 @@ TEST_F(TestDynamicsFdPlugin, no_inertia_published)
   // initialize the  plugin
   auto robot_param = rclcpp::Parameter();
   ASSERT_TRUE(node_->get_parameter("robot_description", robot_param));
+  auto robot_description_str = robot_param.as_string();
+  ASSERT_TRUE(
+    dyn_->initialize(robot_description_str, node_->get_node_parameters_interface(), "dynamics"));
   RCLCPP_INFO(node_->get_logger(), "Plugin instantiated successfully!");
 
   // dummy joint position and velocity
@@ -388,9 +394,9 @@ TEST_F(TestDynamicsFdPlugin, incorrect_input_sizes)
   auto robot_param = rclcpp::Parameter();
   ASSERT_TRUE(node_->get_parameter("robot_description", robot_param));
   auto robot_description_str = robot_param.as_string();
-
   ASSERT_TRUE(
     dyn_->initialize(robot_description_str, node_->get_node_parameters_interface(), "dynamics"));
+  RCLCPP_INFO(node_->get_logger(), "Plugin instantiated successfully!");
 
   // define correct values
   Eigen::Matrix<double, Eigen::Dynamic, 1> pos =
@@ -490,8 +496,7 @@ TEST_F(TestDynamicsFdPlugin, FD_plugin_as_kinematics_interface_only)
     kyn_->convert_joint_deltas_to_cartesian_deltas(pos, delta_theta, end_effector_, delta_x_est));
 
   // Ensure kinematics math is correct
-  for (size_t i = 0; i < static_cast<size_t>(delta_x.size()); ++i)
-  {
+  for (size_t i = 0; i < static_cast<size_t>(delta_x.size()); ++i) {
     ASSERT_NEAR(delta_x[i], delta_x_est[i], 0.02);
   }
 }
diff --git a/dynamics_interface_kdl/include/dynamics_interface_kdl/dynamics_interface_kdl.hpp b/dynamics_interface_kdl/include/dynamics_interface_kdl/dynamics_interface_kdl.hpp
index 6855d32..ae1eb9d 100644
--- a/dynamics_interface_kdl/include/dynamics_interface_kdl/dynamics_interface_kdl.hpp
+++ b/dynamics_interface_kdl/include/dynamics_interface_kdl/dynamics_interface_kdl.hpp
@@ -67,6 +67,10 @@ class DynamicsInterfaceKDL : public dynamics_interface::DynamicsInterface
     const std::string & link_name,
     Eigen::Matrix<double, 6, Eigen::Dynamic> & jacobian_derivative) override;
 
+  bool calculate_jacobian_inverse(
+    const Eigen::VectorXd & joint_pos, const std::string & link_name,
+    Eigen::Matrix<double, Eigen::Dynamic, 6> & jacobian_inverse) override;
+
   bool calculate_inertia(
     const Eigen::VectorXd & joint_pos,
     Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> & inertia) override;
@@ -85,12 +89,13 @@ class DynamicsInterfaceKDL : public dynamics_interface::DynamicsInterface
     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:
   // 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_jacobian_inverse(const Eigen::Matrix<double, Eigen::Dynamic, 6> & 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);
@@ -105,6 +110,7 @@ class DynamicsInterfaceKDL : public dynamics_interface::DynamicsInterface
   KDL::JntArrayVel q_array_vel_;
   KDL::Frame frame_;
   std::shared_ptr<KDL::Jacobian> jacobian_, jacobian_derivative_;
+  std::shared_ptr<Eigen::Matrix<double, Eigen::Dynamic, 6>> jacobian_inverse_;
   std::shared_ptr<KDL::JntSpaceInertiaMatrix> inertia_;
   KDL::JntArray coriolis_, gravity_;
   std::shared_ptr<KDL::ChainJntToJacSolver> jac_solver_;
diff --git a/dynamics_interface_kdl/src/dynamics_interface_kdl.cpp b/dynamics_interface_kdl/src/dynamics_interface_kdl.cpp
index ee324fc..f122012 100644
--- a/dynamics_interface_kdl/src/dynamics_interface_kdl.cpp
+++ b/dynamics_interface_kdl/src/dynamics_interface_kdl.cpp
@@ -34,31 +34,31 @@ bool DynamicsInterfaceKDL::initialize(
   std::string ns = !param_namespace.empty() ? param_namespace + "." : "";
 
   std::string robot_description_local;
-  if (robot_description.empty())
-  {
+  if (robot_description.empty()) {
     // If the robot_description input argument is empty, try to get the
     // robot_description from the node's parameters.
     auto robot_param = rclcpp::Parameter();
-    if (!parameters_interface->get_parameter("robot_description", robot_param))
-    {
+    if (!parameters_interface->get_parameter("robot_description", robot_param)) {
       RCLCPP_ERROR(LOGGER, "parameter robot_description not set in kinematics_interface_kdl");
       return false;
     }
     robot_description_local = robot_param.as_string();
-  }
-  else
-  {
+  } else {
     robot_description_local = robot_description;
   }
 
+  // get alpha damping term
+  auto alpha_param = rclcpp::Parameter("alpha", 0.000005);
+  if (parameters_interface->has_parameter(ns + "alpha")) {
+    parameters_interface->get_parameter(ns + "alpha", alpha_param);
+  }
+  alpha = alpha_param.as_double();
+
   // get end-effector name
   auto end_effector_name_param = rclcpp::Parameter("tip");
-  if (parameters_interface->has_parameter(ns + "tip"))
-  {
+  if (parameters_interface->has_parameter(ns + "tip")) {
     parameters_interface->get_parameter(ns + "tip", end_effector_name_param);
-  }
-  else
-  {
+  } else {
     RCLCPP_ERROR(LOGGER, "Failed to find end effector name parameter [tip].");
     return false;
   }
@@ -70,26 +70,21 @@ bool DynamicsInterfaceKDL::initialize(
 
   // get root name
   auto base_param = rclcpp::Parameter();
-  if (parameters_interface->has_parameter(ns + "base"))
-  {
+  if (parameters_interface->has_parameter(ns + "base")) {
     parameters_interface->get_parameter(ns + "base", base_param);
     root_name_ = base_param.as_string();
-  }
-  else
-  {
+  } else {
     root_name_ = robot_tree.getRootSegment()->first;
   }
 
-  if (!robot_tree.getChain(root_name_, end_effector_name, chain_))
-  {
+  if (!robot_tree.getChain(root_name_, end_effector_name, chain_)) {
     RCLCPP_ERROR(
       LOGGER, "failed to find chain from robot root %s to end effector %s", root_name_.c_str(),
       end_effector_name.c_str());
     return false;
   }
 
-  if (!robot_tree.getChain(root_name_, end_effector_name, chain_))
-  {
+  if (!robot_tree.getChain(root_name_, end_effector_name, chain_)) {
     RCLCPP_ERROR(
       LOGGER, "failed to find chain from robot root %s to end effector %s", root_name_.c_str(),
       end_effector_name.c_str());
@@ -97,19 +92,16 @@ bool DynamicsInterfaceKDL::initialize(
   }
 
   // create map from link names to their index
-  for (size_t i = 0; i < chain_.getNrOfSegments(); ++i)
-  {
+  for (size_t i = 0; i < chain_.getNrOfSegments(); ++i) {
     link_name_map_[chain_.getSegment(i).getName()] = i + 1;
   }
 
   // get gravity vector in base frame
   auto gravity_param = rclcpp::Parameter();
-  if (parameters_interface->has_parameter(ns + "gravity"))
-  {
+  if (parameters_interface->has_parameter(ns + "gravity")) {
     parameters_interface->get_parameter(ns + "gravity", gravity_param);
     std::vector<double> gravity_vec = gravity_param.as_double_array();
-    if (gravity_vec.size() != 3)
-    {
+    if (gravity_vec.size() != 3) {
       RCLCPP_ERROR(
         LOGGER, "The size of the gravity vector (%zu) does not match the required size of 3",
         gravity_vec.size());
@@ -118,9 +110,7 @@ bool DynamicsInterfaceKDL::initialize(
     gravity_in_base_frame_[0] = gravity_vec[0];
     gravity_in_base_frame_[1] = gravity_vec[1];
     gravity_in_base_frame_[2] = gravity_vec[2];
-  }
-  else
-  {
+  } else {
     RCLCPP_ERROR(LOGGER, "Please specify a gravity vector in base frame '%s'.", root_name_.c_str());
     return false;
     /*
@@ -144,6 +134,7 @@ bool DynamicsInterfaceKDL::initialize(
 
   jacobian_ = std::make_shared<KDL::Jacobian>(num_joints_);
   jacobian_derivative_ = std::make_shared<KDL::Jacobian>(num_joints_);
+  jacobian_inverse_ = std::make_shared<Eigen::Matrix<double, Eigen::Dynamic, 6>>(num_joints_, 6);
   inertia_ = std::make_shared<KDL::JntSpaceInertiaMatrix>(num_joints_);
   coriolis_ = KDL::JntArray(num_joints_);
   gravity_ = KDL::JntArray(num_joints_);
@@ -165,8 +156,7 @@ bool DynamicsInterfaceKDL::calculate_link_transform(
   Eigen::Isometry3d & transform)
 {
   // verify inputs
-  if (!verify_initialized() || !verify_joint_vector(joint_pos) || !verify_link_name(link_name))
-  {
+  if (!verify_initialized() || !verify_joint_vector(joint_pos) || !verify_link_name(link_name)) {
     return false;
   }
 
@@ -177,8 +167,7 @@ bool DynamicsInterfaceKDL::calculate_link_transform(
   transform.setIdentity();
 
   // special case: since the root is not in the robot tree, need to return identity transform
-  if (link_name == root_name_)
-  {
+  if (link_name == root_name_) {
     return true;
   }
 
@@ -210,6 +199,34 @@ bool DynamicsInterfaceKDL::calculate_jacobian(
   return true;
 }
 
+bool DynamicsInterfaceKDL::calculate_jacobian_inverse(
+  const Eigen::Matrix<double, Eigen::Dynamic, 1> & joint_pos, const std::string & link_name,
+  Eigen::Matrix<double, Eigen::Dynamic, 6> & jacobian_inverse)
+{
+  // verify inputs
+  if (
+    !verify_initialized() || !verify_joint_vector(joint_pos) || !verify_link_name(link_name) ||
+    !verify_jacobian_inverse(jacobian_inverse))
+  {
+    return false;
+  }
+
+  // get joint array
+  q_.data = joint_pos;
+
+  // calculate Jacobian
+  jac_solver_->JntToJac(q_, *jacobian_, link_name_map_[link_name]);
+  Eigen::Matrix<double, 6, Eigen::Dynamic> jacobian = jacobian_->data;
+
+  // damped inverse
+  *jacobian_inverse_ =
+    (jacobian.transpose() * jacobian + alpha * I).inverse() * jacobian.transpose();
+
+  jacobian_inverse = *jacobian_inverse_;
+
+  return true;
+}
+
 bool DynamicsInterfaceKDL::calculate_jacobian_derivative(
   const Eigen::Matrix<double, Eigen::Dynamic, 1> & joint_pos,
   const Eigen::Matrix<double, Eigen::Dynamic, 1> & joint_vel, const std::string & link_name,
@@ -245,8 +262,7 @@ bool DynamicsInterfaceKDL::calculate_inertia(
   Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> & inertia)
 {
   // verify inputs
-  if (!verify_initialized() || !verify_joint_vector(joint_pos) || !verify_inertia(inertia))
-  {
+  if (!verify_initialized() || !verify_joint_vector(joint_pos) || !verify_inertia(inertia)) {
     return false;
   }
 
@@ -286,8 +302,7 @@ bool DynamicsInterfaceKDL::calculate_gravity(
   const Eigen::VectorXd & joint_pos, Eigen::VectorXd & gravity)
 {
   // verify inputs
-  if (!verify_initialized() || !verify_joint_vector(joint_pos) || !verify_gravity(gravity))
-  {
+  if (!verify_initialized() || !verify_joint_vector(joint_pos) || !verify_gravity(gravity)) {
     return false;
   }
 
@@ -340,17 +355,12 @@ bool DynamicsInterfaceKDL::convert_cartesian_deltas_to_joint_deltas(
     return false;
   }
 
-  // get joint array
-  q_.data = joint_pos;
+  // calculate Jacobian inverse
+  if (!calculate_jacobian_inverse(joint_pos, link_name, *jacobian_inverse_)) {
+    return false;
+  }
 
-  // calculate Jacobian
-  jac_solver_->JntToJac(q_, *jacobian_, link_name_map_[link_name]);
-  // TODO(anyone): this dynamic allocation needs to be replaced
-  Eigen::Matrix<double, 6, Eigen::Dynamic> J = jacobian_->data;
-  // damped inverse
-  Eigen::Matrix<double, Eigen::Dynamic, 6> J_inverse =
-    (J.transpose() * J + alpha * I).inverse() * J.transpose();
-  delta_theta = J_inverse * delta_x;
+  delta_theta = *jacobian_inverse_ * delta_x;
 
   return true;
 }
@@ -360,15 +370,12 @@ bool DynamicsInterfaceKDL::convert_cartesian_deltas_to_joint_deltas(
 
 bool DynamicsInterfaceKDL::verify_link_name(const std::string & link_name)
 {
-  if (link_name == root_name_)
-  {
+  if (link_name == root_name_) {
     return true;
   }
-  if (link_name_map_.find(link_name) == link_name_map_.end())
-  {
+  if (link_name_map_.find(link_name) == link_name_map_.end()) {
     std::string links;
-    for (size_t i = 0; i < chain_.getNrOfSegments(); ++i)
-    {
+    for (size_t i = 0; i < chain_.getNrOfSegments(); ++i) {
       links += "\n" + chain_.getSegment(i).getName();
     }
     RCLCPP_ERROR(
@@ -381,8 +388,7 @@ bool DynamicsInterfaceKDL::verify_link_name(const std::string & link_name)
 
 bool DynamicsInterfaceKDL::verify_joint_vector(const Eigen::VectorXd & joint_vector)
 {
-  if (static_cast<size_t>(joint_vector.size()) != num_joints_)
-  {
+  if (static_cast<size_t>(joint_vector.size()) != num_joints_) {
     RCLCPP_ERROR(
       LOGGER, "Invalid joint vector size (%zu). Expected size is %zu.", joint_vector.size(),
       num_joints_);
@@ -394,8 +400,7 @@ bool DynamicsInterfaceKDL::verify_joint_vector(const Eigen::VectorXd & joint_vec
 bool DynamicsInterfaceKDL::verify_initialized()
 {
   // check if interface is initialized
-  if (!initialized)
-  {
+  if (!initialized) {
     RCLCPP_ERROR(
       LOGGER,
       "The KDL kinematics plugin was not initialized. Ensure you called the initialize method.");
@@ -407,8 +412,7 @@ bool DynamicsInterfaceKDL::verify_initialized()
 bool DynamicsInterfaceKDL::verify_jacobian(
   const Eigen::Matrix<double, 6, Eigen::Dynamic> & jacobian)
 {
-  if (jacobian.rows() != jacobian_->rows() || jacobian.cols() != jacobian_->columns())
-  {
+  if (jacobian.rows() != jacobian_->rows() || jacobian.cols() != jacobian_->columns()) {
     RCLCPP_ERROR(
       LOGGER, "The size of the jacobian (%zu, %zu) does not match the required size of (%u, %u)",
       jacobian.rows(), jacobian.cols(), jacobian_->rows(), jacobian_->columns());
@@ -417,11 +421,24 @@ bool DynamicsInterfaceKDL::verify_jacobian(
   return true;
 }
 
+bool DynamicsInterfaceKDL::verify_jacobian_inverse(
+  const Eigen::Matrix<double, Eigen::Dynamic, 6> & jacobian_inverse)
+{
+  if (
+    jacobian_inverse.rows() != jacobian_->columns() || jacobian_inverse.cols() != jacobian_->rows())
+  {
+    RCLCPP_ERROR(
+      LOGGER, "The size of the jacobian (%zu, %zu) does not match the required size of (%u, %u)",
+      jacobian_inverse.rows(), jacobian_inverse.cols(), jacobian_->columns(), jacobian_->rows());
+    return false;
+  }
+  return true;
+}
+
 bool DynamicsInterfaceKDL::verify_inertia(
   const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> & inertia)
 {
-  if (inertia.rows() != inertia_->rows() || inertia.cols() != inertia_->columns())
-  {
+  if (inertia.rows() != inertia_->rows() || inertia.cols() != inertia_->columns()) {
     RCLCPP_ERROR(
       LOGGER,
       "The size of the inertia matrix (%zu, %zu) does not match the required size of (%u, %u)",
@@ -433,8 +450,7 @@ bool DynamicsInterfaceKDL::verify_inertia(
 
 bool DynamicsInterfaceKDL::verify_coriolis(const Eigen::VectorXd & coriolis)
 {
-  if (static_cast<size_t>(coriolis.size()) != num_joints_)
-  {
+  if (static_cast<size_t>(coriolis.size()) != num_joints_) {
     RCLCPP_ERROR(
       LOGGER, "The size of the coriolis vector (%zu) does not match the required size of (%lu)",
       coriolis.size(), num_joints_);
@@ -445,8 +461,7 @@ bool DynamicsInterfaceKDL::verify_coriolis(const Eigen::VectorXd & coriolis)
 
 bool DynamicsInterfaceKDL::verify_gravity(const Eigen::VectorXd & gravity)
 {
-  if (static_cast<size_t>(gravity.size()) != num_joints_)
-  {
+  if (static_cast<size_t>(gravity.size()) != num_joints_) {
     RCLCPP_ERROR(
       LOGGER, "The size of the gravity vector (%zu) does not match the required size of (%lu)",
       gravity.size(), num_joints_);
diff --git a/dynamics_interface_kdl/test/test_dynamics_interface_kdl.cpp b/dynamics_interface_kdl/test/test_dynamics_interface_kdl.cpp
index a5f9798..936d501 100644
--- a/dynamics_interface_kdl/test/test_dynamics_interface_kdl.cpp
+++ b/dynamics_interface_kdl/test/test_dynamics_interface_kdl.cpp
@@ -64,7 +64,7 @@ class TestKDLPlugin : public ::testing::Test
   void loadURDFParameter()
   {
     auto urdf = std::string(ros2_control_test_assets::urdf_head) +
-                std::string(ros2_control_test_assets::urdf_tail);
+      std::string(ros2_control_test_assets::urdf_tail);
 
     rclcpp::Parameter param_urdf("robot_description", urdf);
     node_->declare_parameter("robot_description", "");
@@ -153,8 +153,7 @@ TEST_F(TestKDLPlugin, KDL_plugin_function)
     dyn_->convert_joint_deltas_to_cartesian_deltas(pos, delta_theta, end_effector_, delta_x_est));
 
   // Ensure kinematics math is correct
-  for (size_t i = 0; i < static_cast<size_t>(delta_x.size()); ++i)
-  {
+  for (size_t i = 0; i < static_cast<size_t>(delta_x.size()); ++i) {
     ASSERT_NEAR(delta_x[i], delta_x_est[i], 0.02);
   }
 }
@@ -208,8 +207,7 @@ TEST_F(TestKDLPlugin, KDL_plugin_function_std_vector)
     dyn_->convert_joint_deltas_to_cartesian_deltas(pos, delta_theta, end_effector_, delta_x_est));
 
   // Ensure kinematics math is correct
-  for (size_t i = 0; i < static_cast<size_t>(delta_x.size()); ++i)
-  {
+  for (size_t i = 0; i < static_cast<size_t>(delta_x.size()); ++i) {
     ASSERT_NEAR(delta_x[i], delta_x_est[i], 0.02);
   }
 }
@@ -261,6 +259,7 @@ TEST_F(TestKDLPlugin, incorrect_input_sizes)
 TEST_F(TestKDLPlugin, KDL_plugin_no_robot_description)
 {
   // load alpha to parameter server
+  loadURDFParameter();
   loadAlphaParameter();
   loadEndEffectorNameParameter(end_effector_);
   loadGravityParameter();
@@ -295,7 +294,7 @@ TEST_F(TestKDLPlugin, KDL_plugin_no_gravity)
   ASSERT_TRUE(node_->get_parameter("robot_description", robot_param));
   auto robot_description_str = robot_param.as_string();
 
-  ASSERT_TRUE(
+  ASSERT_FALSE(
     dyn_->initialize(robot_description_str, node_->get_node_parameters_interface(), "dynamics"));
 }
 
@@ -323,6 +322,22 @@ TEST_F(TestKDLPlugin, KDL_plugin_as_kinematics_interface_only)
   Eigen::Matrix<double, 6, Eigen::Dynamic> jacobian = Eigen::Matrix<double, 6, 2>::Zero();
   ASSERT_TRUE(kyn_->calculate_jacobian(pos, end_effector_, jacobian));
 
+
+  // calculate jacobian inverse
+  Eigen::Matrix<double, Eigen::Dynamic, 6> jacobian_inverse =
+    jacobian.completeOrthogonalDecomposition().pseudoInverse();
+
+  Eigen::Matrix<double, Eigen::Dynamic, 6> jacobian_inverse_est =
+    Eigen::Matrix<double, 2, 6>::Zero();
+  ASSERT_TRUE(kyn_->calculate_jacobian_inverse(pos, end_effector_, jacobian_inverse_est));
+
+  // ensure jacobian inverse math is correct
+  for (size_t i = 0; i < static_cast<size_t>(jacobian_inverse.rows()); ++i) {
+    for (size_t j = 0; j < static_cast<size_t>(jacobian_inverse.cols()); ++j) {
+      ASSERT_NEAR(jacobian_inverse(i, j), jacobian_inverse_est(i, j), 0.02);
+    }
+  }
+
   // convert cartesian delta to joint delta
   Eigen::Matrix<double, 6, 1> delta_x = Eigen::Matrix<double, 6, 1>::Zero();
   delta_x[2] = 1;
@@ -336,8 +351,7 @@ TEST_F(TestKDLPlugin, KDL_plugin_as_kinematics_interface_only)
     kyn_->convert_joint_deltas_to_cartesian_deltas(pos, delta_theta, end_effector_, delta_x_est));
 
   // Ensure kinematics math is correct
-  for (size_t i = 0; i < static_cast<size_t>(delta_x.size()); ++i)
-  {
+  for (size_t i = 0; i < static_cast<size_t>(delta_x.size()); ++i) {
     ASSERT_NEAR(delta_x[i], delta_x_est[i], 0.02);
   }
 }