From 8db10bb454ce987be9b29a17f60e60378ed7a869 Mon Sep 17 00:00:00 2001
From: odri <odri@furano.laas.fr>
Date: Fri, 16 Jul 2021 12:40:42 +0200
Subject: [PATCH] Switch Inverse Kinematics block to full c++ instead of a mix
of python and c++
---
include/qrw/InvKin.hpp | 67 +++++++++++++++----------
include/qrw/Types.h | 6 +++
python/gepadd.cpp | 10 ++--
scripts/Controller.py | 10 ++--
scripts/QP_WBC.py | 23 +++++----
src/InvKin.cpp | 111 ++++++++++++++++++++++++++++-------------
6 files changed, 148 insertions(+), 79 deletions(-)
diff --git a/include/qrw/InvKin.hpp b/include/qrw/InvKin.hpp
index 83619203..f3445c6b 100644
--- a/include/qrw/InvKin.hpp
+++ b/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
};
diff --git a/include/qrw/Types.h b/include/qrw/Types.h
index 233c80df..b712677d 100644
--- a/include/qrw/Types.h
+++ b/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>;
diff --git a/python/gepadd.cpp b/python/gepadd.cpp
index 4db42fb7..1f2cbe3f 100644
--- a/python/gepadd.cpp
+++ b/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");
}
diff --git a/scripts/Controller.py b/scripts/Controller.py
index 9f131287..ec48c16f 100644
--- a/scripts/Controller.py
+++ b/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
diff --git a/scripts/QP_WBC.py b/scripts/QP_WBC.py
index 4905b848..7ba23e2f 100644
--- a/scripts/QP_WBC.py
+++ b/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()
diff --git a/src/InvKin.cpp b/src/InvKin.cpp
index 2cf65a1f..4122c8c5 100644
--- a/src/InvKin.cpp
+++ b/src/InvKin.cpp
@@ -1,52 +1,68 @@
#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_ = ¶ms;
- // 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_;
+
+}
--
GitLab