From 7244557fb91f2c725e29a39c72d4fe6274c16d01 Mon Sep 17 00:00:00 2001
From: paleziart <paleziart@laas.fr>
Date: Wed, 7 Sep 2022 18:44:25 +0200
Subject: [PATCH] Tweak formula for step height depending on contact height
---
src/FootTrajectoryGenerator.cpp | 3 ++-
1 file changed, 2 insertions(+), 1 deletion(-)
diff --git a/src/FootTrajectoryGenerator.cpp b/src/FootTrajectoryGenerator.cpp
index ee5fa8d9..7b8b4898 100644
--- a/src/FootTrajectoryGenerator.cpp
+++ b/src/FootTrajectoryGenerator.cpp
@@ -137,7 +137,8 @@ void FootTrajectoryGenerator::updateFootPosition(int const j, Vector3 const &tar
// Coefficients for z (deterministic)
double Tz = t_swing[j];
if (t0s[j] == 0) {
- stepHeight_[j] = maxHeight_ * (1.0 - 0.5 * targetFootstep_(2, j) / maxHeight_);
+ stepHeight_[j] = maxHeight_ - std::max(0.0, targetFootstep_(2, j)); //* (1.0 - 0.5 * std::max(0.0, targetFootstep_(2, j)) / maxHeight_);
+ // stepHeight_[j] = maxHeight_ * (1.0 - 0.5 * targetFootstep_(2, j) / maxHeight_);
}
Vector3 Az; // z trajectory is split in two halfs (before/after apex point)
--
GitLab