From 3b250682a5727ccbddeb89e92025df705c62c54e Mon Sep 17 00:00:00 2001
From: odri <odri@furano.laas.fr>
Date: Fri, 22 Oct 2021 18:04:03 +0200
Subject: [PATCH] Debugging WBC
---
scripts/Controller.py | 4 ++--
src/InvKin.cpp | 6 +++---
src/QPWBC.cpp | 29 +++++++++++++++++++++++++----
src/walk_parameters.yaml | 16 ++++++++--------
4 files changed, 38 insertions(+), 17 deletions(-)
diff --git a/scripts/Controller.py b/scripts/Controller.py
index 4fd25211..6ec5ac05 100644
--- a/scripts/Controller.py
+++ b/scripts/Controller.py
@@ -323,13 +323,13 @@ class Controller:
# Retrieve reference contact forces in horizontal frame
self.x_f_mpc = self.mpc_wrapper.get_latest_result()
- """if self.k >= 8220 and (self.k % self.k_mpc == 0):
+ if self.k >= 8220 and (self.k % self.k_mpc == 0):
print(self.k)
print(self.x_f_mpc[:, 0])
from matplotlib import pyplot as plt
plt.figure()
plt.plot(self.x_f_mpc[6, :])
- plt.show(block=True)"""
+ plt.show(block=True)
# Store o_targetFootstep, used with MPC_planner
self.o_targetFootstep = o_targetFootstep.copy()
diff --git a/src/InvKin.cpp b/src/InvKin.cpp
index 485b6373..bd8263b4 100644
--- a/src/InvKin.cpp
+++ b/src/InvKin.cpp
@@ -77,10 +77,10 @@ void InvKin::initialize(Params& params) {
void InvKin::refreshAndCompute(Matrix14 const& contacts, Matrix43 const& pgoals, Matrix43 const& vgoals,
Matrix43 const& agoals) {
- /*std::cout << std::fixed;
- std::cout << std::setprecision(2);
+ std::cout << std::fixed;
+ std::cout << std::setprecision(5);
- std::cout << "pgoals:" << std::endl;
+ /*std::cout << "pgoals:" << std::endl;
std::cout << pgoals << std::endl;
std::cout << "posf_" << std::endl;
std::cout << posf_ << std::endl;*/
diff --git a/src/QPWBC.cpp b/src/QPWBC.cpp
index 3a715478..50a6b225 100644
--- a/src/QPWBC.cpp
+++ b/src/QPWBC.cpp
@@ -736,6 +736,16 @@ void WbcWrapper::compute(VectorN const &q, VectorN const &dq, VectorN const &f_c
// std::cout << "agoals " << std::endl << agoals << std::endl;
// std::cout << "ddq_cmd_bis " << std::endl << ddq_cmd_.transpose() << std::endl;
+ // std::cout << "M : " << std::endl << data_.M.block(0, 0, 3, 18) << std::endl;
+ // std::cout << "ddq: " << std::endl << ddq_cmd_.transpose() << std::endl;
+
+ std::cout << "-- BEFORE QP PROBLEM --" << std::endl;
+ std::cout << "M ddq_u: " << std::endl << (data_.M.block(0, 0, 3, 6) * ddq_cmd_.head(6)).transpose() << std::endl;
+ std::cout << "M ddq_a: " << std::endl << (data_.M.block(0, 6, 3, 12) * ddq_cmd_.tail(12)).transpose() << std::endl;
+ pinocchio::rnea(model_, data_, q_wbc_, dq_wbc_, VectorN::Zero(model_.nv));
+ std::cout << "Non linear effects: " << std::endl << data_.tau.head(6).transpose() << std::endl;
+ std::cout << "JcT f_cmd: " << std::endl << (Jc_.transpose() * (f_cmd + f_compensation)).transpose() << std::endl;
+
// Solve the QP problem
box_qp_->run(data_.M, Jc_, ddq_cmd_, f_cmd + f_compensation, data_.tau.head(6), k_since_contact_);
@@ -744,19 +754,19 @@ void WbcWrapper::compute(VectorN const &q, VectorN const &dq, VectorN const &f_c
ddq_with_delta_.head(6) = ddq_cmd_.head(6) + box_qp_->get_ddq_res();
ddq_with_delta_.tail(12) = ddq_cmd_.tail(12);
- /* DEBUG INERTIA AND NON LINEAR EFFECTS
+ // DEBUG INERTIA AND NON LINEAR EFFECTS
Vector6 left = data_.M.block(0, 0, 6, 6) * box_qp_->get_ddq_res() - Jc_.transpose() * box_qp_->get_f_res();
Vector6 right = - data_.tau.head(6) + Jc_.transpose() * (f_cmd + f_compensation);
Vector6 tmp_RNEA = data_.tau.head(6);
//std::cout << "RNEA: " << std::endl << data_.tau.head(6).transpose() << std::endl;
- //std::cout << "left: " << std::endl << left.transpose() << std::endl;
- //std::cout << "right: " << std::endl << right.transpose() << std::endl;
+ std::cout << "left: " << std::endl << left.transpose() << std::endl;
+ std::cout << "right: " << std::endl << right.transpose() << std::endl;
//std::cout << "M: " << std::endl << data_.M.block(0, 0, 6, 6) << std::endl;
//std::cout << "JcT: " << std::endl << Jc_.transpose() << std::endl;
//std::cout << "M: " << std::endl << data_.M.block(0, 0, 3, 18) << std::endl;
-
+ /*
pinocchio::rnea(model_, data_, q_wbc_, dq_wbc_, VectorN::Zero(model_.nv));
Vector6 tmp_NLE = data_.tau.head(6);
@@ -824,6 +834,17 @@ void WbcWrapper::compute(VectorN const &q, VectorN const &dq, VectorN const &f_c
pinocchio::rnea(model_, data_, q_wbc_, dq_wbc_, ddq_test);
std::cout << "M DDQ Delta Bis: " << std::endl << (data_.tau.head(6) - tmp_NLE).transpose() << std::endl;*/
+
+ std::cout << "-- AFTER QP PROBLEM --" << std::endl;
+ std::cout << "M ddq_u: " << std::endl << (data_.M.block(0, 0, 3, 6) * ddq_with_delta_.head(6)).transpose() << std::endl;
+ std::cout << "M ddq_a: " << std::endl << (data_.M.block(0, 6, 3, 12) * ddq_with_delta_.tail(12)).transpose() << std::endl;
+ pinocchio::rnea(model_, data_, q_wbc_, dq_wbc_, VectorN::Zero(model_.nv));
+ std::cout << "Non linear effects: " << std::endl << data_.tau.head(6).transpose() << std::endl;
+ std::cout << "JcT f_cmd: " << std::endl << (Jc_.transpose() * f_with_delta_).transpose() << std::endl;
+
+ std::cout << "LEFT " << (tmp_RNEA.head(3) + data_.M.block(0, 0, 3, 6) * box_qp_->get_ddq_res()).transpose() << std::endl;
+
+
// Increment log counter
k_log_++;
}
diff --git a/src/walk_parameters.yaml b/src/walk_parameters.yaml
index 11927438..17f0e731 100644
--- a/src/walk_parameters.yaml
+++ b/src/walk_parameters.yaml
@@ -5,22 +5,22 @@ robot:
DEMONSTRATION: false # Enable/disable demonstration functionalities
SIMULATION: true # Enable/disable PyBullet simulation or running on real robot
LOGGING: false # Enable/disable logging during the experiment
- PLOTTING: false # Enable/disable automatic plotting at the end of the experiment
+ PLOTTING: true # Enable/disable automatic plotting at the end of the experiment
envID: 0 # Identifier of the environment to choose in which one the simulation will happen
use_flat_plane: true # If True the ground is flat, otherwise it has bumps
- predefined_vel: false # If we are using a predefined reference velocity (True) or a joystick (False)
+ predefined_vel: true # If we are using a predefined reference velocity (True) or a joystick (False)
velID: 10 # Identifier of the reference velocity profile to choose which one will be sent to the robot
- N_SIMULATION: 100000 # Number of simulated wbc time steps
- enable_pyb_GUI: true # Enable/disable PyBullet GUI
+ N_SIMULATION: 9000 # Number of simulated wbc time steps
+ enable_pyb_GUI: false # Enable/disable PyBullet GUI
enable_corba_viewer: true # Enable/disable Corba Viewer
- enable_multiprocessing: true # Enable/disable running the MPC in another process in parallel of the main loop
+ enable_multiprocessing: false # Enable/disable running the MPC in another process in parallel of the main loop
perfect_estimator: false # Enable/disable perfect estimator by using data directly from PyBullet
# General control parameters
# [0.0, 0.865, -1.583, 0.0, 0.865, -1.583, 0.0, 0.865, -1.583, 0.0, 0.865, -1.583] # h_com = 0.2
- # q_init: [0.0, 0.764, -1.407, 0.0, 0.76407, -1.4, 0.0, 0.76407, -1.407, 0.0, 0.764, -1.407] #Â h_com = 0.218
- q_init: [0.0, 0.7, -1.4, 0.0, 0.7, -1.4, 0.0, -0.7, 1.4, 0.0, -0.7, 1.4] # Initial articular positions
- dt_wbc: 0.002 # Time step of the whole body control
+ q_init: [0.0, 0.764, -1.407, 0.0, 0.76407, -1.4, 0.0, 0.76407, -1.407, 0.0, 0.764, -1.407] #Â h_com = 0.218
+ # q_init: [0.0, 0.7, -1.4, 0.0, 0.7, -1.4, 0.0, -0.7, 1.4, 0.0, -0.7, 1.4] # Initial articular positions
+ dt_wbc: 0.001 # Time step of the whole body control
dt_mpc: 0.02 # Time step of the model predictive control
type_MPC: 0 # Which MPC solver you want to use: 0 for OSQP MPC, 1, 2, 3 for Crocoddyl MPCs
Kp_main: [3.0, 3.0, 3.0] # Proportional gains for the PD+
--
GitLab