From ace49f32e92e15da10082153723b4f721a5d5953 Mon Sep 17 00:00:00 2001
From: paleziart <paleziart@laas.fr>
Date: Mon, 26 Jul 2021 12:37:55 +0200
Subject: [PATCH] Finish switching to full C++ for the whole-body control part
+ Error is no longer an attribute of the wbc class
---
include/qrw/QPWBC.hpp | 84 ++++++++++++++++++++++
python/gepadd.cpp | 42 +++++++++++
scripts/Controller.py | 52 +++++++-------
scripts/LoggerControl.py | 6 +-
scripts/main_solo12_control.py | 4 +-
src/QPWBC.cpp | 127 +++++++++++++++++++++++++++++++++
6 files changed, 283 insertions(+), 32 deletions(-)
diff --git a/include/qrw/QPWBC.hpp b/include/qrw/QPWBC.hpp
index 602a8fcb..cdd57f21 100644
--- a/include/qrw/QPWBC.hpp
+++ b/include/qrw/QPWBC.hpp
@@ -14,6 +14,16 @@
#include "osqp.h"
#include "other/st_to_cc.hpp"
+// For wrapper
+#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 "pinocchio/algorithm/rnea.hpp"
+#include "pinocchio/algorithm/crba.hpp"
+
class QPWBC {
private:
@@ -101,4 +111,78 @@ class QPWBC {
};
+class WbcWrapper
+{
+public:
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Empty constructor
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ WbcWrapper();
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Destructor.
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ~WbcWrapper() {}
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Initializer
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void initialize(Params& params);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Compute the remaining and total duration of a swing phase or a stance phase based
+ /// on the content of the gait matrix
+ ///
+ /// \param[in] i considered phase (row of the gait matrix)
+ /// \param[in] j considered foot (col of the gait matrix)
+ /// \param[in] value 0.0 for swing phase detection, 1.0 for stance phase detection
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void compute(VectorN const& q, VectorN const& dq, MatrixN const& f_cmd, MatrixN const& contacts,
+ MatrixN const& pgoals, MatrixN const& vgoals, MatrixN const& agoals);
+
+ VectorN get_qdes() { return qdes_; }
+ VectorN get_vdes() { return vdes_; }
+ VectorN get_tau_ff() { return tau_ff_; }
+ VectorN get_f_with_delta() { box_qp_->get_f_res(); }
+ MatrixN get_feet_pos() { return MatrixN::Zero(4, 3); }
+ MatrixN get_feet_err() { return MatrixN::Zero(4, 3); }
+ MatrixN get_feet_vel() { return MatrixN::Zero(4, 3); }
+ MatrixN get_feet_pos_target() { return MatrixN::Zero(4, 3); }
+ MatrixN get_feet_vel_target() { return MatrixN::Zero(4, 3); }
+ MatrixN get_feet_acc_target() { return MatrixN::Zero(4, 3); }
+
+private:
+
+ Params* params_; // Object that stores parameters
+ QPWBC* box_qp_; // QP problem solver for the whole body control
+ InvKin* invkin_; // Inverse Kinematics solver for the whole body control
+
+ pinocchio::Model model_; // Pinocchio model for frame computations
+ pinocchio::Data data_; // Pinocchio datas for frame computations
+
+ Eigen::Matrix<double, 18, 18> M_; // Mass matrix
+ Eigen::Matrix<double, 12, 18> Jc_; // Jacobian matrix
+ Eigen::Matrix<double, 6, 18> Jc_tmp_; // Temporary matrix to store result of getFrameJacobian
+ Eigen::Matrix<double, 1, 4> k_since_contact_;
+ Vector12 qdes_; // Desired actuator positions
+ Vector12 vdes_; // Desired actuator velocities
+ Vector12 tau_ff_; // Desired actuator torques (feedforward)
+
+ Vector18 ddq_cmd_; // Actuator accelerations computed by Inverse Kinematics
+ Vector19 q_default_; // Default configuration vector to compute the mass matrix
+ Vector12 f_with_delta_; // Contact forces with deltas found by QP solver
+ Vector18 ddq_with_delta_; // Actuator accelerations with deltas found by QP solver
+
+ int k_log_; // Counter for logging purpose
+ int indexes_[4] = {10, 18, 26, 34}; // Indexes of feet frames in this order: [FL, FR, HL, HR]
+};
+
#endif // QPWBC_H_INCLUDED
diff --git a/python/gepadd.cpp b/python/gepadd.cpp
index b84ed8a0..7f302d41 100644
--- a/python/gepadd.cpp
+++ b/python/gepadd.cpp
@@ -245,6 +245,47 @@ struct QPWBCPythonVisitor : public bp::def_visitor<QPWBCPythonVisitor<QPWBC>>
};
void exposeQPWBC() { QPWBCPythonVisitor<QPWBC>::expose(); }
+/////////////////////////////////
+/// Binding WbcWrapper class
+/////////////////////////////////
+template <typename WbcWrapper>
+struct WbcWrapperPythonVisitor : public bp::def_visitor<WbcWrapperPythonVisitor<WbcWrapper>>
+{
+ template <class PyClassWbcWrapper>
+ void visit(PyClassWbcWrapper& cl) const
+ {
+ cl.def(bp::init<>(bp::arg(""), "Default constructor."))
+
+ .def("initialize", &WbcWrapper::initialize, bp::args("params"), "Initialize WbcWrapper from Python.\n")
+
+ .def("get_qdes", &WbcWrapper::get_qdes, "Get qdes_.\n")
+ .def("get_vdes", &WbcWrapper::get_vdes, "Get vdes_.\n")
+ .def("get_tau_ff", &WbcWrapper::get_tau_ff, "Get tau_ff_.\n")
+
+ .def_readonly("qdes", &WbcWrapper::get_qdes)
+ .def_readonly("vdes", &WbcWrapper::get_vdes)
+ .def_readonly("tau_ff", &WbcWrapper::get_tau_ff)
+ .def_readonly("f_with_delta", &WbcWrapper::get_f_with_delta)
+ .def_readonly("feet_pos", &WbcWrapper::get_feet_pos)
+ .def_readonly("feet_err", &WbcWrapper::get_feet_err)
+ .def_readonly("feet_vel", &WbcWrapper::get_feet_vel)
+ .def_readonly("feet_pos_target", &WbcWrapper::get_feet_pos_target)
+ .def_readonly("feet_vel_target", &WbcWrapper::get_feet_vel_target)
+ .def_readonly("feet_acc_target", &WbcWrapper::get_feet_acc_target)
+
+ // Run WbcWrapper from Python
+ .def("compute", &WbcWrapper::compute, bp::args("q", "dq", "f_cmd", "contacts", "pgoals", "vgoals", "agoals"), "Run WbcWrapper from Python.\n");
+ }
+
+ static void expose()
+ {
+ bp::class_<WbcWrapper>("WbcWrapper", bp::no_init).def(WbcWrapperPythonVisitor<WbcWrapper>());
+
+ ENABLE_SPECIFIC_MATRIX_TYPE(matXd);
+ }
+};
+void exposeWbcWrapper() { WbcWrapperPythonVisitor<WbcWrapper>::expose(); }
+
/////////////////////////////////
/// Binding Estimator class
/////////////////////////////////
@@ -364,6 +405,7 @@ BOOST_PYTHON_MODULE(libquadruped_reactive_walking)
exposeFootTrajectoryGenerator();
exposeInvKin();
exposeQPWBC();
+ exposeWbcWrapper();
exposeEstimator();
exposeParams();
}
\ No newline at end of file
diff --git a/scripts/Controller.py b/scripts/Controller.py
index be42e404..05f1e950 100644
--- a/scripts/Controller.py
+++ b/scripts/Controller.py
@@ -134,6 +134,9 @@ class Controller:
self.estimator = lqrw.Estimator()
self.estimator.initialize(params)
+ self.wbcWrapper = lqrw.WbcWrapper()
+ self.wbcWrapper.initialize(params)
+
# Wrapper that makes the link with the solver that you want to use for the MPC
self.mpc_wrapper = MPC_Wrapper.MPC_Wrapper(params, self.q)
@@ -141,10 +144,6 @@ class Controller:
# import ForceMonitor
# myForceMonitor = ForceMonitor.ForceMonitor(pyb_sim.robotId, pyb_sim.planeId)
- # Define the default controller
- self.myController = wbc_controller(params)
- self.myController.qdes[:] = q_init.ravel()
-
self.envID = params.envID
self.velID = params.velID
self.dt_wbc = params.dt_wbc
@@ -173,6 +172,7 @@ class Controller:
self.feet_v_cmd = np.zeros((3, 4))
self.feet_p_cmd = np.zeros((3, 4))
+ self.error = False # True if something wrong happens in the controller
self.error_flag = 0
self.q_security = np.array([np.pi*0.4, np.pi*80/180, np.pi] * 4)
@@ -278,12 +278,12 @@ class Controller:
# Target state for the whole body control
self.x_f_wbc = (self.x_f_mpc[:24, 0]).copy()
if not self.gait.getIsStatic():
- self.x_f_wbc[0] = self.myController.dt * xref[6, 1]
- self.x_f_wbc[1] = self.myController.dt * xref[7, 1]
+ self.x_f_wbc[0] = self.dt_wbc * xref[6, 1]
+ self.x_f_wbc[1] = self.dt_wbc * xref[7, 1]
self.x_f_wbc[2] = self.h_ref
self.x_f_wbc[3] = 0.0
self.x_f_wbc[4] = 0.0
- self.x_f_wbc[5] = self.myController.dt * xref[11, 1]
+ self.x_f_wbc[5] = self.dt_wbc * xref[11, 1]
else: # Sort of position control to avoid slow drift
self.x_f_wbc[0:3] = self.planner.q_static[0:3, 0] # TODO: Adapt to new code
self.x_f_wbc[3:6] = self.planner.RPY_static[:, 0]
@@ -291,17 +291,17 @@ class Controller:
# Whole Body Control
# If nothing wrong happened yet in the WBC controller
- if (not self.myController.error) and (not self.joystick.stop):
+ if (not self.error) and (not self.joystick.stop):
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[:] # with reference angular positions of previous loop
+ self.q_wbc[7:, 0] = self.wbcWrapper.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[:] # with reference angular velocities of previous loop
+ self.b_v[6:, 0] = self.wbcWrapper.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])
@@ -309,18 +309,18 @@ class Controller:
self.feet_p_cmd = self.footTrajectoryGenerator.getFootPositionBaseFrame(oRh.transpose(), np.array([[0.0], [0.0], [self.h_ref]]) + oTh)
# Run InvKin + WBC QP
- self.myController.compute(self.q_wbc, self.b_v,
- self.x_f_wbc[12:], cgait[0, :],
- self.feet_p_cmd,
- self.feet_v_cmd,
- self.feet_a_cmd)
+ self.wbcWrapper.compute(self.q_wbc, self.b_v,
+ self.x_f_wbc[12:], np.array([cgait[0, :]]),
+ self.feet_p_cmd,
+ self.feet_v_cmd,
+ self.feet_a_cmd)
# 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[:]
- self.result.v_des[:] = self.myController.vdes[:]
- self.result.tau_ff[:] = 0.8 * self.myController.tau_ff
+ self.result.q_des[:] = self.wbcWrapper.qdes[:]
+ self.result.v_des[:] = self.wbcWrapper.vdes[:]
+ self.result.tau_ff[:] = 0.8 * self.wbcWrapper.tau_ff
# Display robot in Gepetto corba viewer
"""if self.k % 5 == 0:
@@ -353,19 +353,19 @@ class Controller:
def security_check(self):
- if (self.error_flag == 0) and (not self.myController.error) and (not self.joystick.stop):
- self.error_flag = self.estimator.security_check(self.myController.tau_ff)
+ if (self.error_flag == 0) and (not self.error) and (not self.joystick.stop):
+ self.error_flag = self.estimator.security_check(self.wbcWrapper.tau_ff)
if (self.error_flag != 0):
- self.myController.error = True
+ self.error = True
if (self.error_flag == 1):
self.error_value = self.estimator.getQFilt()[7:] * 180 / 3.1415
elif (self.error_flag == 2):
self.error_value = self.estimator.getVSecu()
else:
- self.error_value = self.myController.tau_ff
+ self.error_value = self.wbcWrapper.tau_ff
# If something wrong happened in the controller we stick to a security controller
- if self.myController.error or self.joystick.stop:
+ if self.error or self.joystick.stop:
# Quantities sent to the control board
self.result.P = np.zeros(12)
@@ -381,8 +381,6 @@ class Controller:
self.t_list_mpc[self.k] = t_mpc - t_planner
self.t_list_wbc[self.k] = t_wbc - t_mpc
self.t_list_loop[self.k] = time.time() - tic
- self.t_list_InvKin[self.k] = self.myController.tac - self.myController.tic
- self.t_list_QPWBC[self.k] = self.myController.toc - self.myController.tac
def updateState(self):
@@ -397,13 +395,13 @@ class Controller:
Ryaw = np.array([[math.cos(self.yaw_estim), -math.sin(self.yaw_estim)],
[math.sin(self.yaw_estim), math.cos(self.yaw_estim)]])
- self.q[0:2, 0:1] = self.q[0:2, 0:1] + Ryaw @ self.v_ref[0:2, 0:1] * self.myController.dt
+ self.q[0:2, 0:1] = self.q[0:2, 0:1] + Ryaw @ self.v_ref[0:2, 0:1] * self.dt_wbc
# Mix perfect x and y with height measurement
self.q[2, 0] = self.estimator.getQFilt()[2]
# Mix perfect yaw with pitch and roll measurements
- self.yaw_estim += self.v_ref[5, 0] * self.myController.dt
+ self.yaw_estim += self.v_ref[5, 0] * self.dt_wbc
self.q[3:7, 0] = pin.Quaternion(pin.rpy.rpyToMatrix(self.estimator.getRPY()[0], self.estimator.getRPY()[1], self.yaw_estim)).coeffs()
# Actuators measurements
diff --git a/scripts/LoggerControl.py b/scripts/LoggerControl.py
index 70962a24..4023d3bc 100644
--- a/scripts/LoggerControl.py
+++ b/scripts/LoggerControl.py
@@ -160,11 +160,11 @@ class LoggerControl():
self.wbc_tau_ff[self.i] = loop.result.tau_ff
self.wbc_f_ctc[self.i] = wbc.f_with_delta[:, 0]
self.wbc_feet_pos[self.i] = wbc.feet_pos
- self.wbc_feet_pos_target[self.i] = wbc.log_feet_pos_target[:, :, self.i+1]
+ self.wbc_feet_pos_target[self.i] = wbc.feet_pos_target
self.wbc_feet_err[self.i] = wbc.feet_err
self.wbc_feet_vel[self.i] = wbc.feet_vel
- self.wbc_feet_vel_target[self.i] = wbc.log_feet_vel_target[:, :, self.i+1]
- self.wbc_feet_acc_target[self.i] = wbc.log_feet_acc_target[:, :, self.i+1]
+ self.wbc_feet_vel_target[self.i] = wbc.feet_vel_target
+ self.wbc_feet_acc_target[self.i] = wbc.feet_acc_target
#self.wbc_feet_pos_invkin[self.i] = wbc.invKin.cpp_posf.transpose() # TODO: Adapt logging
#self.wbc_feet_vel_invkin[self.i] = wbc.invKin.cpp_vf.transpose()
diff --git a/scripts/main_solo12_control.py b/scripts/main_solo12_control.py
index 07d0be99..e22bb582 100644
--- a/scripts/main_solo12_control.py
+++ b/scripts/main_solo12_control.py
@@ -172,7 +172,7 @@ def control_loop(name_interface, name_interface_clone=None, des_vel_analysis=Non
t = 0.0
t_max = (params.N_SIMULATION-2) * params.dt_wbc
- while ((not device.hardware.IsTimeout()) and (t < t_max) and (not controller.myController.error)):
+ while ((not device.hardware.IsTimeout()) and (t < t_max) and (not controller.error)):
# Update sensor data (IMU, encoders, Motion capture)
device.UpdateMeasurment()
@@ -316,7 +316,7 @@ def control_loop(name_interface, name_interface_clone=None, des_vel_analysis=Non
# Disconnect the PyBullet server (also close the GUI)
device.Stop()
- if controller.myController.error:
+ if controller.error:
if (controller.error_flag == 1):
print("-- POSITION LIMIT ERROR --")
elif (controller.error_flag == 2):
diff --git a/src/QPWBC.cpp b/src/QPWBC.cpp
index 4d1c442e..f8338c3f 100644
--- a/src/QPWBC.cpp
+++ b/src/QPWBC.cpp
@@ -547,3 +547,130 @@ void QPWBC::update_PQ() {
my_print_csc_matrix(P, t_char);*/
}
+
+
+
+WbcWrapper::WbcWrapper()
+ : M_(Eigen::Matrix<double, 18, 18>::Zero())
+ , Jc_(Eigen::Matrix<double, 12, 18>::Zero())
+ , Jc_tmp_(Eigen::Matrix<double, 6, 18>::Zero())
+ , k_since_contact_(Eigen::Matrix<double, 1, 4>::Zero())
+ , qdes_(Vector12::Zero())
+ , vdes_(Vector12::Zero())
+ , tau_ff_(Vector12::Zero())
+ , ddq_cmd_(Vector18::Zero())
+ , q_default_(Vector19::Zero())
+ , f_with_delta_(Vector12::Zero())
+ , ddq_with_delta_(Vector18::Zero())
+ , k_log_(0)
+{}
+
+void WbcWrapper::initialize(Params& params)
+{
+ // Params store parameters
+ params_ = ¶ms;
+
+ // 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, pinocchio::JointModelFreeFlyer(), model_, false);
+
+ // 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));
+
+ // Initialize inverse kinematic and box QP solvers
+ invkin_ = new InvKin();
+ invkin_->initialize(params);
+ box_qp_ = new QPWBC();
+ box_qp_->initialize(params);
+
+ // Initialize quaternion
+ q_default_(6, 0) = 1.0;
+}
+
+void WbcWrapper::compute(VectorN const& q, VectorN const& dq, MatrixN const& f_cmd, MatrixN const& contacts,
+ MatrixN const& pgoals, MatrixN const& vgoals, MatrixN const& agoals)
+{
+ // Update nb of iterations since contact
+ k_since_contact_ += contacts; // Increment feet in stance phase
+ k_since_contact_ = k_since_contact_.cwiseProduct(contacts); // Reset feet in swing phase
+
+ // Compute Inverse Kinematics
+ invkin_->run_InvKin(q.tail(12), dq.tail(12), contacts, pgoals.transpose(), vgoals.transpose(), agoals.transpose());
+ ddq_cmd_.tail(12) = invkin_->get_ddq_cmd();
+
+ // TODO: Adapt logging of feet_pos, feet_err, feet_vel
+
+ // Compute the upper triangular part of the joint space inertia matrix M by using the Composite Rigid Body Algorithm
+ // Result is stored in data_.M
+ pinocchio::crba(model_, data_, q_default_);
+
+ // Make mass matrix symetric
+ data_.M.triangularView<Eigen::StrictlyLower>() = data_.M.transpose().triangularView<Eigen::StrictlyLower>();
+
+ // TODO: Check if needed because crbaMinimal may allow to directly get the jacobian
+ // TODO: Check if possible to use the model of InvKin to avoid computations
+ pinocchio::computeJointJacobians(model_, data_, q);
+
+ // TODO: Check if we can save time by switching MatrixXd to defined sized vector since they are
+ // not called from python anymore
+
+ // Retrieve feet jacobian
+ Jc_.setZero();
+ for (int i = 0; i < 4; i++)
+ {
+ if (contacts(0, i))
+ {
+ Jc_tmp_.setZero();
+ pinocchio::getFrameJacobian(model_, data_, indexes_[i], pinocchio::LOCAL_WORLD_ALIGNED, Jc_tmp_);
+ Jc_.block(3 * i, 0, 3, 18) = Jc_tmp_.block(0, 0, 3, 18);
+ }
+ }
+
+ // Compute the inverse dynamics, aka the joint torques according to the current state of the system,
+ // the desired joint accelerations and the external forces, using the Recursive Newton Euler Algorithm.
+ // Result is stored in data_.tau
+ pinocchio::rnea(model_, data_, q, dq, ddq_cmd_);
+
+ /*std::cout << "M" << std::endl;
+ std::cout << data_.M << std::endl;
+ std::cout << "Jc" << std::endl;
+ std::cout << Jc_ << std::endl;
+ std::cout << "f_cmd" << std::endl;
+ std::cout << f_cmd << std::endl;
+ std::cout << "rnea" << std::endl;
+ std::cout << data_.tau.head(6) << std::endl;
+ std::cout << "k_since" << std::endl;
+ std::cout << k_since_contact_ << std::endl;*/
+
+ // Solve the QP problem
+ box_qp_->run(data_.M, Jc_, Eigen::Map<const VectorN>(f_cmd.data(), f_cmd.size()), data_.tau.head(6), k_since_contact_);
+
+ // Add to reference quantities the deltas found by the QP solver
+ f_with_delta_ = box_qp_->get_f_res();
+ ddq_with_delta_.head(6) = ddq_cmd_.head(6) + box_qp_->get_ddq_res();
+ ddq_with_delta_.tail(12) = ddq_cmd_.tail(12);
+
+ // Compute joint torques from contact forces and desired accelerations
+ pinocchio::rnea(model_, data_, q, dq, ddq_with_delta_);
+
+ /*std::cout << "rnea delta" << std::endl;
+ std::cout << data_.tau.tail(12) << std::endl;
+ std::cout << "ddq del" << std::endl;
+ std::cout << ddq_with_delta_ << std::endl;
+ std::cout << "f del" << std::endl;
+ std::cout << f_with_delta_ << std::endl;*/
+
+ tau_ff_ = data_.tau.tail(12) - Jc_.block(0, 6, 12, 12).transpose() * f_with_delta_;
+
+ // Retrieve desired positions and velocities
+ vdes_ = invkin_->get_dq_cmd();
+ qdes_ = invkin_->get_q_cmd();
+
+ // Increment log counter
+ k_log_++;
+}
--
GitLab