Switch Inverse Kinematics block to full c++ instead of a mix of python and c++

include/qrw/InvKin.hpp

@@ -3,6 +3,12 @@
 
 #include "pinocchio/math/rpy.hpp"
 #include "pinocchio/spatial/explog.hpp"
+#include "pinocchio/multibody/model.hpp"
+#include "pinocchio/multibody/data.hpp"
+#include "pinocchio/parsers/urdf.hpp"
+#include "pinocchio/algorithm/compute-all-terms.hpp"
+#include "pinocchio/algorithm/jacobian.hpp"
+#include "pinocchio/algorithm/frames.hpp"
 #include <Eigen/Core>
 #include <Eigen/Dense>
 #include <cmath>
@@ -10,6 +16,7 @@
 #include <iostream>
 #include <string>
 #include "qrw/Params.hpp"
+#include "qrw/Types.h"
 
 class InvKin
 {
@@ -17,41 +24,51 @@ public:
     InvKin();
     void initialize(Params& params);
 
-    Eigen::Matrix<double, 1, 3> cross3(Eigen::Matrix<double, 1, 3> left, Eigen::Matrix<double, 1, 3> right);
+    void refreshAndCompute(Matrix14 const& contacts, Matrix43 const& pgoals, Matrix43 const& vgoals, Matrix43 const& agoals);
+    
+    void run_InvKin(VectorN const& q, VectorN const& dq, MatrixN const& contacts, MatrixN const& pgoals, MatrixN const& vgoals, MatrixN const& agoals);
 
-    Eigen::MatrixXd refreshAndCompute(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);
-    Eigen::MatrixXd get_q_step();
-    Eigen::MatrixXd get_dq_cmd();
+    VectorN get_q_step() { return q_step_; }
+    VectorN get_q_cmd() { return q_cmd_; }
+    VectorN get_dq_cmd() { return dq_cmd_; }
+    VectorN get_ddq_cmd() { return ddq_cmd_; }
+    int get_foot_id(int i) { return foot_ids_[i];}
 
 private:
     // Inputs of the constructor
     Params* params_;
 
     // 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, 12, 12> invJ = Eigen::Matrix<double, 12, 12>::Zero();
-    Eigen::Matrix<double, 1, 12> acc = Eigen::Matrix<double, 1, 12>::Zero();
-    Eigen::Matrix<double, 1, 12> x_err = Eigen::Matrix<double, 1, 12>::Zero();
-    Eigen::Matrix<double, 1, 12> dx_r = Eigen::Matrix<double, 1, 12>::Zero();
+    Matrix12 invJ;
+    Matrix112 acc;
+    Matrix112 x_err;
+    Matrix112 dx_r;
+
+    Matrix43 pfeet_err;
+    Matrix43 vfeet_ref;
+    Matrix43 afeet;
+    Matrix13 e_basispos;
+    Matrix13 abasis;
+    Matrix13 e_basisrot;
+    Matrix13 awbasis;
+
+    int foot_ids_[4] = {0, 0, 0, 0};
+
+    Matrix43 posf_;
+    Matrix43 vf_;
+    Matrix43 wf_;
+    Matrix43 af_;
+    Matrix12 Jf_;
+    Eigen::Matrix<double, 6, 12> Jf_tmp_;
 
-    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();
+    Vector12 ddq_cmd_;
+    Vector12 dq_cmd_;
+    Vector12 q_cmd_;
+    Vector12 q_step_;
 
-    Eigen::MatrixXd ddq = Eigen::MatrixXd::Zero(12, 1);
-    Eigen::MatrixXd q_step = Eigen::MatrixXd::Zero(12, 1);
-    Eigen::MatrixXd dq_cmd = Eigen::MatrixXd::Zero(12, 1);
+    pinocchio::Model model_;  // Pinocchio model for frame computations and inverse kinematics
+    pinocchio::Data data_;  // Pinocchio datas for frame computations and inverse kinematics
 
 };
 

include/qrw/Types.h

@@ -24,7 +24,13 @@ using VectorN = Eigen::Matrix<double, Eigen::Dynamic, 1>;
 
 using Matrix3 = Eigen::Matrix<double, 3, 3>;
 using Matrix4 = Eigen::Matrix<double, 4, 4>;
+using Matrix12 = Eigen::Matrix<double, 12, 12>;
+
+using Matrix13 = Eigen::Matrix<double, 1, 3>;
+using Matrix14 = Eigen::Matrix<double, 1, 4>;
+using Matrix112 = Eigen::Matrix<double, 1, 12>;
 using Matrix34 = Eigen::Matrix<double, 3, 4>;
