calculate jacobian matrix and inertia matrix

cartesian_compliance_controller/include/cartesian_compliance_controller/cartesian_compliance_controller.h

@@ -49,6 +49,15 @@
 #include <dynamic_reconfigure/server.h>
 #include <cartesian_compliance_controller/ComplianceControllerConfig.h>
 
+// KDL
+#include <kdl/frames.hpp>
+#include <kdl/chain.hpp>
+#include <kdl/jacobian.hpp>
+#include <kdl/chainjnttojacsolver.hpp>
+#include <kdl/chaindynparam.hpp>
+#include <kdl/chainfksolverpos_recursive.hpp>
+#include <kdl/chainfksolvervel_recursive.hpp>
+
 namespace cartesian_compliance_controller
 {
 
@@ -114,6 +123,9 @@ class CartesianComplianceController
 
     std::shared_ptr<dynamic_reconfigure::Server<ComplianceConfig> > m_dyn_conf_server;
     dynamic_reconfigure::Server<ComplianceConfig>::CallbackType m_callback_type;
+
+    std::shared_ptr<KDL::ChainJntToJacSolver> m_jnt_jacobian_solver;
+    std::shared_ptr<KDL::ChainDynParam>       m_jnt_space_inertia_solver;
 };
 
 }

cartesian_compliance_controller/include/cartesian_compliance_controller/cartesian_compliance_controller.hpp

@@ -97,6 +97,10 @@ bool CartesianComplianceController<HardwareInterface>::init(
       ros::NodeHandle(nh.getNamespace() + "/stiffness")));
   m_dyn_conf_server->setCallback(m_callback_type);
 
+  // KDL::Chain chain = Base::m_ik_solver->getChain();
+  m_jnt_jacobian_solver.reset(new KDL::ChainJntToJacSolver(Base::m_ik_solver->getChain()));
+  m_jnt_space_inertia_solver.reset(new KDL::ChainDynParam(Base::m_ik_solver->getChain(),KDL::Vector::Zero()));
+
   return true;
 }
 
@@ -195,7 +199,8 @@ CartesianComplianceController<HardwareInterface>::computeComplianceError() {
     double R_det = R_base2surface.determinant();
     std::cout << "R_det:" << std::endl << R_det << std::endl;
     // ctrl::Matrix3D px;
-    // px << 0, -pose.p.z(), pose.p.y(), pose.p.z(), 0, -pose.p.x(), -pose.p.y(),
+    // px << 0, -pose.p.z(), pose.p.y(), pose.p.z(), 0, -pose.p.x(),
+    // -pose.p.y(),
     //     pose.p.x(), 0;
     // std::cout << "px:" << std::endl << px << std::endl;
     // ctrl::Matrix3D pxR;
@@ -230,6 +235,20 @@ CartesianComplianceController<HardwareInterface>::computeComplianceError() {
     //     (ctrl::Matrix6D::Identity() - m_selection_matrix_pd);
     // std::cout << "m_selection_matrix_pd_check: " << std::endl <<
     // m_selection_matrix_pd_check << std::endl;
+    auto current_positions = Base::m_ik_solver->getPositions();
+    auto current_velocity = Base::m_ik_solver->getVelocity();
+    // auto jacobian = Base::m_forward_kinematics_solver->m_jnt_jacobian.data;
+    // auto jacobian = Base::m_forward_kinematics_solver;
+    // Base::m_jnt_space_inertia.data;
+    KDL::JntSpaceInertiaMatrix jnt_space_inertia;
+    m_jnt_space_inertia_solver->JntToMass(current_positions,
+                                          jnt_space_inertia);
+    // std::cout << "jnt_space_inertia: " << std::endl
+    // std::cout << jnt_space_inertia << std::endl;
+    KDL::Jacobian jnt_jacobian;
+    m_jnt_jacobian_solver->JntToJac(current_positions, jnt_jacobian);
+    // std::cout << "jnt_jacobian: " << std::endl
+    // std::cout << jnt_jacobian << std::endl;
 
     net_force =
 

cartesian_controller_base/include/cartesian_controller_base/IKSolver.h

@@ -122,6 +122,10 @@ class IKSolver
      */
     const KDL::JntArray& getPositions() const;
 
+    const KDL::JntArray& getVelocity() const;
+
+    const KDL::Chain getChain() const;
+
     //! Set initial joint configuration
     bool setStartState(const std::vector<hardware_interface::JointHandle>& joint_handles);
 

cartesian_controller_base/src/IKSolver.cpp

@@ -77,6 +77,15 @@ namespace cartesian_controller_base{
     return m_current_positions;
   }
 
+  const KDL::JntArray& IKSolver::getVelocity() const
+  {
+    return m_current_velocities;
+  }
+
+  const KDL::Chain IKSolver::getChain() const
+  {
+    return m_chain;
+  }
 
   bool IKSolver::setStartState(
       const std::vector<hardware_interface::JointHandle>& joint_handles)