From 396e43537131e0fcad54407680d85a8fed999c01 Mon Sep 17 00:00:00 2001
From: Ali K <ali@kscale.dev>
Date: Fri, 3 Jan 2025 23:45:33 -0800
Subject: [PATCH] add standing config

---
 sim/envs/__init__.py               |  3 ++-
 sim/envs/humanoids/zbot2_config.py | 42 +++++++++++++++++++++++++++++-
 2 files changed, 43 insertions(+), 2 deletions(-)

diff --git a/sim/envs/__init__.py b/sim/envs/__init__.py
index 6fedd0f8..016df468 100755
--- a/sim/envs/__init__.py
+++ b/sim/envs/__init__.py
@@ -18,7 +18,7 @@
 from sim.envs.humanoids.h1_env import H1FreeEnv
 from sim.envs.humanoids.xbot_config import XBotCfg, XBotCfgPPO
 from sim.envs.humanoids.xbot_env import XBotLFreeEnv
-from sim.envs.humanoids.zbot2_config import ZBot2Cfg, ZBot2CfgPPO
+from sim.envs.humanoids.zbot2_config import ZBot2Cfg, ZBot2CfgPPO, ZBot2StandingCfg
 from sim.envs.humanoids.zbot2_env import ZBot2Env
 from sim.envs.humanoids.zeroth_config import ZerothCfg, ZerothCfgPPO
 from sim.envs.humanoids.zeroth_env import ZerothEnv
@@ -32,4 +32,5 @@
 task_registry.register("g1", G1FreeEnv, G1Cfg(), G1CfgPPO())
 task_registry.register("XBotL_free", XBotLFreeEnv, XBotCfg(), XBotCfgPPO())
 task_registry.register("zbot2", ZBot2Env, ZBot2Cfg(), ZBot2CfgPPO())
+task_registry.register("zbot2_standing", ZBot2Env, ZBot2StandingCfg(), ZBot2CfgPPO())
 task_registry.register("zeroth", ZerothEnv, ZerothCfg(), ZerothCfgPPO())
diff --git a/sim/envs/humanoids/zbot2_config.py b/sim/envs/humanoids/zbot2_config.py
index bd947b87..b2f8050c 100644
--- a/sim/envs/humanoids/zbot2_config.py
+++ b/sim/envs/humanoids/zbot2_config.py
@@ -40,7 +40,7 @@ class asset(LeggedRobotCfg.asset):
         foot_name = ["FOOT", "FOOT_1"]
         knee_name = ["WJ-DP00-0002-FK-AP-020_7_8", "SJ-WK00-0023BOTTOMCASE_12_13"]
 
-        termination_height = 0.05
+        termination_height = 0.1
         default_feet_height = 0.01
 
         penalize_contacts_on = []
@@ -205,6 +205,46 @@ class viewer:
         lookat = [0, -2, 0]
 
 
+class ZBot2StandingCfg(ZBot2Cfg):
+    """Standing configuration for the ZBot2 humanoid robot."""
+
+    class init_state(LeggedRobotCfg.init_state):
+        # Use the same or slightly higher base position if you'd like a different "standing" height
+        pos = [0.0, 0.0, Robot.height]
+        rot = Robot.rotation
+
+        # Start with all zeros
+        default_joint_angles = {k: 0.0 for k in Robot.all_joints()}
+
+        # Overwrite with the default standing angles from Robot
+        default_positions = Robot.default_standing()
+        for joint in default_positions:
+            default_joint_angles[joint] = default_positions[joint]
+
+    class rewards:
+        base_height_target = Robot.height
+        min_dist = 0.2
+        max_dist = 0.5
+        target_joint_pos_scale = 0.17  # rad
+        target_feet_height = 0.05  # m
+        cycle_time = 0.5  # sec
+        only_positive_rewards = False
+        tracking_sigma = 5
+        max_contact_force = 200
+
+        class scales:
+            # You can keep or change these from ZBot2Cfg
+            default_joint_pos = 1.0
+            orientation = 1
+            base_height = 0.2
+            base_acc = 0.2
+            action_smoothness = -0.002
+            torques = -1e-5
+            dof_vel = -1e-3
+            dof_acc = -2.5e-7
+            collision = -1.0
+
+
 class ZBot2CfgPPO(LeggedRobotCfgPPO):
     seed = 5
     runner_class_name = "OnPolicyRunner"  # DWLOnPolicyRunner