+using Matrix43 = Eigen::Matrix<double, 4, 3>;
 using Matrix64 = Eigen::Matrix<double, 6, 4>;
 using MatrixN4 = Eigen::Matrix<double, Eigen::Dynamic, 4>;
 using Matrix3N = Eigen::Matrix<double, 3, Eigen::Dynamic>;

python/gepadd.cpp

@@ -194,12 +194,14 @@ struct InvKinPythonVisitor : public bp::def_visitor<InvKinPythonVisitor<InvKin>>
             
             .def("initialize", &InvKin::initialize, bp::args("params"), "Initialize InvKin from Python.\n")
 
-            .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")
+            .def("get_q_step", &InvKin::get_q_step, "Get position step of inverse kinematics.\n")
+            .def("get_q_cmd", &InvKin::get_q_cmd, "Get position command.\n")
+            .def("get_dq_cmd", &InvKin::get_dq_cmd, "Get velocity command.\n")
+            .def("get_ddq_cmd", &InvKin::get_ddq_cmd, "Get acceleration command.\n")
+            .def("get_foot_id", &InvKin::get_foot_id, bp::args("i"), "Get food frame id.\n")
 
             // Run InvKin from Python
-            .def("refreshAndCompute", &InvKin::refreshAndCompute,
-                 bp::args("contacts", "goals", "vgoals", "agoals", "posf", "vf", "wf", "af", "Jf"),
+            .def("run_InvKin", &InvKin::run_InvKin, bp::args("contacts", "goals", "vgoals", "agoals"),
                  "Run InvKin from Python.\n");
     }
 

scripts/Controller.py

@@ -143,7 +143,7 @@ class Controller:
 
         # Define the default controller
         self.myController = wbc_controller(params)
-        self.myController.qdes[7:] = q_init.ravel()
+        self.myController.qdes[:] = q_init.ravel()
 
         self.envID = params.envID
         self.velID = params.velID
@@ -296,12 +296,12 @@ class Controller:
             self.q_wbc = np.zeros((19, 1))
             self.q_wbc[2, 0] = self.h_ref  # at position (0.0, 0.0, h_ref)
             self.q_wbc[6, 0] = 1.0  # with orientation (0.0, 0.0, 0.0)
-            self.q_wbc[7:, 0] = self.myController.qdes[7:]  # with reference angular positions of previous loop
+            self.q_wbc[7:, 0] = self.myController.qdes[:]  # with reference angular positions of previous loop
 
             # Get velocity in base frame for Pinocchio (not current base frame but desired base frame)
             self.b_v = self.v.copy()
             self.b_v[:6, 0] = self.v_ref[:6, 0]  # Base at reference velocity (TODO: add hRb once v_ref is considered in base frame)
-            self.b_v[6:, 0] = self.myController.vdes[6:, 0]  # with reference angular velocities of previous loop
+            self.b_v[6:, 0] = self.myController.vdes[:]  # with reference angular velocities of previous loop
 
             # Feet command position, velocity and acceleration in base frame
             self.feet_a_cmd = self.footTrajectoryGenerator.getFootAccelerationBaseFrame(oRh.transpose(), self.v_ref[3:6, 0:1])
@@ -318,8 +318,8 @@ class Controller:
             # Quantities sent to the control board
             self.result.P = 6.0 * np.ones(12)
             self.result.D = 0.3 * np.ones(12)
-            self.result.q_des[:] = self.myController.qdes[7:]
-            self.result.v_des[:] = self.myController.vdes[6:, 0]
+            self.result.q_des[:] = self.myController.qdes[:]
+            self.result.v_des[:] = self.myController.vdes[:]
             self.result.tau_ff[:] = 0.8 * self.myController.tau_ff
 
             # Display robot in Gepetto corba viewer

scripts/QP_WBC.py

@@ -22,7 +22,9 @@ class wbc_controller():
 
         self.dt = params.dt_wbc  # Time step
 
-        self.invKin = Solo12InvKin(params)  # Inverse Kinematics object
+        self.invKin = lrw.InvKin() # Inverse Kinematics solver in C++
+        self.invKin.initialize(params)
+
         self.box_qp = lrw.QPWBC()  # Box Quadratic Programming solver
         self.box_qp.initialize(params)
 
@@ -43,8 +45,8 @@ class wbc_controller():
         self.log_feet_acc_target = np.zeros((3, 4, params.N_SIMULATION))
 
         # Arrays to store results (for solo12)
-        self.qdes = np.zeros((19, ))
-        self.vdes = np.zeros((18, 1))
+        self.qdes = np.zeros(12)
+        self.vdes = np.zeros(12)
         self.tau_ff = np.zeros(12)
 
         # Indexes of feet frames in this order: [FL, FR, HL, HR]
