From c9d4f14359da7251e746b9f12327ccf45369798e Mon Sep 17 00:00:00 2001
From: odri <odri@furano.laas.fr>
Date: Wed, 25 Aug 2021 16:23:10 +0200
Subject: [PATCH] Tracking of feet in IK now always active, even during stance
phases
---
src/InvKin.cpp | 4 ++--
1 file changed, 2 insertions(+), 2 deletions(-)
diff --git a/src/InvKin.cpp b/src/InvKin.cpp
index 91ab49b8..0e866c35 100644
--- a/src/InvKin.cpp
+++ b/src/InvKin.cpp
@@ -52,9 +52,9 @@ void InvKin::refreshAndCompute(Matrix14 const& contacts, Matrix43 const& pgoals,
afeet.row(i) = +params_->Kp_flyingfeet * pfeet_err.row(i) - params_->Kd_flyingfeet * (vf_.row(i) - vgoals.row(i)) +
agoals.row(i);
- if (contacts(0, i) == 1.0) {
+ /*if (contacts(0, i) == 1.0) {
afeet.row(i).setZero(); // Set to 0.0 to disable position/velocity control of feet in contact
- }
+ }*/
afeet.row(i) -= af_.row(i) + (wf_.row(i)).cross(vf_.row(i)); // Drift
}
--
GitLab