diff --git a/include/qrw/InvKin.hpp b/include/qrw/InvKin.hpp
index 37f6ed50939688e4c4770243662799b15590d548..93f8ca15c132a3e613b69cd4e49f47decbdfdca4 100644
--- a/include/qrw/InvKin.hpp
+++ b/include/qrw/InvKin.hpp
@@ -153,9 +153,10 @@ class InvKin {
/// \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);
+Eigen::MatrixXd pseudoInverse(const _Matrix_Type_ &a, double epsilon = std::numeric_limits<double>::epsilon()) {
+ Eigen::JacobiSVD<Eigen::MatrixXd> 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() *
diff --git a/include/qrw/QPWBC.hpp b/include/qrw/QPWBC.hpp
index 8f251bbf541848819b661f2eadaff2396fe3146d..95d94cf7dea68c141c30f9a0effe217be21ff74e 100644
--- a/include/qrw/QPWBC.hpp
+++ b/include/qrw/QPWBC.hpp
@@ -290,6 +290,7 @@ class WbcWrapper {
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 &xgoals);
+ VectorN get_bdes() { return bdes_; }
VectorN get_qdes() { return qdes_; }
VectorN get_vdes() { return vdes_; }
VectorN get_tau_ff() { return tau_ff_; }
@@ -314,6 +315,7 @@ class WbcWrapper {
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
+ Vector7 bdes_; // Desired base positions
Vector12 qdes_; // Desired actuator positions
Vector12 vdes_; // Desired actuator velocities
Vector12 tau_ff_; // Desired actuator torques (feedforward)
diff --git a/python/gepadd.cpp b/python/gepadd.cpp
index 202841fde5e5ef5130aa03f605e01f76c0fe96d9..56ff1cf6105ad403ea399e8badef959a2395c985 100644
--- a/python/gepadd.cpp
+++ b/python/gepadd.cpp
@@ -290,10 +290,12 @@ struct WbcWrapperPythonVisitor : public bp::def_visitor<WbcWrapperPythonVisitor<
.def("initialize", &WbcWrapper::initialize, bp::args("params"), "Initialize WbcWrapper from Python.\n")
+ .def("get_bdes", &WbcWrapper::get_bdes, "Get bdes_.\n")
.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_readonly("bdes", &WbcWrapper::get_bdes)
.def_readonly("qdes", &WbcWrapper::get_qdes)
.def_readonly("vdes", &WbcWrapper::get_vdes)
.def_readonly("tau_ff", &WbcWrapper::get_tau_ff)
diff --git a/src/InvKin.cpp b/src/InvKin.cpp
index 47ac551da780a5213a8e90c3bcbeac375e3e94cb..69bddfe2bca1e8aae2bef25c8775b7fd8053c485 100644
--- a/src/InvKin.cpp
+++ b/src/InvKin.cpp
@@ -2,9 +2,9 @@
InvKin::InvKin()
: invJ(Matrix12::Zero()),
- acc(Matrix118::Zero()),
- x_err(Matrix118::Zero()),
- dx_r(Matrix118::Zero()),
+ acc(Eigen::Matrix<double, 1, 27>::Zero()),
+ x_err(Eigen::Matrix<double, 1, 27>::Zero()),
+ dx_r(Eigen::Matrix<double, 1, 27>::Zero()),
pfeet_err(Matrix43::Zero()),
vfeet_ref(Matrix43::Zero()),
afeet(Matrix43::Zero()),
@@ -28,8 +28,8 @@ InvKin::InvKin()
abasis(Vector3::Zero()),
awbasis(Vector3::Zero()),
Jb_(Eigen::Matrix<double, 6, 18>::Zero()),
- J_(Eigen::Matrix<double, 24, 18>::Zero()),
- invJ_(Eigen::Matrix<double, 18, 24>::Zero()),
+ J_(Eigen::Matrix<double, 27, 18>::Zero()),
+ invJ_(Eigen::Matrix<double, 18, 27>::Zero()),
ddq_cmd_(Vector18::Zero()),
dq_cmd_(Vector18::Zero()),
q_cmd_(Vector19::Zero()),
@@ -73,9 +73,9 @@ void InvKin::refreshAndCompute(Matrix14 const& contacts, Matrix43 const& pgoals,
Matrix43 const& agoals) {
/*std::cout << "pgoals:" << std::endl;
- std::cout << pgoals.row(0) << std::endl;
+ std::cout << pgoals << std::endl;
std::cout << "posf_" << std::endl;
- std::cout << posf_.row(0) << std::endl;*/
+ std::cout << posf_ << std::endl;*/
// Acceleration references for the feet tracking task
for (int i = 0; i < 4; i++) {
@@ -125,41 +125,86 @@ void InvKin::refreshAndCompute(Matrix14 const& contacts, Matrix43 const& pgoals,
// Gather all acceleration references in a single vector
// Feet tracking task
for (int i = 0; i < 4; i++) {
- acc.block(0, 3*i, 1, 3) = afeet.row(i);
+ if (contacts(0, i) == 1.0) // Feet in contact
+ {
+ acc.block(0, 3*i, 1, 3).setZero();
+ }
+ else // Feet not in contact
+ {
+ acc.block(0, 3*i, 1, 3) = afeet.row(i);
+ }
}
// Base orientation task
acc.block(0, 12, 1, 3) = awbasis.transpose();
// Base / feet position task
for (int i = 0; i < 4; i++) {
- acc.block(0, 15+3*i, 1, 3) = abasis.transpose() - afeet.row(i);
+ if (contacts(0, i) == 1.0) // Feet in contact
+ {
+ acc.block(0, 15+3*i, 1, 3) = abasis.transpose() - afeet.row(i);
+ }
+ else // Feet not in contact
+ {
+ acc.block(0, 15+3*i, 1, 3).setZero();
+ }
}
// Gather all task errors in a single vector
// Feet tracking task
for (int i = 0; i < 4; i++) {
- x_err.block(0, 3*i, 1, 3) = pfeet_err.row(i);
+ if (contacts(0, i) == 1.0) // Feet in contact
+ {
+ x_err.block(0, 3*i, 1, 3).setZero();
+ }
+ else // Feet not in contact
+ {
+ x_err.block(0, 3*i, 1, 3) = pfeet_err.row(i);
+ }
}
// Base orientation task
x_err.block(0, 12, 1, 3) = rotb_err_.transpose();
// Base / feet position task
for (int i = 0; i < 4; i++) {
- x_err.block(0, 15+3*i, 1, 3) = posb_err_.transpose() - pfeet_err.row(i);
+ if (contacts(0, i) == 1.0) // Feet in contact
+ {
+ x_err.block(0, 15+3*i, 1, 3) = posb_err_.transpose() - pfeet_err.row(i);
+ }
+ else // Feet not in contact
+ {
+ x_err.block(0, 15+3*i, 1, 3).setZero();
+ }
}
// Gather all task velocity references in a single vector
// Feet tracking task
for (int i = 0; i < 4; i++) {
- dx_r.block(0, 3*i, 1, 3) = vfeet_ref.row(i);
+ if (contacts(0, i) == 1.0) // Feet in contact
+ {
+ dx_r.block(0, 3*i, 1, 3).setZero();
+ }
+ else // Feet not in contact
+ {
+ dx_r.block(0, 3*i, 1, 3) = vfeet_ref.row(i);
+ }
}
// Base orientation task
dx_r.block(0, 12, 1, 3) = wb_ref_.transpose();
// Base / feet position task
for (int i = 0; i < 4; i++) {
- dx_r.block(0, 15+3*i, 1, 3) = vb_ref_.transpose() - pfeet_err.row(i);
+ if (contacts(0, i) == 1.0) // Feet in contact
+ {
+ dx_r.block(0, 15+3*i, 1, 3) = vb_ref_.transpose() - vfeet_ref.row(i);
+ }
+ else // Feet not in contact
+ {
+ dx_r.block(0, 15+3*i, 1, 3).setZero();
+ }
}
// Jacobian inversion using damped pseudo inverse
invJ_ = pseudoInverse(J_);
+ /*Eigen::MatrixXd test = pseudoInverse(J_);
+ std::cout << test.rows() << std::endl;
+ std::cout << test.cols() << std::endl;*/
// Store data and invert the Jacobian
/*
@@ -244,15 +289,20 @@ void InvKin::run_InvKin(VectorN const& q, VectorN const& dq, MatrixN const& cont
// IK output for positions of actuators
q_cmd_ = pinocchio::integrate(model_, q, q_step_);
- /*pinocchio::forwardKinematics(model_, data_, q_cmd_, dq, VectorN::Zero(model_.nv));
+ /*pinocchio::forwardKinematics(model_, data_, q_cmd_, dq_cmd_, ddq_cmd_);
pinocchio::computeJointJacobians(model_, data_);
pinocchio::updateFramePlacements(model_, data_);
std::cout << "pos after step" << std::endl;
- std::cout << data_.oMf[foot_ids_[0]].translation() << std::endl;*/
+ std::cout << data_.oMf[foot_ids_[0]].translation() << std::endl;
+ std::cout << "vel after step" << std::endl;
+ std::cout << pinocchio::getFrameVelocity(model_, data_, foot_ids_[0], pinocchio::LOCAL_WORLD_ALIGNED).linear() << std::endl;
+ std::cout << "acc after step" << std::endl;
+ std::cout << pinocchio::getFrameAcceleration(model_, data_, foot_ids_[0], pinocchio::LOCAL_WORLD_ALIGNED).linear() << std::endl;*/
/*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 262546b5cd27f61da23b4f436181d1996878d77a..993de3884eb9ad0a945dfdae70a483e6a8202366 100644
--- a/src/QPWBC.cpp
+++ b/src/QPWBC.cpp
@@ -442,6 +442,7 @@ WbcWrapper::WbcWrapper()
: M_(Eigen::Matrix<double, 18, 18>::Zero()),
Jc_(Eigen::Matrix<double, 12, 6>::Zero()),
k_since_contact_(Eigen::Matrix<double, 1, 4>::Zero()),
+ bdes_(Vector7::Zero()),
qdes_(Vector12::Zero()),
vdes_(Vector12::Zero()),
tau_ff_(Vector12::Zero()),
@@ -512,8 +513,8 @@ void WbcWrapper::compute(VectorN const &q, VectorN const &dq, VectorN const &f_c
log_feet_acc_target = agoals;
// Retrieve configuration data
- q_wbc_(2, 0) = q(2, 0); // Height
- q_wbc_.block(3, 0, 4, 1) = pinocchio::SE3::Quaternion(pinocchio::rpy::rpyToMatrix(q(3, 0), q(4, 0), 0.0)).coeffs(); // Roll, Pitch
+ q_wbc_.head(3) = q.head(3);
+ q_wbc_.block(3, 0, 4, 1) = pinocchio::SE3::Quaternion(pinocchio::rpy::rpyToMatrix(q(3, 0), q(4, 0), q(5, 0))).coeffs(); // Roll, Pitch
q_wbc_.tail(12) = q.tail(12); // Encoders
// Retrieve velocity data
@@ -581,6 +582,7 @@ void WbcWrapper::compute(VectorN const &q, VectorN const &dq, VectorN const &f_c
// std::cout << "GET:" << invkin_->get_q_cmd() << std::endl;
qdes_ = invkin_->get_q_cmd().tail(12);
+ bdes_ = invkin_->get_q_cmd().head(7);
/*std::cout << vdes_.transpose() << std::endl;
std::cout << qdes_.transpose() << std::endl;*/