diff --git a/include/qrw/InvKin.hpp b/include/qrw/InvKin.hpp
index 50f9fa158e2dbc2ecc3bb052d441a81604d977e9..76b63417a07a3ec9a20c385beae26a5ee34696ad 100644
--- a/include/qrw/InvKin.hpp
+++ b/include/qrw/InvKin.hpp
@@ -35,6 +35,7 @@ public:
Matrix12 get_Jf() { return Jf_; }
int get_foot_id(int i) { return foot_ids_[i];}
Matrix43 get_posf() { return posf_; }
+ Matrix43 get_vf() { return vf_; }
private:
// Inputs of the constructor
diff --git a/include/qrw/QPWBC.hpp b/include/qrw/QPWBC.hpp
index 6530e4889f3a87c6be77cf1728926d7fadc78bcf..ec900b965a5d5ada8120697939116c3498c199be 100644
--- a/include/qrw/QPWBC.hpp
+++ b/include/qrw/QPWBC.hpp
@@ -152,12 +152,12 @@ public:
VectorN get_vdes() { return vdes_; }
VectorN get_tau_ff() { return tau_ff_; }
VectorN get_f_with_delta() { return f_with_delta_; }
- MatrixN get_feet_pos() { return MatrixN::Zero(3, 4); }
- MatrixN get_feet_err() { return MatrixN::Zero(3, 4); }
- MatrixN get_feet_vel() { return MatrixN::Zero(3, 4); }
- MatrixN get_feet_pos_target() { return MatrixN::Zero(3, 4); }
- MatrixN get_feet_vel_target() { return MatrixN::Zero(3, 4); }
- MatrixN get_feet_acc_target() { return MatrixN::Zero(3, 4); }
+ MatrixN get_feet_pos() { return invkin_->get_posf().transpose(); }
+ MatrixN get_feet_err() { return log_feet_pos_target - invkin_->get_posf().transpose(); }
+ MatrixN get_feet_vel() { return invkin_->get_vf().transpose(); }
+ MatrixN get_feet_pos_target() { return log_feet_pos_target; }
+ MatrixN get_feet_vel_target() { return log_feet_vel_target; }
+ MatrixN get_feet_acc_target() { return log_feet_acc_target; }
private:
@@ -182,6 +182,10 @@ private:
Matrix43 posf_tmp_; // Temporary matrix to store posf_ from invkin_
+ Matrix34 log_feet_pos_target; // Store the target feet positions
+ Matrix34 log_feet_vel_target; // Store the target feet velocities
+ Matrix34 log_feet_acc_target; // Store the target feet accelerations
+
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]
};
diff --git a/scripts/MPC_Wrapper.py b/scripts/MPC_Wrapper.py
index 4911761e0e4a07160088d2d62d1d60fd24035527..f68f97487328eeb5b1d171d06ad2bbb41b484faa 100644
--- a/scripts/MPC_Wrapper.py
+++ b/scripts/MPC_Wrapper.py
@@ -3,8 +3,8 @@
import numpy as np
import libquadruped_reactive_walking as MPC
from multiprocessing import Process, Value, Array
-# import crocoddyl_class.MPC_crocoddyl as MPC_crocoddyl
-# import crocoddyl_class.MPC_crocoddyl_planner as MPC_crocoddyl_planner
+import crocoddyl_class.MPC_crocoddyl as MPC_crocoddyl
+import crocoddyl_class.MPC_crocoddyl_planner as MPC_crocoddyl_planner
import pinocchio as pin
class Dummy:
@@ -38,7 +38,6 @@ class MPC_Wrapper:
self.not_first_iter = False
self.params = params
- self.testoui = True
# Number of WBC steps for 1 step of the MPC
self.k_mpc = int(params.dt_mpc/params.dt_wbc)
@@ -47,7 +46,9 @@ class MPC_Wrapper:
self.n_steps = np.int(params.T_mpc/params.dt_mpc)
self.T_gait = params.T_gait
self.N_gait = params.N_gait
- self.gait_memory = np.zeros(4)
+ self.gait_past = np.zeros(4)
+ self.gait_next = np.zeros(4)
+ self.mass = params.mass
self.mpc_type = params.type_MPC
self.multiprocessing = params.enable_multiprocessing
@@ -100,21 +101,19 @@ class MPC_Wrapper:
else: # Run in the same process than main loop
self.run_MPC_synchronous(k, xref, fsteps, l_targetFootstep)
- if k > 2:
- self.last_available_result[12:(12+self.n_steps), :] = np.roll(self.last_available_result[12:(12+self.n_steps), :], -1, axis=1)
- self.testoui = False
-
- pt = 0
- while (np.any(gait[pt, :])):
- pt += 1
- if k > 2 and not np.array_equal(gait[0, :], gait[pt-1, :]):
- mass = 2.5 # TODO: grab from URDF?
- nb_ctc = np.sum(gait[pt-1, :])
- F = 9.81 * mass / nb_ctc
- self.last_available_result[12:24, self.n_steps-1] = np.zeros(12)
- for i in range(4):
- if (gait[pt-1, i] == 1):
- self.last_available_result[12+3*i+2, self.n_steps-1] = F
+ if not np.allclose(gait[0, :], self.gait_past): # If gait status has changed
+ if np.allclose(gait[0, :], self.gait_next): # If we're still doing what was planned the last time MPC was solved
+ self.last_available_result[12:24, 0] = self.last_available_result[12:24, 1].copy()
+ else: # Otherwise use a default contact force command till we get the actual result of the MPC for this new sequence
+ F = 9.81 * self.mass / np.sum(gait[0, :])
+ self.last_available_result[12:24:3, 0] = 0.0
+ self.last_available_result[13:24:3, 0] = 0.0
+ self.last_available_result[14:24:3, 0] = F
+
+ self.last_available_result[12:24, 1:] = 0.0
+ self.gait_past = gait[0, :].copy()
+
+ self.gait_next = gait[1, :].copy()
return 0
diff --git a/src/QPWBC.cpp b/src/QPWBC.cpp
index d3ae392362d872e6bfa96f4aebb3295a6ff94716..ba9b0760df02ef995d9b9613dc16603cc9c18841 100644
--- a/src/QPWBC.cpp
+++ b/src/QPWBC.cpp
@@ -562,6 +562,9 @@ WbcWrapper::WbcWrapper()
, f_with_delta_(Vector12::Zero())
, ddq_with_delta_(Vector18::Zero())
, posf_tmp_(Matrix43::Zero())
+ , log_feet_pos_target(Matrix34::Zero())
+ , log_feet_vel_target(Matrix34::Zero())
+ , log_feet_acc_target(Matrix34::Zero())
, k_log_(0)
{}
@@ -610,6 +613,11 @@ void WbcWrapper::compute(VectorN const& q, VectorN const& dq, MatrixN const& f_c
k_since_contact_ += contacts; // Increment feet in stance phase
k_since_contact_ = k_since_contact_.cwiseProduct(contacts); // Reset feet in swing phase
+ // Store target positions, velocities and acceleration for logging purpose
+ log_feet_pos_target = pgoals;
+ log_feet_vel_target = vgoals;
+ log_feet_acc_target = agoals;
+
// 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();