@@ -69,13 +71,16 @@ class wbc_controller():
         self.tic = time()
 
         # Compute Inverse Kinematics
-        ddq_cmd = np.array([self.invKin.refreshAndCompute(q[7:, 0:1], dq[6:, 0:1], contacts, pgoals, vgoals, agoals)]).T
+        self.invKin.run_InvKin(q[7:, 0:1], dq[6:, 0:1], contacts, pgoals.transpose(), vgoals.transpose(), agoals.transpose())
+        ddq_cmd = np.zeros((18, 1))
+        ddq_cmd[6:, 0] = self.invKin.get_ddq_cmd()
 
-        for i in range(4):
+        # TODO: Adapt logging
+        """for i in range(4):
             self.log_feet_pos[:, i, self.k_log] = self.invKin.robot.data.oMf[self.indexes[i]].translation
             self.log_feet_err[:, i, self.k_log] = pgoals[:, i] - self.invKin.robot.data.oMf[self.indexes[i]].translation # self.invKin.pfeet_err[i]
             self.log_feet_vel[:, i, self.k_log] = pin.getFrameVelocity(self.invKin.robot.model, self.invKin.robot.data,
-                                                                       self.indexes[i], pin.LOCAL_WORLD_ALIGNED).linear
+                                                                       self.indexes[i], pin.LOCAL_WORLD_ALIGNED).linear"""
         self.feet_pos = self.log_feet_pos[:, :, self.k_log]
         self.feet_err = self.log_feet_err[:, :, self.k_log]
         self.feet_vel = self.log_feet_vel[:, :, self.k_log]
@@ -98,7 +103,7 @@ class wbc_controller():
         self.Jc = np.zeros((12, 18))
         for i_ee in range(4):
             if contacts[i_ee]:
-                idx = int(self.invKin.foot_ids[i_ee])
+                idx = self.invKin.get_foot_id(i_ee)
                 self.Jc[(3*i_ee):(3*(i_ee+1)), :] = pin.getFrameJacobian(self.robot.model, self.robot.data, idx, pin.LOCAL_WORLD_ALIGNED)[:3]
 
         # Compute joint torques according to the current state of the system and the desired joint accelerations
@@ -118,8 +123,8 @@ class wbc_controller():
         self.tau_ff[:] = RNEA_delta - ((self.Jc[:, 6:].transpose()) @ self.f_with_delta).ravel()
 
         # Retrieve desired positions and velocities
-        self.vdes[:, 0] = self.invKin.dq_cmd
-        self.qdes[:] = self.invKin.q_cmd
+        self.vdes[:] = self.invKin.get_dq_cmd()
+        self.qdes[:] = self.invKin.get_q_cmd()
 
         self.toc = time()
 

src/InvKin.cpp

 #include "qrw/InvKin.hpp"
 
-InvKin::InvKin() {}
+InvKin::InvKin()
+    : invJ(Matrix12::Zero())
+    , acc(Matrix112::Zero())
+    , x_err(Matrix112::Zero())
+    , dx_r(Matrix112::Zero())
+    , pfeet_err(Matrix43::Zero())
+    , vfeet_ref(Matrix43::Zero())
+    , afeet(Matrix43::Zero())
+    , e_basispos(Matrix13::Zero())
+    , abasis(Matrix13::Zero())
+    , e_basisrot(Matrix13::Zero())
+    , awbasis(Matrix13::Zero())
+    , posf_(Matrix43::Zero())
+    , vf_(Matrix43::Zero())
+    , wf_(Matrix43::Zero())
+    , af_(Matrix43::Zero())
+    , Jf_(Matrix12::Zero())
+    , Jf_tmp_(Eigen::Matrix<double, 6, 12>::Zero())
+    , ddq_cmd_(Vector12::Zero())
+    , dq_cmd_(Vector12::Zero())
+    , q_cmd_(Vector12::Zero())
+    , q_step_(Vector12::Zero())
+{}
 
 void InvKin::initialize(Params& params) {
 
     // Params store parameters
     params_ = &params;
 
-    // Starting reference position of feet
-    feet_position_ref = (Eigen::Map<Matrix34, Eigen::Unaligned>(params.footsteps_init.data())).transpose();
+    // Path to the robot URDF (TODO: Automatic path)
+    const std::string filename = std::string("/opt/openrobots/share/example-robot-data/robots/solo_description/robots/solo12.urdf");
 
-}
+    // Build model from urdf (base is not free flyer)
+    pinocchio::urdf::buildModel(filename, model_, false);
 
-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;
-}
+    // Construct data from model
+    data_ = pinocchio::Data(model_);
 
+    // Update all the quantities of the model
+    pinocchio::computeAllTerms(model_, data_ , VectorN::Zero(model_.nq), VectorN::Zero(model_.nv));
 
