diff --git a/include/qrw/InvKin.hpp b/include/qrw/InvKin.hpp
index 435622f2e2416cdc3f3ca996f3b84f2054a86088..2021d26d56d2399408e28058a6d0d51088101ca7 100644
--- a/include/qrw/InvKin.hpp
+++ b/include/qrw/InvKin.hpp
@@ -18,6 +18,7 @@
#include "pinocchio/algorithm/compute-all-terms.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/frames.hpp"
+#include "pinocchio/algorithm/joint-configuration.hpp"
#include <Eigen/Core>
#include <Eigen/Dense>
#include <cmath>
@@ -75,16 +76,17 @@ class InvKin {
/// \param[in] pgoals Desired positions of the four feet in base frame
/// \param[in] vgoals Desired velocities of the four feet in base frame
/// \param[in] agoals Desired accelerations of the four feet in base frame
+ /// \param[in] x_cmd Desired position, orientation and velocity of the base
///
////////////////////////////////////////////////////////////////////////////////////////////////
void run_InvKin(VectorN const& q, VectorN const& dq, MatrixN const& contacts, MatrixN const& pgoals,
- MatrixN const& vgoals, MatrixN const& agoals);
+ MatrixN const& vgoals, MatrixN const& agoals, MatrixN const& x_cmd);
- Eigen::MatrixXd get_q_step() { return q_step_; }
- Eigen::MatrixXd get_dq_cmd() { return dq_cmd_; }
+ VectorN get_q_step() { return q_step_; }
+ VectorN get_dq_cmd() { return dq_cmd_; }
VectorN get_q_cmd() { return q_cmd_; }
VectorN get_ddq_cmd() { return ddq_cmd_; }
- Matrix12 get_Jf() { return Jf_; }
+ Eigen::Matrix<double, 12, 18> get_Jf() { return Jf_; }
int get_foot_id(int i) { return foot_ids_[i]; }
Matrix43 get_posf() { return posf_; }
Matrix43 get_vf() { return vf_; }
@@ -95,31 +97,72 @@ class InvKin {
// Matrices initialisation
Matrix12 invJ; // Inverse of the feet Jacobian
- Matrix112 acc; // Reshaped feet acceleration references to get command accelerations for actuators
- Matrix112 x_err; // Reshaped feet position errors to get command position step for actuators
- Matrix112 dx_r; // Reshaped feet velocity references to get command velocities for actuators
+ Matrix118 acc; // Reshaped feet acceleration references to get command accelerations for actuators
+ Matrix118 x_err; // Reshaped feet position errors to get command position step for actuators
+ Matrix118 dx_r; // Reshaped feet velocity references to get command velocities for actuators
Matrix43 pfeet_err; // Feet position errors to get command position step for actuators
Matrix43 vfeet_ref; // Feet velocity references to get command velocities for actuators
Matrix43 afeet; // Feet acceleration references to get command accelerations for actuators
int foot_ids_[4] = {0, 0, 0, 0}; // Feet frame IDs
+ int base_id_ = 0; // Base ID
Matrix43 posf_; // Current feet positions
Matrix43 vf_; // Current feet linear velocities
Matrix43 wf_; // Current feet angular velocities
Matrix43 af_; // Current feet linear accelerations
- Matrix12 Jf_; // Current feet Jacobian (only linear part)
- Eigen::Matrix<double, 6, 12> Jf_tmp_; // Temporary storage variable to only retrieve the linear part of the Jacobian
+ Eigen::Matrix<double, 12, 18> Jf_; // Current feet Jacobian (only linear part)
+ Eigen::Matrix<double, 6, 18> Jf_tmp_; // Temporary storage variable to only retrieve the linear part of the Jacobian
// and discard the angular part
- Vector12 ddq_cmd_; // Actuator command accelerations
- Vector12 dq_cmd_; // Actuator command velocities
- Vector12 q_cmd_; // Actuator command positions
- Vector12 q_step_; // Actuator command position steps
+ Vector3 posb_;
+ Vector3 posb_ref_;
+ Vector3 posb_err_;
+ Matrix3 rotb_;
+ Matrix3 rotb_ref_;
+ Vector3 rotb_err_;
+ Vector3 vb_;
+ Vector3 vb_ref_;
+ Vector3 wb_;
+ Vector3 wb_ref_;
+ Vector6 ab_;
+ Vector3 abasis;
+ Vector3 awbasis;
+ Eigen::Matrix<double, 6, 18> Jb_;
+
+ Eigen::Matrix<double, 18, 18> J_;
+ Eigen::Matrix<double, 18, 18> invJ_;
+
+ Vector3 Kp_base_position;
+ Vector3 Kd_base_position;
+ Vector3 Kp_base_orientation;
+ Vector3 Kd_base_orientation;
+
+ Vector18 ddq_cmd_; // Actuator command accelerations
+ Vector18 dq_cmd_; // Actuator command velocities
+ Vector19 q_cmd_; // Actuator command positions
+ Vector18 q_step_; // Actuator command position steps
pinocchio::Model model_; // Pinocchio model for frame computations and inverse kinematics
pinocchio::Data data_; // Pinocchio datas for frame computations and inverse kinematics
};
+////////////////////////////////////////////////////////////////////////////////////////////////
+///
+/// \brief Compute the pseudo inverse of a matrix using the Jacobi SVD formula
+///
+////////////////////////////////////////////////////////////////////////////////////////////////
+template <typename _Matrix_Type_>
+_Matrix_Type_ pseudoInverse(const _Matrix_Type_ &a, double epsilon = std::numeric_limits<double>::epsilon()) {
+ Eigen::JacobiSVD<_Matrix_Type_> svd(a, Eigen::ComputeThinU | Eigen::ComputeThinV);
+ double tolerance =
+ epsilon * static_cast<double>(std::max(a.cols(), a.rows())) * svd.singularValues().array().abs()(0);
+ return svd.matrixV() *
+ (svd.singularValues().array().abs() > tolerance)
+ .select(svd.singularValues().array().inverse(), 0)
+ .matrix()
+ .asDiagonal() *
+ svd.matrixU().adjoint();
+}
#endif // INVKIN_H_INCLUDED
diff --git a/include/qrw/QPWBC.hpp b/include/qrw/QPWBC.hpp
index c96fb0cc6492fc23bfd443ace637e1e304e981a8..586689106f0bc4c9cf538a0a89dfb1b7076d806f 100644
--- a/include/qrw/QPWBC.hpp
+++ b/include/qrw/QPWBC.hpp
@@ -285,10 +285,12 @@ class WbcWrapper {
/// \param[in] vgoals Desired velocities of the four feet in base frame
/// \param[in] agoals Desired accelerations of the four feet in base frame
/// \param[in] q_mpc Estimated configuration vector given to the MPC
+ /// \param[in] v_mpc Estimated velocity vector given to the MPC
///
////////////////////////////////////////////////////////////////////////////////////////////////
void compute(VectorN const &q, VectorN const &dq, VectorN const &f_cmd, MatrixN const &contacts,
- MatrixN const &pgoals, MatrixN const &vgoals, MatrixN const &agoals, VectorN const &q_mpc);
+ MatrixN const &pgoals, MatrixN const &vgoals, MatrixN const &agoals, VectorN const &q_mpc,
+ VectorN const &v_mpc);
VectorN get_qdes() { return qdes_; }
VectorN get_vdes() { return vdes_; }
@@ -343,22 +345,4 @@ class WbcWrapper {
int indexes_[4] = {10, 18, 26, 34}; // Indexes of feet frames in this order: [FL, FR, HL, HR]
};
-////////////////////////////////////////////////////////////////////////////////////////////////
-///
-/// \brief Compute the pseudo inverse of a matrix using the Jacobi SVD formula
-///
-////////////////////////////////////////////////////////////////////////////////////////////////
-template <typename _Matrix_Type_>
-_Matrix_Type_ pseudoInverse(const _Matrix_Type_ &a, double epsilon = std::numeric_limits<double>::epsilon()) {
- Eigen::JacobiSVD<_Matrix_Type_> svd(a, Eigen::ComputeThinU | Eigen::ComputeThinV);
- double tolerance =
- epsilon * static_cast<double>(std::max(a.cols(), a.rows())) * svd.singularValues().array().abs()(0);
- return svd.matrixV() *
- (svd.singularValues().array().abs() > tolerance)
- .select(svd.singularValues().array().inverse(), 0)
- .matrix()
- .asDiagonal() *
- svd.matrixU().adjoint();
-}
-
#endif // QPWBC_H_INCLUDED
diff --git a/include/qrw/Types.h b/include/qrw/Types.h
index 8a536853704d08da24da9022d70dbe7d36219e86..a6cb074abf0a40a276c6ccbc02eb788027b7926b 100644
--- a/include/qrw/Types.h
+++ b/include/qrw/Types.h
@@ -30,6 +30,7 @@ using Matrix12 = Eigen::Matrix<double, 12, 12>;
using Matrix13 = Eigen::Matrix<double, 1, 3>;
using Matrix14 = Eigen::Matrix<double, 1, 4>;
using Matrix112 = Eigen::Matrix<double, 1, 12>;
+using Matrix118 = Eigen::Matrix<double, 1, 18>;
using Matrix34 = Eigen::Matrix<double, 3, 4>;
using Matrix43 = Eigen::Matrix<double, 4, 3>;
using Matrix64 = Eigen::Matrix<double, 6, 4>;
diff --git a/python/gepadd.cpp b/python/gepadd.cpp
index 89d28c3bf8444d77efb23c7cdb6a67099d37acbe..db8ad87e828bcf3a045e28b27af7892a33ea5b77 100644
--- a/python/gepadd.cpp
+++ b/python/gepadd.cpp
@@ -233,7 +233,7 @@ struct InvKinPythonVisitor : public bp::def_visitor<InvKinPythonVisitor<InvKin>>
.def("get_foot_id", &InvKin::get_foot_id, bp::args("i"), "Get food frame id.\n")
// Run InvKin from Python
- .def("run_InvKin", &InvKin::run_InvKin, bp::args("contacts", "pgoals", "vgoals", "agoals"),
+ .def("run_InvKin", &InvKin::run_InvKin, bp::args("contacts", "pgoals", "vgoals", "agoals", "x_cmd"),
"Run InvKin from Python.\n");
}
@@ -309,7 +309,8 @@ struct WbcWrapperPythonVisitor : public bp::def_visitor<WbcWrapperPythonVisitor<
.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");
+ .def("compute", &WbcWrapper::compute, bp::args("q", "dq", "f_cmd", "contacts", "pgoals", "vgoals",
+ "agoals", "q_mpc", "v_mpc"), "Run WbcWrapper from Python.\n");
}
static void expose()
diff --git a/scripts/Controller.py b/scripts/Controller.py
index d46c6b3d0c43636d5ac700e9668d54c9536d577d..f8a7af9d0df4fb9a9bd8c8de5537d64988f7c3b0 100644
--- a/scripts/Controller.py
+++ b/scripts/Controller.py
@@ -325,7 +325,8 @@ class Controller:
self.feet_p_cmd,
self.feet_v_cmd,
self.feet_a_cmd,
- self.q_filt_mpc[:, 0:1])
+ self.q_filt_mpc[:, 0:1],
+ self.h_v_filt_mpc[:, 0:1])
# Quantities sent to the control board
self.result.P = np.array(self.Kp_main.tolist() * 4)
diff --git a/src/InvKin.cpp b/src/InvKin.cpp
index 0e866c356cefea44f851fbe65cd7a16bd1085a09..511ac6355ac13467b64d9cdb2ca3a53366dcf72d 100644
--- a/src/InvKin.cpp
+++ b/src/InvKin.cpp
@@ -2,9 +2,9 @@
InvKin::InvKin()
: invJ(Matrix12::Zero()),
- acc(Matrix112::Zero()),
- x_err(Matrix112::Zero()),
- dx_r(Matrix112::Zero()),
+ acc(Matrix118::Zero()),
+ x_err(Matrix118::Zero()),
+ dx_r(Matrix118::Zero()),
pfeet_err(Matrix43::Zero()),
vfeet_ref(Matrix43::Zero()),
afeet(Matrix43::Zero()),
@@ -12,12 +12,28 @@ InvKin::InvKin()
vf_(Matrix43::Zero()),
wf_(Matrix43::Zero()),
af_(Matrix43::Zero()),
- Jf_(Matrix12::Zero()),
- Jf_tmp_(Eigen::Matrix<double, 6, 12>::Zero()),
- ddq_cmd_(Vector12::Zero()),
- dq_cmd_(Vector12::Zero()),
- q_cmd_(Vector12::Zero()),
- q_step_(Vector12::Zero()) {}
+ Jf_(Eigen::Matrix<double, 12, 18>::Zero()),
+ Jf_tmp_(Eigen::Matrix<double, 6, 18>::Zero()),
+ posb_(Vector3::Zero()),
+ posb_ref_(Vector3::Zero()),
+ posb_err_(Vector3::Zero()),
+ rotb_(Matrix3::Identity()),
+ rotb_ref_(Matrix3::Identity()),
+ rotb_err_(Vector3::Zero()),
+ vb_(Vector3::Zero()),
+ vb_ref_(Vector3::Zero()),
+ wb_(Vector3::Zero()),
+ wb_ref_(Vector3::Zero()),
+ ab_(Vector6::Zero()),
+ abasis(Vector3::Zero()),
+ awbasis(Vector3::Zero()),
+ Jb_(Eigen::Matrix<double, 6, 18>::Zero()),
+ J_(Eigen::Matrix<double, 18, 18>::Zero()),
+ invJ_(Eigen::Matrix<double, 18, 18>::Zero()),
+ ddq_cmd_(Vector18::Zero()),
+ dq_cmd_(Vector18::Zero()),
+ q_cmd_(Vector19::Zero()),
+ q_step_(Vector18::Zero()) {}
void InvKin::initialize(Params& params) {
// Params store parameters
@@ -28,7 +44,7 @@ void InvKin::initialize(Params& params) {
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, model_, false);
+ pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(), model_, false);
// Construct data from model
data_ = pinocchio::Data(model_);
@@ -41,6 +57,16 @@ void InvKin::initialize(Params& params) {
foot_ids_[1] = static_cast<int>(model_.getFrameId("FR_FOOT"));
foot_ids_[2] = static_cast<int>(model_.getFrameId("HL_FOOT"));
foot_ids_[3] = static_cast<int>(model_.getFrameId("HR_FOOT"));
+
+ // Get base ID
+ base_id_ = static_cast<int>(model_.getFrameId("base_link")); // from long uint to int
+
+ // Set task gains
+ Kp_base_position << 0.0, 0.0, 0.0;
+ Kd_base_position << 0.0, 0.0, 0.0;
+ Kp_base_orientation << 0.0, 0.0, 0.0;
+ Kd_base_orientation << 0.0, 0.0, 0.0;
+
}
void InvKin::refreshAndCompute(Matrix14 const& contacts, Matrix43 const& pgoals, Matrix43 const& vgoals,
@@ -57,31 +83,71 @@ void InvKin::refreshAndCompute(Matrix14 const& contacts, Matrix43 const& pgoals,
}*/
afeet.row(i) -= af_.row(i) + (wf_.row(i)).cross(vf_.row(i)); // Drift
}
+ J_.block(6, 0, 12, 18) = Jf_.block(0, 0, 12, 18);
+
+ // Process base position
+ posb_err_ = posb_ref_ - posb_;
+ abasis = Kp_base_position.cwiseProduct(posb_err_) - Kd_base_position.cwiseProduct(vb_ - vb_ref_);
+ abasis -= ab_.head(3) + wb_.cross(vb_);
+
+ // Process base orientation
+ rotb_err_ = -rotb_ref_ * pinocchio::log3(rotb_ref_.transpose() * rotb_);
+ awbasis = Kp_base_orientation.cwiseProduct(rotb_err_) - Kd_base_orientation.cwiseProduct(wb_ - wb_ref_);
+ awbasis -= ab_.tail(3);
+
+ J_.block(0, 0, 6, 18) = Jb_.block(0, 0, 6, 18); // Position and orientation
+
+ acc.block(0, 0, 1, 3) = abasis.transpose();
+ acc.block(0, 3, 1, 3) = awbasis.transpose();
+ for (int i = 0; i < 4; i++) {
+ acc.block(0, 6+3*i, 1, 3) = afeet.row(i);
+ }
+
+ x_err.block(0, 0, 1, 3) = posb_err_.transpose();
+ x_err.block(0, 3, 1, 3) = rotb_err_.transpose();
+ for (int i = 0; i < 4; i++) {
+ x_err.block(0, 6+3*i, 1, 3) = pfeet_err.row(i);
+ }
+
+ dx_r.block(0, 0, 1, 3) = vb_ref_.transpose();
+ dx_r.block(0, 3, 1, 3) = wb_ref_.transpose();
+ for (int i = 0; i < 4; i++) {
+ dx_r.block(0, 6+3*i, 1, 3) = vfeet_ref.row(i);
+ }
+
+ // Jacobian inversion using damped pseudo inverse
+ invJ_ = pseudoInverse(J_);
// Store data and invert the Jacobian
+ /*
for (int i = 0; i < 4; i++) {
acc.block(0, 3 * i, 1, 3) = afeet.row(i);
x_err.block(0, 3 * i, 1, 3) = pfeet_err.row(i);
dx_r.block(0, 3 * i, 1, 3) = vfeet_ref.row(i);
invJ.block(3 * i, 3 * i, 3, 3) = Jf_.block(3 * i, 3 * i, 3, 3).inverse();
}
+ */
// Once Jacobian has been inverted we can get command accelerations, velocities and positions
- ddq_cmd_ = invJ * acc.transpose();
- dq_cmd_ = invJ * dx_r.transpose();
- q_step_ = invJ * x_err.transpose(); // Not a position but a step in position
+ ddq_cmd_ = invJ_ * acc.transpose();
+ dq_cmd_ = invJ_ * dx_r.transpose();
+ q_step_ = invJ_ * x_err.transpose(); // Not a position but a step in position
- /*
- std::cout << "J" << std::endl << Jf << std::endl;
- std::cout << "invJ" << std::endl << invJ << std::endl;
+ /*std::cout << "J" << std::endl << J_ << std::endl;
+ std::cout << "invJ" << std::endl << invJ_ << std::endl;
std::cout << "acc" << std::endl << acc << std::endl;
- std::cout << "q_step" << std::endl << q_step << std::endl;
- std::cout << "dq_cmd" << std::endl << dq_cmd << std::endl;
- */
+ std::cout << "dx_r" << std::endl << dx_r << std::endl;
+ std::cout << "x_err" << std::endl << x_err << std::endl;
+ std::cout << "ddq_cmd" << std::endl << ddq_cmd_ << std::endl;
+ std::cout << "dq_cmd" << std::endl << dq_cmd_ << std::endl;
+ std::cout << "q_step" << std::endl << q_step_ << std::endl;*/
+
}
void InvKin::run_InvKin(VectorN const& q, VectorN const& dq, MatrixN const& contacts, MatrixN const& pgoals,
- MatrixN const& vgoals, MatrixN const& agoals) {
+ MatrixN const& vgoals, MatrixN const& agoals, MatrixN const& x_cmd) {
+ // std::cout << "run invkin q: " << q << std::endl;
+
// Update model and data of the robot
pinocchio::forwardKinematics(model_, data_, q, dq, VectorN::Zero(model_.nv));
pinocchio::computeJointJacobians(model_, data_);
@@ -98,13 +164,45 @@ void InvKin::run_InvKin(VectorN const& q, VectorN const& dq, MatrixN const& cont
Jf_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_, data_, idx, pinocchio::LOCAL_WORLD_ALIGNED, Jf_tmp_);
- Jf_.block(3 * i, 0, 3, 12) = Jf_tmp_.block(0, 0, 3, 12);
+ Jf_.block(3 * i, 0, 3, 18) = Jf_tmp_.block(0, 0, 3, 18);
}
+ // Update position and velocity of the base
+ posb_ = data_.oMf[base_id_].translation(); // Position
+ rotb_ = data_.oMf[base_id_].rotation(); // Orientation
+ pinocchio::Motion nu = pinocchio::getFrameVelocity(model_, data_, base_id_, pinocchio::LOCAL_WORLD_ALIGNED);
+ vb_ = nu.linear(); // Linear velocity
+ wb_ = nu.angular(); // Angular velocity
+ pinocchio::Motion acc = pinocchio::getFrameAcceleration(model_, data_, base_id_, pinocchio::LOCAL_WORLD_ALIGNED);
+ ab_.head(3) = acc.linear(); // Linear acceleration
+ ab_.tail(3) = acc.angular(); // Angular acceleration
+ pinocchio::getFrameJacobian(model_, data_, base_id_, pinocchio::LOCAL_WORLD_ALIGNED, Jb_);
+
+ // Update reference position and reference velocity of the base
+ posb_ref_ = x_cmd.block(0, 0, 3, 1); // Ref position
+ rotb_ref_ = pinocchio::rpy::rpyToMatrix(x_cmd(3, 0), x_cmd(4, 0), x_cmd(5, 0)); // Ref orientation
+ vb_ref_ = x_cmd.block(6, 0, 3, 1); // Ref linear velocity
+ wb_ref_ = x_cmd.block(9, 0, 3, 1); // Ref angular velocity
+
+ /*std::cout << "----" << std::endl;
+ std::cout << posf_ << std::endl;
+ std::cout << Jf_ << std::endl;
+ std::cout << posb_ << std::endl;
+ std::cout << rotb_ << std::endl;
+ std::cout << vb_ << std::endl;
+ std::cout << wb_ << std::endl;
+ std::cout << ab_ << std::endl;*/
+
// IK output for accelerations of actuators (stored in ddq_cmd_)
// IK output for velocities of actuators (stored in dq_cmd_)
refreshAndCompute(contacts, pgoals, vgoals, agoals);
// IK output for positions of actuators
- q_cmd_ = q + q_step_;
+ q_cmd_ = pinocchio::integrate(model_, q, q_step_);
+
+ /*std::cout << "q: " << q << std::endl;
+ std::cout << "q_step_: " << q_step_ << std::endl;
+ std::cout << " q_cmd_: " << q_cmd_ << std::endl;*/
+
+
}
diff --git a/src/QPWBC.cpp b/src/QPWBC.cpp
index 496e7eed87bbf9bd1ca20e7cf1abfb97a0cf8c39..73fc634f113f8b906cd74a6cc7b463682a72ee4e 100644
--- a/src/QPWBC.cpp
+++ b/src/QPWBC.cpp
@@ -516,7 +516,7 @@ void WbcWrapper::initialize(Params ¶ms) {
}
void WbcWrapper::compute(VectorN const &q, VectorN const &dq, VectorN const &f_cmd, MatrixN const &contacts,
- MatrixN const &pgoals, MatrixN const &vgoals, MatrixN const &agoals, VectorN const &q_mpc) {
+ MatrixN const &pgoals, MatrixN const &vgoals, MatrixN const &agoals, VectorN const &q_mpc, VectorN const &v_mpc) {
if (f_cmd.rows() != 12) {
throw std::runtime_error("f_cmd should be a vector of size 12");
@@ -570,8 +570,17 @@ void WbcWrapper::compute(VectorN const &q, VectorN const &dq, VectorN const &f_c
// 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();
+ Vector19 q_IK = Vector19::Zero();
+ q_IK.block(3, 0, 4, 1) = q_mpc_.block(3, 0, 4, 1);
+ q_IK.tail(12) = q.tail(12);
+ Vector18 dq_IK = Vector18::Zero();
+ dq_IK.tail(12) = dq.tail(12);
+
+ /*std::cout << q.transpose() << std::endl;
+ std::cout << q_IK.transpose() << std::endl;*/
+
+ 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;
@@ -592,6 +601,7 @@ void WbcWrapper::compute(VectorN const &q, VectorN const &dq, VectorN const &f_c
}
}
+ /*
int cpt = 0;
double ax = 0.0;
double ay = 0.0;
@@ -607,6 +617,7 @@ void WbcWrapper::compute(VectorN const &q, VectorN const &dq, VectorN const &f_c
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.
@@ -641,13 +652,26 @@ void WbcWrapper::compute(VectorN const &q, VectorN const &dq, VectorN const &f_c
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;*/
+ std::cout << f_with_delta_ << std::endl;
+ 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().transpose() * f_with_delta_;
+ tau_ff_ = data_.tau.tail(12) - invkin_->get_Jf().block(0, 6, 12, 12).transpose() * f_with_delta_;
// Retrieve desired positions and velocities
- vdes_ = invkin_->get_dq_cmd();
- qdes_ = invkin_->get_q_cmd();
+ vdes_ = invkin_->get_dq_cmd().tail(12);
+
+ // std::cout << "GET:" << invkin_->get_q_cmd() << std::endl;
+
+ qdes_ = invkin_->get_q_cmd().tail(12);
+
+ /*std::cout << vdes_.transpose() << std::endl;
+ std::cout << qdes_.transpose() << std::endl;*/
+
+ /*std::cout << "----" << std::endl;
+ std::cout << qdes_.transpose() << std::endl;
+ std::cout << vdes_.transpose() << std::endl;
+ std::cout << tau_ff_.transpose() << std::endl;*/
// Increment log counter
k_log_++;