From 8dcb9d2fa2d20dc1842318a303da722224bbda35 Mon Sep 17 00:00:00 2001
From: Taylor Howell <taylorhowell@google.com>
Date: Wed, 6 Nov 2024 03:02:50 -0800
Subject: [PATCH] Add a parameter to control the arm posture during foot/hand
 stand in quadruped flat task.

PiperOrigin-RevId: 693656096
Change-Id: Ibfed9043aae12ffa27a1a317e4e10ec119e2732a
---
 mjpc/tasks/quadruped/quadruped.cc  | 22 ++++++++++++++--------
 mjpc/tasks/quadruped/quadruped.h   |  1 +
 mjpc/tasks/quadruped/task_flat.xml |  1 +
 3 files changed, 16 insertions(+), 8 deletions(-)

diff --git a/mjpc/tasks/quadruped/quadruped.cc b/mjpc/tasks/quadruped/quadruped.cc
index fad38ad19..a28f55e0d 100644
--- a/mjpc/tasks/quadruped/quadruped.cc
+++ b/mjpc/tasks/quadruped/quadruped.cc
@@ -182,16 +182,21 @@ void QuadrupedFlat::ResidualFn::Residual(const mjModel* model,
   if (current_mode_ == kModeBiped) {
     // loosen the "hands" in Biped mode
     bool handstand = ReinterpretAsInt(parameters_[biped_type_param_id_]);
+    double arm_posture = parameters_[arm_posture_param_id_];
     if (handstand) {
-      residual[counter + 4] *= 0.03;
-      residual[counter + 5] *= 0.03;
-      residual[counter + 10] *= 0.03;
-      residual[counter + 11] *= 0.03;
+      residual[counter + 6] *= arm_posture;
+      residual[counter + 7] *= arm_posture;
+      residual[counter + 8] *= arm_posture;
+      residual[counter + 9] *= arm_posture;
+      residual[counter + 10] *= arm_posture;
+      residual[counter + 11] *= arm_posture;
     } else {
-      residual[counter + 1] *= 0.03;
-      residual[counter + 2] *= 0.03;
-      residual[counter + 7] *= 0.03;
-      residual[counter + 8] *= 0.03;
+      residual[counter + 0] *= arm_posture;
+      residual[counter + 1] *= arm_posture;
+      residual[counter + 2] *= arm_posture;
+      residual[counter + 3] *= arm_posture;
+      residual[counter + 4] *= arm_posture;
+      residual[counter + 5] *= arm_posture;
     }
   }
   counter += model->nu;
@@ -521,6 +526,7 @@ void QuadrupedFlat::ResetLocked(const mjModel* model) {
   residual_.cadence_param_id_ = ParameterIndex(model, "Cadence");
   residual_.amplitude_param_id_ = ParameterIndex(model, "Amplitude");
   residual_.duty_param_id_ = ParameterIndex(model, "Duty ratio");
+  residual_.arm_posture_param_id_ = ParameterIndex(model, "Arm posture");
   residual_.balance_cost_id_ = CostTermByName(model, "Balance");
   residual_.upright_cost_id_ = CostTermByName(model, "Upright");
   residual_.height_cost_id_ = CostTermByName(model, "Height");
diff --git a/mjpc/tasks/quadruped/quadruped.h b/mjpc/tasks/quadruped/quadruped.h
index 5fb21f28e..62f552dd1 100644
--- a/mjpc/tasks/quadruped/quadruped.h
+++ b/mjpc/tasks/quadruped/quadruped.h
@@ -203,6 +203,7 @@ class QuadrupedFlat : public Task {
     int cadence_param_id_     = -1;
     int amplitude_param_id_   = -1;
     int duty_param_id_        = -1;
+    int arm_posture_param_id_ = -1;
     int upright_cost_id_      = -1;
     int balance_cost_id_      = -1;
     int height_cost_id_       = -1;
diff --git a/mjpc/tasks/quadruped/task_flat.xml b/mjpc/tasks/quadruped/task_flat.xml
index 7826fa4b8..a53f7334a 100644
--- a/mjpc/tasks/quadruped/task_flat.xml
+++ b/mjpc/tasks/quadruped/task_flat.xml
@@ -29,6 +29,7 @@
     <numeric name="residual_select_Biped type" data="0"/>
     <text name="residual_list_Biped type" data="Foot Stand|Hand Stand"/>
     <numeric name="residual_Heading" data="0 -3.14 3.14" />
+    <numeric name="residual_Arm posture" data=".03 0 1"/>
 
     <!-- estimator -->
     <numeric name="estimator" data="0" />