-Eigen::MatrixXd InvKin::refreshAndCompute(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) {
+    // Get feet frame IDs
+    foot_ids_[0] = static_cast<int>(model_.getFrameId("FL_FOOT")); // from long uint to int
+    foot_ids_[1] = static_cast<int>(model_.getFrameId("FR_FOOT"));
+    foot_ids_[2] = static_cast<int>(model_.getFrameId("HL_FOOT"));
+    foot_ids_[3] = static_cast<int>(model_.getFrameId("HR_FOOT"));
 
-    // 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();
-    }
+void InvKin::refreshAndCompute(Matrix14 const& contacts, Matrix43 const& pgoals,
+                                          Matrix43 const& vgoals, Matrix43 const& agoals) {
 
     // 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);
+        pfeet_err.row(i) = pgoals.row(i) - posf_.row(i);
+        vfeet_ref.row(i) = vgoals.row(i);
 
-        afeet.row(i) = + params_->Kp_flyingfeet * pfeet_err.row(i) - params_->Kd_flyingfeet * (vf.row(i)-feet_velocity_ref.row(i)) + feet_acceleration_ref.row(i);
-        if (flag_in_contact(0, i)) {
+        afeet.row(i) = + params_->Kp_flyingfeet * pfeet_err.row(i) - params_->Kd_flyingfeet * (vf_.row(i)-vgoals.row(i)) + agoals.row(i);
+        if (contacts(0, i)) {
             afeet.row(i) *= 0.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
+        afeet.row(i) -= af_.row(i) + cross3(wf_.row(i), vf_.row(i)); // Drift
     }
 
     // Store data and invert the Jacobian
@@ -54,13 +70,13 @@ Eigen::MatrixXd InvKin::refreshAndCompute(const Eigen::MatrixXd &contacts,
         acc.block(0, 3*i, 1, 3) = afeet.row(i);
         x_err.block(0, 3*i, 1, 3) = pfeet_err.row(i);
         dx_r.block(0, 3*i, 1, 3) = vfeet_ref.row(i);
-        invJ.block(3*i, 3*i, 3, 3) = Jf.block(3*i, 3*i, 3, 3).inverse();
+        invJ.block(3*i, 3*i, 3, 3) = Jf_.block(3*i, 3*i, 3, 3).inverse();
     }
 
     // Once Jacobian has been inverted we can get command accelerations, velocities and positions
-    ddq = invJ * acc.transpose();
-    dq_cmd = invJ * dx_r.transpose();
-    q_step = invJ * x_err.transpose(); // Not a position but a step in position
+    ddq_cmd_ = invJ * acc.transpose();
+    dq_cmd_ = invJ * dx_r.transpose();
+    q_step_ = invJ * x_err.transpose(); // Not a position but a step in position
 
     /*
     std::cout << "J" << std::endl << Jf << std::endl;
@@ -69,9 +85,32 @@ Eigen::MatrixXd InvKin::refreshAndCompute(const Eigen::MatrixXd &contacts,
     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
+void InvKin::run_InvKin(VectorN const& q, VectorN const& dq, MatrixN const& contacts, MatrixN const& pgoals, MatrixN const& vgoals, MatrixN const& agoals)
+{
+    // Update model and data of the robot
+    pinocchio::computeJointJacobians(model_, data_, q);
+    pinocchio::forwardKinematics(model_, data_, q, dq, VectorN::Zero(model_.nv));
+    pinocchio::updateFramePlacements(model_, data_);
+
+    // Get data required by IK with Pinocchio
+    for (int i = 0; i < 4; i++) {
+        int idx = foot_ids_[i];
+        posf_.row(i) = data_.oMf[idx].translation();
+        pinocchio::Motion nu = pinocchio::getFrameVelocity(model_, data_, idx, pinocchio::LOCAL_WORLD_ALIGNED);
+        vf_.row(i) = nu.linear();
+        wf_.row(i) = nu.angular();
+        af_.row(i) = pinocchio::getFrameAcceleration(model_, data_, idx, pinocchio::LOCAL_WORLD_ALIGNED).linear();
+        pinocchio::getFrameJacobian(model_, data_, idx, pinocchio::LOCAL_WORLD_ALIGNED, Jf_tmp_);
+        Jf_.block(3 * i, 0, 3, 12) = Jf_tmp_.block(0, 0, 3, 12);
+    }
+
+    // IK output for accelerations of actuators (stored in ddq_cmd_)
+    // IK output for velocities of actuators (stored in dq_cmd_)
+    refreshAndCompute(contacts, pgoals, vgoals, agoals);
+
+    // IK output for positions of actuators
+    q_cmd_ = q + q_step_;
+
+}