diff --git a/include/qrw/QPWBC.hpp b/include/qrw/QPWBC.hpp
index 586689106f0bc4c9cf538a0a89dfb1b7076d806f..80befe6fcfbbd79b2cbaf38860b4903c43e0734e 100644
--- a/include/qrw/QPWBC.hpp
+++ b/include/qrw/QPWBC.hpp
@@ -295,7 +295,6 @@ class WbcWrapper {
VectorN get_qdes() { return qdes_; }
VectorN get_vdes() { return vdes_; }
VectorN get_tau_ff() { return tau_ff_; }
- VectorN get_tau_ff_mpc() { return tau_ff_mpc_; }
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_; }
@@ -314,15 +313,6 @@ class WbcWrapper {
pinocchio::Model model_; // Pinocchio model for frame computations
pinocchio::Data data_; // Pinocchio datas for frame computations
- pinocchio::Model model_fmpc_; // Pinocchio model for frame computations with MPC forces
- pinocchio::Data data_fmpc_; // Pinocchio datas for frame computations with MPC forces
- Matrix12 Jfmpc_; // Feet Jacobian (only linear part) for MPC forces
- Eigen::Matrix<double, 6, 18> Jfmpc_tmp_; // Temporary storage variable to only retrieve the linear part of the Jacobian
- // and discard the angular part
- Vector19 q_mpc_; // Configuration vector to compute the feet jacobian for mpc forces
- Vector12 tau_ff_mpc_; // Desired actuator torques (feedforward)
- int foot_ids_[4] = {0, 0, 0, 0}; // Feet frame IDs
-
Eigen::Matrix<double, 18, 18> M_; // Mass matrix
Eigen::Matrix<double, 12, 6> Jc_; // Jacobian matrix
Matrix14 k_since_contact_; // Number of time step during which feet have been in the current stance phase
diff --git a/python/gepadd.cpp b/python/gepadd.cpp
index db8ad87e828bcf3a045e28b27af7892a33ea5b77..75c4e9eae05186fd8d4f2d0a3ec3b6233ab2c113 100644
--- a/python/gepadd.cpp
+++ b/python/gepadd.cpp
@@ -293,7 +293,6 @@ struct WbcWrapperPythonVisitor : public bp::def_visitor<WbcWrapperPythonVisitor<
.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("get_tau_ff_mpc", &WbcWrapper::get_tau_ff_mpc, "Get tau_ff_mpc_.\n")
.def_readonly("qdes", &WbcWrapper::get_qdes)
.def_readonly("vdes", &WbcWrapper::get_vdes)
diff --git a/src/QPWBC.cpp b/src/QPWBC.cpp
index 73fc634f113f8b906cd74a6cc7b463682a72ee4e..2c49663b8d63d2c4f31ce56eb63d1aa9a8358e3d 100644
--- a/src/QPWBC.cpp
+++ b/src/QPWBC.cpp
@@ -439,11 +439,7 @@ void QPWBC::update_PQ() {
}
WbcWrapper::WbcWrapper()
- : Jfmpc_(Matrix12::Zero()),
- Jfmpc_tmp_(Eigen::Matrix<double, 6, 18>::Zero()),
- q_mpc_(Vector19::Zero()),
- tau_ff_mpc_(Vector12::Zero()),
- M_(Eigen::Matrix<double, 18, 18>::Zero()),
+ : M_(Eigen::Matrix<double, 18, 18>::Zero()),
Jc_(Eigen::Matrix<double, 12, 6>::Zero()),
k_since_contact_(Eigen::Matrix<double, 1, 4>::Zero()),
qdes_(Vector12::Zero()),
@@ -478,23 +474,6 @@ void WbcWrapper::initialize(Params ¶ms) {
q_tmp(6, 0) = 1.0; // Quaternion (0, 0, 0, 1)
pinocchio::computeAllTerms(model_, data_, q_tmp, VectorN::Zero(model_.nv));
- // Build model from urdf (base is not free flyer)
- pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(), model_fmpc_, false);
-
- // Construct data from model
- data_fmpc_ = pinocchio::Data(model_fmpc_);
-
- // Update all the quantities of the model
- VectorN qf_tmp = VectorN::Zero(model_fmpc_.nq);
- qf_tmp(6, 0) = 1.0; // Quaternion (0, 0, 0, 1)
- pinocchio::computeAllTerms(model_fmpc_, data_fmpc_, qf_tmp, VectorN::Zero(model_.nv));
-
- // Get feet frame IDs
- foot_ids_[0] = static_cast<int>(model_fmpc_.getFrameId("FL_FOOT")); // from long uint to int
- foot_ids_[1] = static_cast<int>(model_fmpc_.getFrameId("FR_FOOT"));
- foot_ids_[2] = static_cast<int>(model_fmpc_.getFrameId("HL_FOOT"));
- foot_ids_[3] = static_cast<int>(model_fmpc_.getFrameId("HR_FOOT"));
-
// Initialize inverse kinematic and box QP solvers
invkin_ = new InvKin();
invkin_->initialize(params);
@@ -531,47 +510,9 @@ void WbcWrapper::compute(VectorN const &q, VectorN const &dq, VectorN const &f_c
log_feet_vel_target = vgoals;
log_feet_acc_target = agoals;
- // Update model and data of the robot
- q_mpc_.block(3, 0, 4, 1) = pinocchio::SE3::Quaternion(pinocchio::rpy::rpyToMatrix(q_mpc(3, 0), q_mpc(4, 0), 0.0)).coeffs();
- q_mpc_.tail(12) = q_mpc.tail(12);
- pinocchio::forwardKinematics(model_fmpc_, data_fmpc_, q_mpc_);
- pinocchio::computeJointJacobians(model_fmpc_, data_fmpc_);
- pinocchio::updateFramePlacements(model_fmpc_, data_fmpc_);
-
- // Get data required by IK with Pinocchio
- for (int i = 0; i < 4; i++) {
- int idx = foot_ids_[i];
- Jfmpc_tmp_.setZero(); // Fill with 0s because getFrameJacobian only acts on the coeffs it changes so the
- // other coeffs keep their previous value instead of being set to 0
- pinocchio::getFrameJacobian(model_fmpc_, data_fmpc_, idx, pinocchio::LOCAL_WORLD_ALIGNED, Jfmpc_tmp_);
- Jfmpc_.block(3 * i, 0, 3, 12) = Jfmpc_tmp_.block(0, 6, 3, 12);
- }
-
- tau_ff_mpc_ = Jfmpc_.transpose() * -f_cmd;
-
- // 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_fmpc_, data_fmpc_, q_mpc_);
-
- // Make mass matrix symetric
- data_fmpc_.M.triangularView<Eigen::StrictlyLower>() = data_fmpc_.M.transpose().triangularView<Eigen::StrictlyLower>();
-
- /*
- std::cout << "--- q_mpc ---" << std::endl;
- std::cout << q_mpc_ << std::endl;
- std::cout << "--- fcmd ---" << std::endl;
- std::cout << f_cmd << std::endl;
- std::cout << "--- Jfmpc ---" << std::endl;
- std::cout << Jfmpc_ << std::endl;
- std::cout << "--- tau_ff_mpc ---" << std::endl;
- std::cout << tau_ff_mpc_ << std::endl;*/
-
- // std::cout << data_fmpc_.M.block(6, 6, 12, 12) << std::endl;
-
-
// Compute Inverse Kinematics
Vector19 q_IK = Vector19::Zero();
- q_IK.block(3, 0, 4, 1) = q_mpc_.block(3, 0, 4, 1);
+ q_IK.block(3, 0, 4, 1) = pinocchio::SE3::Quaternion(pinocchio::rpy::rpyToMatrix(q_mpc(3, 0), q_mpc(4, 0), 0.0)).coeffs();
q_IK.tail(12) = q.tail(12);
Vector18 dq_IK = Vector18::Zero();
dq_IK.tail(12) = dq.tail(12);
@@ -582,10 +523,6 @@ void WbcWrapper::compute(VectorN const &q, VectorN const &dq, VectorN const &f_c
invkin_->run_InvKin(q_IK, dq_IK, contacts, pgoals.transpose(), vgoals.transpose(), agoals.transpose(), Vector12::Zero());
ddq_cmd_ = invkin_->get_ddq_cmd();
- // std::cout << data_fmpc_.M.block(6, 6, 12, 12) * ddq_cmd_.tail(12) << std::endl;
-
- tau_ff_mpc_ = Jfmpc_.transpose() * -f_cmd + data_fmpc_.M.block(6, 6, 12, 12) * ddq_cmd_.tail(12);
-
// TODO: Check if we can save time by switching MatrixXd to defined sized vector since they are
// not called from python anymore
@@ -601,24 +538,6 @@ void WbcWrapper::compute(VectorN const &q, VectorN const &dq, VectorN const &f_c
}
}
- /*
- int cpt = 0;
- double ax = 0.0;
- double ay = 0.0;
- for (int i = 0; i < 4; i++) {
- if (contacts(0, i) == 1.0) {
- ax -= agoals(0, i);
- ay -= agoals(1, i);
- cpt++;
- }
- }
- if (cpt > 0) {
- ax *= 1 / cpt;
- ay *= 1 / cpt;
- }
- ddq_cmd_.head(2) << ax, ay;
- */
-
// 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