diff --git a/include/qrw/MPC.hpp b/include/qrw/MPC.hpp
index d42ae05dce07d1dd751b7993ca4a725b7b894ce6..002eaa9a29cdc4a16988084a42d6d78e0aa24c39 100644
--- a/include/qrw/MPC.hpp
+++ b/include/qrw/MPC.hpp
@@ -50,7 +50,7 @@ class MPC {
/// \param[in] fsteps_in Footsteps location over the prediction horizon stored in a Nx12 matrix
///
////////////////////////////////////////////////////////////////////////////////////////////////
- int run(int num_iter, const Eigen::MatrixXd &xref_in, const Eigen::MatrixXd &fsteps_in);
+ int run(int num_iter, const Eigen::MatrixXd &xref_in, const Eigen::MatrixXd &fsteps_in, const Eigen::MatrixXd &nle);
// Getters
Eigen::MatrixXd get_latest_result(); // Return the latest desired contact forces that have been computed
@@ -157,7 +157,7 @@ class MPC {
/// \param[in] fsteps Footsteps location over the prediction horizon stored in a Nx12 matrix
///
////////////////////////////////////////////////////////////////////////////////////////////////
- int update_matrices(Eigen::MatrixXd fsteps);
+ int update_matrices(Eigen::MatrixXd fsteps, const Eigen::Matrix<double, 6, 1> &nle);
////////////////////////////////////////////////////////////////////////////////////////////////
///
@@ -173,7 +173,7 @@ class MPC {
/// \brief Update the N and K matrices involved in the MPC constraint equations M.X = N and L.X <= K
///
////////////////////////////////////////////////////////////////////////////////////////////////
- int update_NK();
+ int update_NK(const Eigen::Matrix<double, 6, 1> &nle);
////////////////////////////////////////////////////////////////////////////////////////////////
///
diff --git a/include/qrw/QPWBC.hpp b/include/qrw/QPWBC.hpp
index a120cabcfce16dac7f98ac3a7272233e172ab8c1..e19c2d7fed1d81980afcdf9a6baced3f77601268 100644
--- a/include/qrw/QPWBC.hpp
+++ b/include/qrw/QPWBC.hpp
@@ -324,6 +324,7 @@ class WbcWrapper {
VectorN get_ddq_cmd() { return ddq_cmd_; }
VectorN get_f_with_delta() { return f_with_delta_; }
VectorN get_ddq_with_delta() { return ddq_with_delta_; }
+ VectorN get_nle() { return nle_; }
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(); }
@@ -331,6 +332,12 @@ class WbcWrapper {
MatrixN get_feet_vel_target() { return log_feet_vel_target; }
MatrixN get_feet_acc_target() { return log_feet_acc_target; }
+ VectorN get_Mddq() { return Mddq;};
+ VectorN get_NLE() { return NLE;};
+ VectorN get_JcTf() { return JcTf;};
+ VectorN get_Mddq_out() { return Mddq_out;};
+ VectorN get_JcTf_out() { return JcTf_out;};
+
private:
Params *params_; // Object that stores parameters
QPWBC *box_qp_; // QP problem solver for the whole body control
@@ -353,12 +360,21 @@ class WbcWrapper {
Vector12 f_with_delta_; // Contact forces with deltas found by QP solver
Vector18 ddq_with_delta_; // Actuator accelerations with deltas found by QP solver
+ Vector6 nle_; // Non linear effects
+
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
+ // Log
+ Vector6 Mddq;
+ Vector6 NLE;
+ Vector6 JcTf;
+ Vector6 Mddq_out;
+ Vector6 JcTf_out;
+
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/python/gepadd.cpp b/python/gepadd.cpp
index 1af730f6021c54fc9ca540f7a5104a78544e8bde..45de57c717b5db250a7458d9be93ed41bdc53078 100644
--- a/python/gepadd.cpp
+++ b/python/gepadd.cpp
@@ -27,7 +27,7 @@ struct MPCPythonVisitor : public bp::def_visitor<MPCPythonVisitor<MPC>>
"Constructor with parameters."))
// Run MPC from Python
- .def("run", &MPC::run, bp::args("num_iter", "xref_in", "fsteps_in"), "Run MPC from Python.\n")
+ .def("run", &MPC::run, bp::args("num_iter", "xref_in", "fsteps_in", "nle"), "Run MPC from Python.\n")
.def("get_latest_result", &MPC::get_latest_result,
"Get latest result (predicted trajectory forces to apply).\n")
.def("get_gait", &MPC::get_gait, "Get gait matrix.\n")
@@ -309,12 +309,18 @@ struct WbcWrapperPythonVisitor : public bp::def_visitor<WbcWrapperPythonVisitor<
.def_readonly("ddq_cmd", &WbcWrapper::get_ddq_cmd)
.def_readonly("f_with_delta", &WbcWrapper::get_f_with_delta)
.def_readonly("ddq_with_delta", &WbcWrapper::get_ddq_with_delta)
+ .def_readonly("nle", &WbcWrapper::get_nle)
.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)
+ .def_readonly("Mddq", &WbcWrapper::get_Mddq)
+ .def_readonly("NLE", &WbcWrapper::get_NLE)
+ .def_readonly("JcTf", &WbcWrapper::get_JcTf)
+ .def_readonly("Mddq_out", &WbcWrapper::get_Mddq_out)
+ .def_readonly("JcTf_out", &WbcWrapper::get_JcTf_out)
// Run WbcWrapper from Python
.def("compute", &WbcWrapper::compute, bp::args("q", "dq", "f_cmd", "contacts", "pgoals", "vgoals",
diff --git a/scripts/Controller.py b/scripts/Controller.py
index 647093a60d47899c54e527e48b7f36a33b892c2c..c65aa9ed5f784efa9b6828a98992b5fd54da65c5 100644
--- a/scripts/Controller.py
+++ b/scripts/Controller.py
@@ -191,6 +191,8 @@ class Controller:
self.filter_mpc_vref = lqrw.Filter()
self.filter_mpc_vref.initialize(params)
+ self.nle = np.zeros((6, 1))
+
# Interface with the PD+ on the control board
self.result = Result()
@@ -252,7 +254,7 @@ class Controller:
o_targetFootstep = self.footstepPlanner.updateFootsteps(self.k % self.k_mpc == 0 and self.k != 0,
int(self.k_mpc - self.k % self.k_mpc),
self.q[:, 0],
- self.h_v_filt_mpc[0:6, 0:1].copy(),
+ self.h_v_windowed[0:6, 0:1].copy(),
self.v_ref[0:6, 0:1])
# Run state planner (outputs the reference trajectory of the base)
@@ -284,11 +286,15 @@ class Controller:
self.footTrajectoryGenerator.getFootJerk(),
self.footTrajectoryGenerator.getTswing() - self.footTrajectoryGenerator.getT0s())
else :
- self.mpc_wrapper.solve(self.k, xref, fsteps, cgait, np.zeros((3,4)))
+ self.mpc_wrapper.solve(self.k, xref, fsteps, cgait, np.zeros((3,4)), nle=self.nle)
except ValueError:
print("MPC Problem")
+ """if (self.k % self.k_mpc) == 0:
+ from IPython import embed
+ embed()"""
+
# Retrieve reference contact forces in horizontal frame
self.x_f_mpc = self.mpc_wrapper.get_latest_result()
@@ -347,6 +353,8 @@ class Controller:
self.xgoals[6:, 0] = self.vref_filt_mpc[:, 0] # Velocities (in horizontal frame!)
+ print(" ###### ")
+
# Run InvKin + WBC QP
self.wbcWrapper.compute(self.q_wbc, self.dq_wbc,
(self.x_f_mpc[12:24, 0:1]).copy(), np.array([cgait[0, :]]),
@@ -363,6 +371,8 @@ class Controller:
self.result.FF = self.Kff_main * np.ones(12)
self.result.tau_ff[:] = self.wbcWrapper.tau_ff
+ self.nle[:3, 0] = self.wbcWrapper.nle[:3]
+
# Display robot in Gepetto corba viewer
if self.enable_corba_viewer and (self.k % 5 == 0):
self.q_display[:3, 0] = self.q_wbc[0:3, 0]
@@ -378,7 +388,7 @@ class Controller:
device.imu.attitude_euler[2])).coeffs().tolist()
pyb.resetBasePositionAndOrientation(device.pyb_sim.robotId, oTh_pyb, q_oRb_pyb)"""
- 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("x_f_mpc: ", self.x_f_mpc[:, 0])
print("ddq delta: ", self.wbcWrapper.ddq_with_delta)
@@ -386,10 +396,10 @@ class Controller:
from matplotlib import pyplot as plt
plt.figure()
plt.plot(self.x_f_mpc[6, :])
- plt.show(block=True)
+ plt.show(block=True)"""
- if self.k == 1:
- quit()
+ """if self.k == 1:
+ quit()"""
t_wbc = time.time()
diff --git a/scripts/MPC_Wrapper.py b/scripts/MPC_Wrapper.py
index a0e9c36089ea811764f0ff480f8104b047189b71..80fc23ec828ab49cb0d169d4dacdbe9cae120109 100644
--- a/scripts/MPC_Wrapper.py
+++ b/scripts/MPC_Wrapper.py
@@ -130,7 +130,8 @@ class MPC_Wrapper:
self.last_available_result[:24, 0] = np.hstack((x_init, np.array([0.0, 0.0, 8.0] * 4)))
def solve(self, k, xref, fsteps, gait, l_fsteps_target, oRh = np.eye(3), oTh = np.zeros((3,1)),
- position = np.zeros((3,4)), velocity = np.zeros((3,4)) , acceleration = np.zeros((3,4)), jerk = np.zeros((3,4)) , dt_flying = np.zeros(4)):
+ position = np.zeros((3,4)), velocity = np.zeros((3,4)) , acceleration = np.zeros((3,4)), jerk = np.zeros((3,4)) ,
+ dt_flying = np.zeros(4), nle = np.zeros((6, 1))):
"""Call either the asynchronous MPC or the synchronous MPC depending on the value of multiprocessing during
the creation of the wrapper
@@ -147,7 +148,7 @@ class MPC_Wrapper:
if self.multiprocessing: # Run in parallel process
self.run_MPC_asynchronous(k, xref, fsteps, l_fsteps_target, oRh, oTh, position, velocity, acceleration, jerk, dt_flying)
else: # Run in the same process than main loop
- self.run_MPC_synchronous(k, xref, fsteps, l_fsteps_target, oRh, oTh, position, velocity, acceleration, jerk, dt_flying)
+ self.run_MPC_synchronous(k, xref, fsteps, l_fsteps_target, oRh, oTh, position, velocity, acceleration, jerk, dt_flying, nle)
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
@@ -188,7 +189,7 @@ class MPC_Wrapper:
self.not_first_iter = True
return self.last_available_result
- def run_MPC_synchronous(self, k, xref, fsteps, l_fsteps_target, oRh, oTh, position, velocity, acceleration, jerk, dt_flying):
+ def run_MPC_synchronous(self, k, xref, fsteps, l_fsteps_target, oRh, oTh, position, velocity, acceleration, jerk, dt_flying, nle):
"""Run the MPC (synchronous version) to get the desired contact forces for the feet currently in stance phase
Args:
@@ -203,7 +204,7 @@ class MPC_Wrapper:
if self.mpc_type == MPC_type.OSQP:
# OSQP MPC
- self.mpc.run(np.int(k), xref.copy(), fsteps.copy())
+ self.mpc.run(np.int(k), xref.copy(), fsteps.copy(), nle.copy())
elif self.mpc_type == MPC_type.CROCODDYL_PLANNER: # Add goal position to stop the optimisation
# Crocoddyl MPC
self.mpc.solve(k, xref.copy(), fsteps.copy(), l_fsteps_target, position, velocity, acceleration, jerk, oRh, oTh, dt_flying)
diff --git a/scripts/main_solo12_control.py b/scripts/main_solo12_control.py
index 266c43a276471f38d0692adce81319c397eafb9b..8103d581e5d24577cf3da6c56d148d3a377abb37 100644
--- a/scripts/main_solo12_control.py
+++ b/scripts/main_solo12_control.py
@@ -1,5 +1,6 @@
# coding: utf8
+import os
import threading
from Controller import Controller
import numpy as np
@@ -173,6 +174,12 @@ def control_loop(name_interface_clone=None, des_vel_analysis=None):
t_start_whole = 0.0
T_whole = time.time()
dT_whole = 0.0
+
+ log_Mddq = np.zeros((params.N_SIMULATION, 6))
+ log_NLE = np.zeros((params.N_SIMULATION, 6))
+ log_JcTf = np.zeros((params.N_SIMULATION, 6))
+ log_Mddq_out = np.zeros((params.N_SIMULATION, 6))
+ log_JcTf_out = np.zeros((params.N_SIMULATION, 6))
while ((not device.is_timeout) and (t < t_max) and (not controller.error)):
t_start_whole = time.time()
@@ -199,6 +206,12 @@ def control_loop(name_interface_clone=None, des_vel_analysis=None):
device.joints.set_desired_velocities(controller.result.v_des)
device.joints.set_torques(controller.result.FF * controller.result.tau_ff.ravel())
+ log_Mddq[k_log_whole] = controller.wbcWrapper.Mddq
+ log_NLE[k_log_whole] = controller.wbcWrapper.NLE
+ log_JcTf[k_log_whole] = controller.wbcWrapper.JcTf
+ log_Mddq_out[k_log_whole] = controller.wbcWrapper.Mddq_out
+ log_JcTf_out[k_log_whole] = controller.wbcWrapper.JcTf_out
+
# Call logger
if params.LOGGING or params.PLOTTING:
loggerSensors.sample(device, qc)
@@ -303,6 +316,21 @@ def control_loop(name_interface_clone=None, des_vel_analysis=None):
print("Masterboard timeout detected.")
print("Either the masterboard has been shut down or there has been a connection issue with the cable/wifi.")
+ from matplotlib import pyplot as plt
+ N = loggerControl.tstamps.shape[0]
+ t_range = np.array([k*loggerControl.dt for k in range(N)])
+ plt.figure()
+ plt.plot(t_range, log_Mddq[:-3, 0], "r")
+ plt.plot(t_range, log_NLE[:-3, 0], "b")
+ plt.plot(t_range, log_Mddq[:-3, 0] + log_NLE[:-3, 0], "violet", linestyle="--")
+ plt.plot(t_range, log_JcTf[:-3, 0], "g")
+ plt.plot(t_range, log_Mddq_out[:-3, 0], "darkred")
+ plt.plot(t_range, log_Mddq_out[:-3, 0] + log_NLE[:-3, 0], "darkorchid", linestyle="--")
+ plt.plot(t_range, log_JcTf_out[:-3, 0], "mediumblue")
+ plt.plot(t_range, loggerControl.planner_gait[:, 0, 0], "k", linewidth=3)
+ plt.legend(["Mddq", "NLE", "Mddq+NLE", "JcT f", "Mddq out", "Mddq out + NLE", "JcT f out", "Contact"])
+ plt.show(block=True)
+
# Plot recorded data
if params.PLOTTING:
loggerControl.plotAll(loggerSensors)
@@ -341,6 +369,7 @@ def main():
help='Name of the clone interface that will reproduce the movement of the first one \
(use ifconfig in a terminal), for instance "enp1s0"')
+ os.nice(-20) # Â Set the process to highest priority (from -20 highest to +20 lowest)
f, v = control_loop(parser.parse_args().clone) # , np.array([1.5, 0.0, 0.0, 0.0, 0.0, 0.0]))
print(f, v)
quit()
diff --git a/src/MPC.cpp b/src/MPC.cpp
index 0a33ed8ea27f6e1ab56a46b55011d4dc7d17ce0d..6a21a3baf021f8ade0c4b8e7998930a95be526bd 100644
--- a/src/MPC.cpp
+++ b/src/MPC.cpp
@@ -377,7 +377,7 @@ int MPC::create_weight_matrices() {
return 0;
}
-int MPC::update_matrices(Eigen::MatrixXd fsteps) {
+int MPC::update_matrices(Eigen::MatrixXd fsteps, const Eigen::Matrix<double, 6, 1> &nle) {
/* M need to be updated between each iteration:
- lever_arms changes since the robot moves
- I_inv changes if the reference velocity vector is modified
@@ -387,7 +387,7 @@ int MPC::update_matrices(Eigen::MatrixXd fsteps) {
/* N need to be updated between each iteration:
- X0 changes since the robot moves
- Xk* changes since X0 is not the same */
- update_NK();
+ update_NK(nle);
// L matrix is constant
// K matrix is constant
@@ -441,16 +441,26 @@ int MPC::update_ML(Eigen::MatrixXd fsteps) {
return 0;
}
-int MPC::update_NK() {
+int MPC::update_NK(const Eigen::Matrix<double, 6, 1> &nle) {
// Matrix g is already created and not changed
+ //std::cout << "NLE MPC: " << std::endl << nle.transpose() << std::endl;
+
// Reset NK
NK_up = Eigen::Matrix<double, Eigen::Dynamic, 1>::Zero(12 * n_steps * 2 + 20 * n_steps, 1);
+ //std::cout << "g: " << std::endl << -g.tail(6).transpose() << std::endl;
+ //std::cout << "gbis: " << std::endl << dt * nle.tail(6).transpose() / mass << std::endl;
+
// Fill N matrix with g matrices
for (int k = 0; k < n_steps; k++) {
NK_up(12 * k + 8, 0) = -g(8, 0); // only 8-th coeff is non zero
}
+ /*for (int k = 0; k < n_steps; k++) {
+ NK_up(12 * k + 6, 0) = dt * nle(0) / mass;
+ NK_up(12 * k + 7, 0) = dt * nle(1) / mass;
+ NK_up(12 * k + 8, 0) = dt * nle(2) / mass;
+ }*/
// Including - A*X0 in the first row of N
NK_up.block(0, 0, 12, 1) += A * (-x0);
@@ -577,7 +587,7 @@ Return the next predicted state of the base
*/
double *MPC::get_x_next() { return x_next; }
-int MPC::run(int num_iter, const Eigen::MatrixXd &xref_in, const Eigen::MatrixXd &fsteps_in) {
+int MPC::run(int num_iter, const Eigen::MatrixXd &xref_in, const Eigen::MatrixXd &fsteps_in, const Eigen::MatrixXd &nle) {
// Recontruct the gait based on the computed footsteps
construct_gait(fsteps_in);
@@ -590,7 +600,7 @@ int MPC::run(int num_iter, const Eigen::MatrixXd &xref_in, const Eigen::MatrixXd
if (num_iter == 0) {
create_matrices();
} else {
- update_matrices(fsteps_in);
+ update_matrices(fsteps_in, nle);
}
// Create an initial guess and call the solver to solve the QP problem
diff --git a/src/QPWBC.cpp b/src/QPWBC.cpp
index 21d62f28b576d5d03eaf6d19c61e203498bdd867..9e94cceea61af1d968bf142d17cac73c1ff71397 100644
--- a/src/QPWBC.cpp
+++ b/src/QPWBC.cpp
@@ -345,10 +345,10 @@ int QPWBC::retrieve_result(const Eigen::Matrix<double, 6, 1> &ddq_cmd, const Eig
// ddq_res = A * f_res + gamma;
// Adding reference contact forces to delta f
- f_res += f_cmd;
+ // f_res += f_cmd;
// Adding reference acceleration to delta ddq
- ddq_res += ddq_cmd;
+ // ddq_res += ddq_cmd;
std::cout << "f_cmd: " << std::endl << f_cmd.transpose() << std::endl;
std::cout << "f_res: " << std::endl << f_res.transpose() << std::endl;
@@ -568,6 +568,7 @@ WbcWrapper::WbcWrapper()
ddq_cmd_(Vector18::Zero()),
f_with_delta_(Vector12::Zero()),
ddq_with_delta_(Vector18::Zero()),
+ nle_(Vector6::Zero()),
posf_tmp_(Matrix43::Zero()),
log_feet_pos_target(Matrix34::Zero()),
log_feet_vel_target(Matrix34::Zero()),
@@ -637,6 +638,13 @@ void WbcWrapper::compute(VectorN const &q, VectorN const &dq, VectorN const &f_c
// Retrieve velocity data
dq_wbc_ = dq;
+ // 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_wbc_);
+
+ // Make mass matrix symetric
+ data_.M.triangularView<Eigen::StrictlyLower>() = data_.M.transpose().triangularView<Eigen::StrictlyLower>();
+
// Compute Inverse Kinematics
invkin_->run_InvKin(q_wbc_, dq_wbc_, contacts, pgoals.transpose(), vgoals.transpose(), agoals.transpose(), xgoals);
ddq_cmd_ = invkin_->get_ddq_cmd();
@@ -654,6 +662,15 @@ void WbcWrapper::compute(VectorN const &q, VectorN const &dq, VectorN const &f_c
}
}
+ Eigen::Matrix<double, 12, 12> Jc_u_ = Eigen::Matrix<double, 12, 12>::Zero();
+ for (int i = 0; i < 4; i++) {
+ if (contacts(0, i)) {
+ Jc_u_.block(3 * i, 0, 3, 12) = invkin_->get_Jf().block(3 * i, 6, 3, 12);
+ } else {
+ Jc_u_.block(3 * i, 0, 3, 12).setZero();
+ }
+ }
+
// 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
@@ -670,17 +687,44 @@ void WbcWrapper::compute(VectorN const &q, VectorN const &dq, VectorN const &f_c
std::cout << "k_since" << std::endl;
std::cout << k_since_contact_ << std::endl;*/
- std::cout << "dqq_cmd_bis " << std::endl << ddq_cmd_.transpose() << std::endl;
+ std::cout << "agoals " << std::endl << agoals << std::endl;
+ std::cout << "ddq_cmd_bis " << std::endl << ddq_cmd_.transpose() << std::endl;
// Solve the QP problem
box_qp_->run(data_.M, Jc_, ddq_cmd_, f_cmd, 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();
+ f_with_delta_ = f_cmd + 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);
+ 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;
+ std::cout << "RNEA: " << std::endl << data_.tau.head(6).transpose() << std::endl;
+ Vector6 tmp_RNEA = data_.tau.head(6);
+ //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));
+ std::cout << "NLE: " << std::endl << data_.tau.head(6).transpose() << std::endl;
+ std::cout << "DDQ: " << std::endl << (tmp_RNEA - data_.tau.head(6)).transpose() << std::endl;
+
+ std::cout << "JcT f_cmd: " << std::endl << (Jc_.transpose() * f_cmd).transpose() << std::endl;
+
+ // std::cout << "Gravity ?: " << std::endl << (data_.M * ddq_cmd_ - data_.tau.head(6)).transpose() << std::endl;
+
+ Mddq = tmp_RNEA - data_.tau.head(6);
+ NLE = data_.tau.head(6);
+ JcTf = Jc_.transpose() * f_cmd;
+
+ nle_ = data_.tau.head(6);
+
// Compute joint torques from contact forces and desired accelerations
pinocchio::rnea(model_, data_, q_wbc_, dq_wbc_, ddq_with_delta_);
@@ -693,7 +737,10 @@ void WbcWrapper::compute(VectorN const &q, VectorN const &dq, VectorN const &f_c
std::cout << "Jf" << std::endl;
std::cout << invkin_->get_Jf().block(0, 6, 12, 12).transpose() << std::endl;*/
- tau_ff_ = data_.tau.tail(12) - invkin_->get_Jf().block(0, 6, 12, 12).transpose() * f_with_delta_;
+ // std::cout << " -- " << std::endl << invkin_->get_Jf().block(0, 6, 12, 12) << std::endl << " -- " << std::endl << Jc_u_ << std::endl;
+
+ tau_ff_ = data_.tau.tail(12) - Jc_u_.transpose() * f_with_delta_;
+ //tau_ff_ = - Jc_u_.transpose() * f_cmd;
// Retrieve desired positions and velocities
vdes_ = invkin_->get_dq_cmd().tail(12);
@@ -703,12 +750,14 @@ void WbcWrapper::compute(VectorN const &q, VectorN const &dq, VectorN const &f_c
qdes_ = invkin_->get_q_cmd().tail(12);
bdes_ = invkin_->get_q_cmd().head(7);
-
pinocchio::rnea(model_, data_, q_wbc_, dq_wbc_, ddq_with_delta_);
- Vector6 tau_left = data_.tau.head(6);
+ Vector6 tau_left = data_.tau.head(6); // data_.M * ddq_with_delta_.head(6) - Jc_.transpose() * f_with_delta_; //
Vector6 tau_right = Jc_.transpose() * f_with_delta_;
- std::cout << "tau_left: " << std::endl << tau_left.transpose() << std::endl;
- std::cout << "tau_right: " << std::endl << tau_right.transpose() << std::endl;
+ // std::cout << "tau_left: " << std::endl << tau_left.transpose() << std::endl;
+ // std::cout << "tau_right: " << std::endl << tau_right.transpose() << std::endl;
+
+ Mddq_out = data_.tau.head(6) - NLE;
+ JcTf_out = Jc_.transpose() * f_with_delta_;
/*std::cout << vdes_.transpose() << std::endl;
std::cout << qdes_.transpose() << std::endl;*/
diff --git a/src/config_solo12.yaml b/src/config_solo12.yaml
index eadfbb2b0fb3987740a73bb6115975ab6d26bb3d..29604b79a856787665fe01555beeda1d27d36dd3 100644
--- a/src/config_solo12.yaml
+++ b/src/config_solo12.yaml
@@ -11,13 +11,13 @@ robot:
velID: 10 # Identifier of the reference velocity profile to choose which one will be sent to the robot
N_SIMULATION: 9000 # Number of simulated wbc time steps
enable_pyb_GUI: false # Enable/disable PyBullet GUI
- enable_corba_viewer: false # Enable/disable Corba Viewer
+ enable_corba_viewer: true # Enable/disable Corba Viewer
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
- # [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.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
@@ -44,21 +44,22 @@ robot:
# Parameters of MPC with OSQP
# [0.0, 0.0, 20.0, 0.25, 0.25, 10.0, 0.05, 0.05, 0.2, 0.0, 0.0, 0.3]
+ # [2.0, 2.0, 5.0, 0.25, 0.25, 1.0, 0.2, 0.2, 0.2, 0.0, 0.0, 0.3]
osqp_w_states: [2.0, 2.0, 20.0, 0.25, 0.25, 10.0, 0.2, 0.2, 0.2, 0.0, 0.0, 0.3] # Weights for state tracking error
osqp_w_forces: [0.00005, 0.00005, 0.00005] # Weights for force regularisation
osqp_Nz_lim: 35.0 # Maximum vertical force that can be applied at contact points
# Parameters of InvKin
- Kp_flyingfeet: 10.0 # Proportional gain for feet position tasks
- Kd_flyingfeet: 1.0 # Derivative gain for feet position tasks
- Kp_base_position: [0.0, 0.0, 0.0] # Proportional gains for the base position task
- Kd_base_position: [100.0, 100.0, 10.0] # Derivative gains for the base position task
+ Kp_flyingfeet: 100.0 # Proportional gain for feet position tasks
+ Kd_flyingfeet: 10.0 # Derivative gain for feet position tasks
+ Kp_base_position: [100.0, 100.0, 100.0] # Proportional gains for the base position task
+ Kd_base_position: [10.0, 10.0, 10.0] # Derivative gains for the base position task
Kp_base_orientation: [100.0, 100.0, 100.0] # Proportional gains for the base orientation task
Kd_base_orientation: [10.0, 10.0, 10.0] # Derivative gains for the base orientation task
- w_tasks: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0] #Â Tasks weights: [feet/base, vx, vy, vz, roll+wroll, pitch+wpitch, wyaw]
+ w_tasks: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0] #Â Tasks weights: [feet/base, vx, vy, vz, roll+wroll, pitch+wpitch, wyaw, contacts]
# Parameters of WBC QP problem
Q1: 0.1 # Weights for the "delta articular accelerations" optimization variables
- Q2: 10.0 # Weights for the "delta contact forces" optimization variables
+ Q2: 1.0 # Weights for the "delta contact forces" optimization variables
Fz_max: 35.0 # Maximum vertical contact force [N]
Fz_min: 0.0 # Minimal vertical contact force [N]
\ No newline at end of file