From 16751206132dd8125d67586c6984b00e600dcfd4 Mon Sep 17 00:00:00 2001
From: Florian Vahl <7vahl@informatik.uni-hamburg.de>
Date: Thu, 28 Nov 2024 18:05:37 +0100
Subject: [PATCH 1/2] Fix foot step placement during rotation

---
 .../bitbots_quintic_walk/src/walk_engine.cpp  | 19 +++++++++++++++----
 1 file changed, 15 insertions(+), 4 deletions(-)

diff --git a/bitbots_motion/bitbots_quintic_walk/src/walk_engine.cpp b/bitbots_motion/bitbots_quintic_walk/src/walk_engine.cpp
index 6cc63a138..331dcc627 100644
--- a/bitbots_motion/bitbots_quintic_walk/src/walk_engine.cpp
+++ b/bitbots_motion/bitbots_quintic_walk/src/walk_engine.cpp
@@ -858,12 +858,23 @@ void WalkEngine::stepFromOrders(const std::vector<double>& linear_orders, double
   // No change in forward step and upward step
   tmp_diff.getOrigin()[0] = linear_orders[0];
   tmp_diff.getOrigin()[2] = linear_orders[2];
+
   // Add lateral foot offset
-  if (is_left_support_foot_) {
-    tmp_diff.getOrigin()[1] = config_.foot_distance;
-  } else {
-    tmp_diff.getOrigin()[1] = -1 * config_.foot_distance;
+  // This is normally just the foot distance, but if we turn we need to also move the foot forward/backward
+  geometry_msgs::msg::Vector3 foot_offset;
+  foot_offset.x = -sin(angular_z / 2) * config_.foot_distance;
+  foot_offset.y = cos(angular_z / 2) * config_.foot_distance;
+
+  // Invert lateral offset for right foot
+  if (!is_left_support_foot_) {
+    foot_offset.x *= -1;
+    foot_offset.y *= -1;
   }
+
+  // Add foot offset to step diff
+  tmp_diff.getOrigin()[0] += foot_offset.x;
+  tmp_diff.getOrigin()[1] += foot_offset.y;
+
   // Allow lateral step only on external foot
   //(internal foot will return to zero pose)
   if ((is_left_support_foot_ && linear_orders[1] > 0.0) || (!is_left_support_foot_ && linear_orders[1] < 0.0)) {

From da4575d0758b6d348c020d8740c2a3b00db703d0 Mon Sep 17 00:00:00 2001
From: Florian Vahl <git@flova.de>
Date: Thu, 12 Dec 2024 10:49:22 +0100
Subject: [PATCH 2/2] Fix eol iron install (remove after upgrade)

---
 Makefile | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Makefile b/Makefile
index 48dc80295..375fe05b5 100644
--- a/Makefile
+++ b/Makefile
@@ -90,7 +90,7 @@ rosdep:
 	# Initialize rosdep if not already done
 	[ -f /etc/ros/rosdep/sources.list.d/20-default.list ] || sudo rosdep init
 	# Update rosdep and install dependencies from meta directory
-	rosdep update
+	rosdep update --include-eol-distros
 	rosdep install --from-paths . --ignore-src --rosdistro iron -y
 
 status: