From e4b42f85e6529c26c40af18f7d291ccdd73eab3e Mon Sep 17 00:00:00 2001
From: Joe DiPrima <epilectrik@gmail.com>
Date: Tue, 24 Feb 2026 10:33:56 -0600
Subject: [PATCH] Add direct knee offset for standing height control
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Planner height input doesn't produce visible knee bend in IDLE mode.
Instead, add a direct offset to both knee motor targets (+5 deg default).
Runtime-adjustable via keys 3/4 (±1 deg). This shifts the standing
equilibrium lower without changing the NN's learned dynamics.

Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
---
 .../input_interface/gamepad_manager.hpp        | 18 ++++++++++++++++++
 .../src/g1_deploy_onnx_ref.cpp                 |  6 ++++++
 2 files changed, 24 insertions(+)

diff --git a/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/include/input_interface/gamepad_manager.hpp b/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/include/input_interface/gamepad_manager.hpp
index 2a47d7d..c3d8999 100644
--- a/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/include/input_interface/gamepad_manager.hpp
+++ b/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/include/input_interface/gamepad_manager.hpp
@@ -58,6 +58,10 @@ inline std::atomic<double> g_waist_pitch_offset_deg{0.0};
 /// Lowered from planner default 0.789m to add knee bend.
 inline std::atomic<double> g_standing_height_m{0.72};
 
+/// Runtime-adjustable knee offset (degrees). Keys 3/4 to adjust via tmux.
+/// Positive = more bent knees = lower stance. Applied to both knees equally.
+inline std::atomic<double> g_knee_offset_deg{5.0};
+
 #if HAS_ROS2
 #include "ros2_input_handler.hpp"
 #endif
@@ -175,6 +179,20 @@ class GamepadManager : public InputInterface {
             is_manager_key = true;
             break;
           }
+          case '3': {
+            double val = g_knee_offset_deg.load() - 1.0;
+            g_knee_offset_deg.store(val);
+            std::cout << "[KNEE] Offset: " << val << " deg" << std::endl;
+            is_manager_key = true;
+            break;
+          }
+          case '4': {
+            double val = g_knee_offset_deg.load() + 1.0;
+            g_knee_offset_deg.store(val);
+            std::cout << "[KNEE] Offset: " << val << " deg" << std::endl;
+            is_manager_key = true;
+            break;
+          }
         }
 
         if (!is_manager_key && current_) {
diff --git a/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/src/g1_deploy_onnx_ref.cpp b/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/src/g1_deploy_onnx_ref.cpp
index 1144577..4740329 100644
--- a/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/src/g1_deploy_onnx_ref.cpp
+++ b/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/src/g1_deploy_onnx_ref.cpp
@@ -2841,6 +2841,12 @@ class G1Deploy {
       if (waist_offset_rad != 0.0) {
         motor_command_tmp.q_target.at(14) += static_cast<float>(waist_offset_rad);
       }
+      // Optional knee offset (keys 3/4). Default +5°. Positive = more bent.
+      double knee_offset_rad = g_knee_offset_deg.load() * M_PI / 180.0;
+      if (knee_offset_rad != 0.0) {
+        motor_command_tmp.q_target.at(3) += static_cast<float>(knee_offset_rad);   // LeftKnee
+        motor_command_tmp.q_target.at(9) += static_cast<float>(knee_offset_rad);   // RightKnee
+      }
 
       // Debug: log raw NN actions + measured positions for waist joints every 1s
       static int waist_dbg_ctr = 0;