C++ Implementation of Inverse Kinematics in WBC

CMakeLists.txt

@@ -47,6 +47,7 @@ set(${PROJECT_NAME}_HEADERS
   include/${PROJECT_NAME}/gepadd.hpp
   include/${PROJECT_NAME}/MPC.hpp
   include/${PROJECT_NAME}/Planner.hpp
+  include/${PROJECT_NAME}/InvKin.hpp
   include/other/st_to_cc.hpp
   #include/osqp_folder/include/osqp_configure.h
   #include/osqp_folder/include/osqp.h
@@ -59,6 +60,7 @@ set(${PROJECT_NAME}_SOURCES
   src/gepadd.cpp
   src/MPC.cpp
   src/Planner.cpp
+  src/InvKin.cpp
   )
 
 add_library(${PROJECT_NAME} SHARED ${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_HEADERS})

include/quadruped-reactive-walking/InvKin.hpp

@@ -8,44 +8,9 @@
 #include <iostream>
 #include <string>
 #include "pinocchio/math/rpy.hpp"
+#include "pinocchio/spatial/explog.hpp"
 
-#define N0_gait 20
-// Number of rows in the gait matrix. Arbitrary value that should be set high
-// enough so that there is always at least one empty line at the end of the gait
-// matrix
-
-typedef Eigen::MatrixXd matXd;
-
-class TrajGen {
-  /* Class that generates a reference trajectory in position, velocity and
-     acceleration that feet it swing phase should follow */
-
- private:
-  double lastCoeffs_x[6];  // Coefficients for the X component
-  double lastCoeffs_y[6];  // Coefficients for the Y component
-  double h = 0.05;         // Apex height of the swinging trajectory
-  double time_adaptative_disabled = 0.2;  // Target lock before the touchdown
-  double x1 = 0.0;                        // Target for the X component
-  double y1 = 0.0;                        // Target for the Y component
-  Eigen::Matrix<double, 11, 1> result =
-      Eigen::Matrix<double, 11, 1>::Zero();  // Output of the generator
-
-  // Coefficients
-  double Ax5 = 0.0, Ax4 = 0.0, Ax3 = 0.0, Ax2 = 0.0, Ax1 = 0.0, Ax0 = 0.0,
-         Ay5 = 0.0, Ay4 = 0.0, Ay3 = 0.0, Ay2 = 0.0, Ay1 = 0.0, Ay0 = 0.0,
-         Az6 = 0.0, Az5 = 0.0, Az4 = 0.0, Az3 = 0.0;
-
- public:
-  TrajGen();  // Empty constructor
-  TrajGen(double h_in, double t_lock_in, double x_in,
-          double y_in);  // Default constructor
-  Eigen::Matrix<double, 11, 1> get_next_foot(double x0, double dx0, double ddx0,
-                                             double y0, double dy0, double ddy0,
-                                             double x1_in, double y1_in,
-                                             double t0, double t1, double dt);
-};
-
-class Planner {
+class InvKin {
   /* Planner that outputs current and future locations of footsteps, the
      reference trajectory of the base and the position, velocity, acceleration
      commands for feet in swing phase based on the reference velocity given by
@@ -144,10 +109,65 @@ class Planner {
   // Inputs of the constructor
   double dt;  // Time step of the contact sequence (time step of the MPC)
 
+  // Matrices initialisation
+  Eigen::Matrix<double, 4, 3> feet_position_ref = Eigen::Matrix<double, 4, 3>::Zero();
+  Eigen::Matrix<double, 4, 3> feet_velocity_ref = Eigen::Matrix<double, 4, 3>::Zero();
+  Eigen::Matrix<double, 4, 3> feet_acceleration_ref = Eigen::Matrix<double, 4, 3>::Zero();
+  Eigen::Matrix<double, 1, 4> flag_in_contact = Eigen::Matrix<double, 1, 4>::Zero();
+  Eigen::Matrix<double, 3, 3> base_orientation_ref = Eigen::Matrix<double, 3, 3>::Zero();
+  Eigen::Matrix<double, 1, 3> base_angularvelocity_ref = Eigen::Matrix<double, 1, 3>::Zero();
+  Eigen::Matrix<double, 1, 3> base_angularacceleration_ref = Eigen::Matrix<double, 1, 3>::Zero();
+  Eigen::Matrix<double, 1, 3> base_position_ref = Eigen::Matrix<double, 1, 3>::Zero();
+  Eigen::Matrix<double, 1, 3> base_linearvelocity_ref = Eigen::Matrix<double, 1, 3>::Zero();
+  Eigen::Matrix<double, 1, 3> base_linearacceleration_ref = Eigen::Matrix<double, 1, 3>::Zero();
+  Eigen::Matrix<double, 6, 1> x_ref = Eigen::Matrix<double, 6, 1>::Zero();
+  Eigen::Matrix<double, 6, 1> x = Eigen::Matrix<double, 6, 1>::Zero();
+  Eigen::Matrix<double, 6, 1> dx_ref = Eigen::Matrix<double, 6, 1>::Zero();
+  Eigen::Matrix<double, 6, 1> dx = Eigen::Matrix<double, 6, 1>::Zero();
+  Eigen::Matrix<double, 18, 18> J = Eigen::Matrix<double, 18, 18>::Zero();
+  Eigen::Matrix<double, 18, 18> invJ = Eigen::Matrix<double, 18, 18>::Zero();
+  Eigen::Matrix<double, 1, 18> acc = Eigen::Matrix<double, 1, 18>::Zero();
+  Eigen::Matrix<double, 1, 18> x_err = Eigen::Matrix<double, 1, 18>::Zero();
+  Eigen::Matrix<double, 1, 18> dx_r = Eigen::Matrix<double, 1, 18>::Zero();
+
+  Eigen::Matrix<double, 4, 3> pfeet_err = Eigen::Matrix<double, 4, 3>::Zero();
+  Eigen::Matrix<double, 4, 3> vfeet_ref = Eigen::Matrix<double, 4, 3>::Zero();
+  Eigen::Matrix<double, 4, 3> afeet = Eigen::Matrix<double, 4, 3>::Zero();
+  Eigen::Matrix<double, 1, 3> e_basispos = Eigen::Matrix<double, 1, 3>::Zero();
+  Eigen::Matrix<double, 1, 3> abasis = Eigen::Matrix<double, 1, 3>::Zero();
+  Eigen::Matrix<double, 1, 3> e_basisrot = Eigen::Matrix<double, 1, 3>::Zero();
+  Eigen::Matrix<double, 1, 3> awbasis = Eigen::Matrix<double, 1, 3>::Zero();
+
+  Eigen::MatrixXd ddq = Eigen::MatrixXd::Zero(18, 1);
+  Eigen::MatrixXd q_step = Eigen::MatrixXd::Zero(18, 1);
+  Eigen::MatrixXd dq_cmd = Eigen::MatrixXd::Zero(18, 1);
+
+  // Gains
+  double Kp_base_orientation = 100.0;
+  double Kd_base_orientation = 2.0 * std::sqrt(Kp_base_orientation);
+  
+  double Kp_base_position = 100.0;
+  double Kd_base_position = 2.0 * std::sqrt(Kp_base_position);
+
+  double Kp_flyingfeet = 100.0;
+  double Kd_flyingfeet = 2.0 * std::sqrt(Kp_flyingfeet);
+
  public:
   InvKin();
-  Invkin(double dt_in);
-
+  InvKin(double dt_in);
+  
+  Eigen::Matrix<double, 1, 3> cross3(Eigen::Matrix<double, 1, 3> left, Eigen::Matrix<double, 1, 3> right);
+
+  Eigen::MatrixXd refreshAndCompute(const Eigen::MatrixXd &x_cmd, const Eigen::MatrixXd &contacts,
+                                    const Eigen::MatrixXd &goals, const Eigen::MatrixXd &vgoals, const Eigen::MatrixXd &agoals,
+                                    const Eigen::MatrixXd &posf, const Eigen::MatrixXd &vf, const Eigen::MatrixXd &wf, const Eigen::MatrixXd &af,
+                                    const Eigen::MatrixXd &Jf, const Eigen::MatrixXd &posb, const Eigen::MatrixXd &rotb, const Eigen::MatrixXd &vb,
+                                    const Eigen::MatrixXd &ab, const Eigen::MatrixXd &Jb);
+  Eigen::MatrixXd computeInvKin(const Eigen::MatrixXd &posf, const Eigen::MatrixXd &vf, const Eigen::MatrixXd &wf, const Eigen::MatrixXd &af,
+                                const Eigen::MatrixXd &Jf, const Eigen::MatrixXd &posb, const Eigen::MatrixXd &rotb, const Eigen::MatrixXd &vb, const Eigen::MatrixXd &ab,
+                                const Eigen::MatrixXd &Jb);
+  Eigen::MatrixXd get_q_step();
+  Eigen::MatrixXd get_dq_cmd();
   /*void Print();
 
   int create_walk();

python/gepadd.cpp

 #include "quadruped-reactive-walking/gepadd.hpp"
 #include "quadruped-reactive-walking/MPC.hpp"
 #include "quadruped-reactive-walking/Planner.hpp"
+#include "quadruped-reactive-walking/InvKin.hpp"
 
 #include <eigenpy/eigenpy.hpp>
 #include <boost/python.hpp>
@@ -64,6 +65,32 @@ struct PlannerPythonVisitor : public bp::def_visitor<PlannerPythonVisitor<Planne
 
 void exposePlanner() { PlannerPythonVisitor<Planner>::expose(); }
 
+template <typename InvKin>
+struct InvKinPythonVisitor : public bp::def_visitor<InvKinPythonVisitor<InvKin> > {
+  template <class PyClassInvKin>
+  void visit(PyClassInvKin& cl) const {
+    cl.def(bp::init<>(bp::arg(""), "Default constructor."))
+        .def(bp::init<double>(bp::args("dt_in"), "Constructor with parameters."))
+
+        .def("get_q_step", &InvKin::get_q_step, "Get velocity goals matrix.\n")
+        .def("get_dq_cmd", &InvKin::get_dq_cmd, "Get acceleration goals matrix.\n")
+
+        // Run InvKin from Python
+        .def("refreshAndCompute", &InvKin::refreshAndCompute, 
+             bp::args("x_cmd", "contacts", "goals", "vgoals", "agoals", "posf", "vf", "wf", "af", "Jf",
+                      "posb", "rotb", "vb", "ab", "Jb"),
+             "Run InvKin from Python.\n");
+  }
+
+  static void expose() {
+    bp::class_<InvKin>("InvKin", bp::no_init).def(InvKinPythonVisitor<InvKin>());
+
+    ENABLE_SPECIFIC_MATRIX_TYPE(matXd);
+  }
+};
+
+void exposeInvKin() { InvKinPythonVisitor<InvKin>::expose(); }
+
 BOOST_PYTHON_MODULE(libquadruped_reactive_walking) {
   boost::python::def("add", gepetto::example::add);
   boost::python::def("sub", gepetto::example::sub);
@@ -72,4 +99,5 @@ BOOST_PYTHON_MODULE(libquadruped_reactive_walking) {
 
   exposeMPC();
   exposePlanner();
+  exposeInvKin();
 }
\ No newline at end of file

scripts/solo12InvKin.py

@@ -3,13 +3,15 @@ from example_robot_data import load
 import time
 import numpy as np
 import pinocchio as pin
-
+import libquadruped_reactive_walking as lrw
 
 class Solo12InvKin:
     def __init__(self, dt):
         self.robot = load('solo12')
         self.dt = dt
 
+        self.InvKinCpp = lrw.InvKin(dt)
+
         # Inputs to be modified bu the user before calling .compute
         self.feet_position_ref = [np.array([0.1946,   0.14695, 0.0191028]), np.array(
             [0.1946,  -0.14695, 0.0191028]), np.array([-0.1946,   0.14695, 0.0191028]),
@@ -43,6 +45,21 @@ class Solo12InvKin:
         # Matrices initialisation
         self.invJ = np.zeros((18, 18))
 
+        self.cpp_posf = np.zeros((4, 3))
+        self.cpp_vf = np.zeros((4, 3))
+        self.cpp_wf = np.zeros((4, 3))
+        self.cpp_af = np.zeros((4, 3))
+        self.cpp_Jf = np.zeros((12, 18))
+
+        self.cpp_posb = np.zeros((1, 3))
+        self.cpp_rotb = np.zeros((3, 3))
+        self.cpp_vb = np.zeros((1, 6))
+        self.cpp_ab = np.zeros((1, 6))
+        self.cpp_Jb = np.zeros((6, 18))
+
+        self.cpp_ddq = np.zeros((18,))
+        self.cpp_q_cmd = np.zeros((19,))
+        self.cpp_dq_cmd = np.zeros((18,))
 
         # Get frame IDs
         FL_FOOT_ID = self.robot.model.getFrameId('FL_FOOT')
@@ -92,12 +109,52 @@ class Solo12InvKin:
             self.feet_acceleration_ref[i] = planner.agoals[0:3, i]
 
         # Update position and velocity reference for the base
-        self.base_position_ref[:] = x_cmd[0:3]
+        """self.base_position_ref[:] = x_cmd[0:3]
         self.base_orientation_ref = pin.utils.rpyToMatrix(x_cmd[3:6])
         self.base_linearvelocity_ref[:] = x_cmd[6:9]
         self.base_angularvelocity_ref[:] = x_cmd[9:12]
 
-        return self.compute(q, dq)
+        ddq = self.compute(q, dq)"""
+
+        # Update model and data
+        pin.computeJointJacobians(self.rmodel, self.rdata, q)
+        pin.forwardKinematics(self.rmodel, self.rdata, q, dq, np.zeros(self.rmodel.nv))
+        pin.updateFramePlacements(self.rmodel, self.rdata)
+
+        # Get data required by IK with Pinocchio
+        for i_ee in range(4):
+            idx = int(self.foot_ids[i_ee])
+            self.cpp_posf[i_ee, :] = self.rdata.oMf[idx].translation
+            nu = pin.getFrameVelocity(self.rmodel, self.rdata, idx, pin.LOCAL_WORLD_ALIGNED)
+            self.cpp_vf[i_ee, :] = nu.linear
+            self.cpp_wf[i_ee, :] = nu.angular
+            self.cpp_af[i_ee, :] = pin.getFrameAcceleration(self.rmodel, self.rdata, idx, pin.LOCAL_WORLD_ALIGNED).linear
+            self.cpp_Jf[(3*i_ee):(3*(i_ee+1)), :] = pin.getFrameJacobian(self.robot.model, self.robot.data, idx, pin.LOCAL_WORLD_ALIGNED)[:3]
+
+        self.cpp_posb[:] = self.rdata.oMf[self.BASE_ID].translation
+        self.cpp_rotb[:, :] = self.rdata.oMf[self.BASE_ID].rotation
+        nu = pin.getFrameVelocity(self.rmodel, self.rdata, self.BASE_ID, pin.LOCAL_WORLD_ALIGNED)
+        self.cpp_vb[0, 0:3] = nu.linear
+        self.cpp_vb[0, 3:6] = nu.angular
+        acc = pin.getFrameAcceleration(self.rmodel, self.rdata, self.BASE_ID, pin.LOCAL_WORLD_ALIGNED)
+        self.cpp_ab[0, 0:3] = acc.linear
+        self.cpp_ab[0, 3:6] = acc.angular
+        self.cpp_Jb[:, :] = pin.getFrameJacobian(self.robot.model, self.robot.data, self.BASE_ID, pin.LOCAL_WORLD_ALIGNED)
+
+        self.cpp_ddq[:] = self.InvKinCpp.refreshAndCompute(np.array([x_cmd]), np.array([contacts]), planner.goals, planner.vgoals, planner.agoals,
+                                                           self.cpp_posf, self.cpp_vf, self.cpp_wf, self.cpp_af, self.cpp_Jf,
+                                                           self.cpp_posb, self.cpp_rotb, self.cpp_vb, self.cpp_ab, self.cpp_Jb)
+
+        self.cpp_q_cmd[:] = pin.integrate(self.robot.model, q, self.InvKinCpp.get_q_step())
+        self.cpp_dq_cmd[:] = self.InvKinCpp.get_dq_cmd()
+
+        self.q_cmd = self.cpp_q_cmd
+        self.dq_cmd = self.cpp_dq_cmd
+
+        """from IPython import embed
+        embed()"""
+
+        return self.cpp_ddq
 
     def compute(self, q, dq):
         # FEET
@@ -135,6 +192,7 @@ class Solo12InvKin:
             self.pfeet_err.append(e1)
             vfeet_ref.append(vref)
 
+
         # BASE POSITION
         idx = self.BASE_ID
         pos = self.rdata.oMf[idx].translation
@@ -193,16 +251,27 @@ class Solo12InvKin:
             self.invJ[(6+3*i):(9+3*i), (6+3*i):(9+3*i)] = inv
         tac = time.time()"""
 
+        print("J:")
+        print(J)
         invJ = np.linalg.pinv(J)  # self.dinv(J)  # or np.linalg.inv(J) since full rank
 
         """toc = time.time()
         print("Old:", toc - tac)
         print("New:", tac - tic)"""
 
+        print("invJ:")
+        print(invJ)
+        print("acc:")
+        print(acc)
+        
         ddq = invJ @ acc
         self.q_cmd = pin.integrate(self.robot.model, q, invJ @ x_err)
         self.dq_cmd = invJ @ dx_ref
 
+        print("q_step")
+        print(invJ @ x_err)
+        print("dq_cmd:")
+        print(self.dq_cmd)
         """from IPython import embed
         embed()"""
 

src/InvKin.cpp

@@ -11,3 +11,126 @@ InvKin::InvKin(double dt_in) {
 
 InvKin::InvKin() {}
 
+Eigen::Matrix<double, 1, 3> InvKin::cross3(Eigen::Matrix<double, 1, 3> left, Eigen::Matrix<double, 1, 3> right) {
+    Eigen::Matrix<double, 1, 3> res;
+    res << left(0, 1) * right(0, 2) - left(0, 2) * right(0, 1),
+           left(0, 2) * right(0, 0) - left(0, 0) * right(0, 2),
+           left(0, 0) * right(0, 1) - left(0, 1) * right(0, 0);
+    return res;
+}
+
+template<typename _Matrix_Type_>
+_Matrix_Type_ pseudoInverse(const _Matrix_Type_ &a, double epsilon = std::numeric_limits<double>::epsilon())
+{
+	Eigen::JacobiSVD< _Matrix_Type_ > svd(a ,Eigen::ComputeThinU | Eigen::ComputeThinV);
+	double tolerance = epsilon * static_cast<double>(std::max(a.cols(), a.rows())) *svd.singularValues().array().abs()(0);
+	return svd.matrixV() *  (svd.singularValues().array().abs() > tolerance).select(svd.singularValues().array().inverse(), 0).matrix().asDiagonal() * svd.matrixU().adjoint();
+}
+
+Eigen::MatrixXd InvKin::refreshAndCompute(const Eigen::MatrixXd &x_cmd, const Eigen::MatrixXd &contacts,
+                                          const Eigen::MatrixXd &goals, const Eigen::MatrixXd &vgoals, const Eigen::MatrixXd &agoals,
+                                          const Eigen::MatrixXd &posf, const Eigen::MatrixXd &vf, const Eigen::MatrixXd &wf, const Eigen::MatrixXd &af,
+                                          const Eigen::MatrixXd &Jf, const Eigen::MatrixXd &posb, const Eigen::MatrixXd &rotb, const Eigen::MatrixXd &vb,
+                                          const Eigen::MatrixXd &ab, const Eigen::MatrixXd &Jb) {
+
+    // Update contact status of the feet
+    flag_in_contact.block(0, 0, 1, 4) = contacts.block(0, 0, 1, 4);
+
+    // Update position, velocity and acceleration references for the feet
+    for (int i = 0; i < 4; i++) {
+        feet_position_ref.block(i, 0, 1, 3) = goals.block(0, i, 3, 1).transpose();
+        feet_velocity_ref.block(i, 0, 1, 3) = vgoals.block(0, i, 3, 1).transpose();
+        feet_acceleration_ref.block(i, 0, 1, 3) = agoals.block(0, i, 3, 1).transpose();
+    }
+
+    // Update position and velocity reference for the base
+    base_position_ref = x_cmd.block(0, 0, 1, 3);
+    base_orientation_ref = pinocchio::rpy::rpyToMatrix(x_cmd(0, 3), x_cmd(0, 4), x_cmd(0, 5));
+    base_linearvelocity_ref = x_cmd.block(0, 6, 1, 3);
+    base_angularvelocity_ref = x_cmd.block(0, 9, 1, 3);
+
+    /* std::cout << base_position_ref << std::endl;
+    std::cout << base_orientation_ref << std::endl;
+    std::cout << base_linearvelocity_ref << std::endl;
+    std::cout << base_angularvelocity_ref << std::endl;
+    std::cout << "--" << std::endl; */
+
+    return computeInvKin(posf, vf, wf, af, Jf, posb, rotb, vb, ab, Jb);
+}
+
+Eigen::MatrixXd InvKin::computeInvKin(const Eigen::MatrixXd &posf, const Eigen::MatrixXd &vf, const Eigen::MatrixXd &wf, const Eigen::MatrixXd &af,
+                                      const Eigen::MatrixXd &Jf, const Eigen::MatrixXd &posb, const Eigen::MatrixXd &rotb, const Eigen::MatrixXd &vb, const Eigen::MatrixXd &ab,
+                                      const Eigen::MatrixXd &Jb) {
+
+    // Process feet
+    for (int i = 0; i < 4; i++) {
+
+        pfeet_err.row(i) = feet_position_ref.row(i) - posf.row(i);
+        vfeet_ref.row(i) = feet_velocity_ref.row(i);
+
+        afeet.row(i) = + Kp_flyingfeet * pfeet_err.row(i) - Kd_flyingfeet * (vf.row(i)-feet_velocity_ref.row(i)) + feet_acceleration_ref.row(i);
+        if (flag_in_contact(0, i)) {
+            afeet.row(i) *= 1.0; // Set to 0.0 to disable position/velocity control of feet in contact
+        }
+        afeet.row(i) -= af.row(i) + cross3(wf.row(i), vf.row(i)); // Drift
+    }
+    J.block(6, 0, 12, 18) = Jf.block(0, 0, 12, 18);
+
+    // Process base position
+    e_basispos = base_position_ref - posb;
+    abasis = Kp_base_position * e_basispos - Kd_base_position * (vb.block(0, 0, 1, 3) - base_linearvelocity_ref);
+    abasis -= ab.block(0, 0, 1, 3) + cross3(vb.block(0, 3, 1, 3), vb.block(0, 0, 1, 3)); // Drift
+    x_ref.block(0, 0, 3, 1) = base_position_ref;
+    x.block(0, 0, 3, 1) = posb;
+    dx_ref.block(0, 0, 3, 1) = base_linearvelocity_ref;
+    x_ref.block(0, 0, 3, 1) = vb.block(0, 0, 1, 3);
+
+    // Process base orientation
+    e_basisrot = -base_orientation_ref * pinocchio::log3(base_orientation_ref.transpose() * rotb);
+    awbasis = Kp_base_orientation * e_basisrot - Kd_base_orientation * (vb.block(0, 3, 1, 3) - base_angularvelocity_ref);
+    awbasis -= ab.block(0, 3, 1, 3);
+
+    x_ref.block(3, 0, 3, 1) = Eigen::Matrix<double, 1, 3>::Zero();
+    x.block(3, 0, 3, 1) = Eigen::Matrix<double, 1, 3>::Zero();
+    dx_ref.block(3, 0, 3, 1) = base_angularvelocity_ref;
+    x_ref.block(3, 0, 3, 1) = vb.block(0, 3, 1, 3);
+
+    J.block(0, 0, 6, 18) = Jb.block(0, 0, 6, 18); // Position and orientation
+
+    acc.block(0, 0, 1, 3) = abasis;
+    acc.block(0, 3, 1, 3) = awbasis;
+    for (int i = 0; i < 4; i++) {
+        acc.block(0, 6+3*i, 1, 3) = afeet.row(i);
+    }
+
+    x_err.block(0, 0, 1, 3) = e_basispos;
+    x_err.block(0, 3, 1, 3) = e_basisrot;
+    for (int i = 0; i < 4; i++) {
+        x_err.block(0, 6+3*i, 1, 3) = pfeet_err.row(i);
+    }
+
+    dx_r.block(0, 0, 1, 3) = base_linearvelocity_ref;
+    dx_r.block(0, 3, 1, 3) = base_angularvelocity_ref;
+    for (int i = 0; i < 4; i++) {
+        dx_r.block(0, 6+3*i, 1, 3) = vfeet_ref.row(i);
+    }
+
+    // std::cout << "J" << std::endl << J << std::endl;
+
+    invJ = pseudoInverse(J);
+    
+    // std::cout << "invJ" << std::endl << invJ << std::endl;
+    // std::cout << "acc" << std::endl << acc << std::endl;
+
+    ddq = invJ * acc.transpose();
+    q_step = invJ * x_err.transpose(); // Need to be pin.integrate in Python to get q_cmd
+    dq_cmd = invJ * dx_r.transpose();
+
+    // std::cout << "q_step" << std::endl << q_step << std::endl;
+    // std::cout << "dq_cmd" << std::endl << dq_cmd << std::endl;
+
+    return ddq;
+}
+
+Eigen::MatrixXd InvKin::get_q_step() { return q_step; }
+Eigen::MatrixXd InvKin::get_dq_cmd() { return dq_cmd; }
\ No newline at end of file