Pos and vel targets with latency

.gitignore

@@ -33,3 +33,4 @@ isaacgym/
 
 # dev
 ref/
+*.csv

.gitmodules

@@ -1,6 +1,3 @@
 [submodule "third_party/isaacgym"]
 	path = third_party/isaacgym
 	url = ../../kscalelabs/isaacgym.git
-[submodule "third_party/kinfer"]
-	path = third_party/kinfer
-	url = https://github.com/kscalelabs/kinfer.git

sim/envs/__init__.py

@@ -14,14 +14,33 @@
 from sim.envs.humanoids.g1_env import G1FreeEnv
 from sim.envs.humanoids.gpr_config import GprCfg, GprCfgPPO, GprStandingCfg
 from sim.envs.humanoids.gpr_env import GprFreeEnv
+from sim.envs.humanoids.gpr_headless_config import GprHeadlessCfg, GprHeadlessCfgPPO
+from sim.envs.humanoids.gpr_headless_env import GprHeadlessEnv
+from sim.envs.humanoids.gpr_headless_latency_config import (
+    GprHeadlessLatencyCfg,
+    GprHeadlessLatencyCfgPPO,
+)
+from sim.envs.humanoids.gpr_headless_latency_env import GprHeadlessLatencyEnv
+from sim.envs.humanoids.gpr_headless_pos_config import (
+    GprHeadlessPosCfg,
+    GprHeadlessPosCfgPPO,
+    GprHeadlessPosStandingCfg,
+)
+from sim.envs.humanoids.gpr_headless_pos_env import GprHeadlessPosEnv
+from sim.envs.humanoids.gpr_latency_config import (
+    GprLatencyCfg,
+    GprLatencyCfgPPO,
+    GprLatencyStandingCfg,
+)
+from sim.envs.humanoids.gpr_latency_env import GprLatencyEnv
+from sim.envs.humanoids.gpr_vel_config import GprVelCfg, GprVelCfgPPO
+from sim.envs.humanoids.gpr_vel_env import GprVelEnv
 from sim.envs.humanoids.h1_config import H1Cfg, H1CfgPPO
 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, 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
 from sim.utils.task_registry import TaskRegistry  # noqa: E402
 
 task_registry = TaskRegistry()
@@ -33,4 +52,14 @@
 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())
+task_registry.register("gpr_headless", GprHeadlessEnv, GprHeadlessCfg(), GprHeadlessCfgPPO())
+task_registry.register("gpr_latency", GprLatencyEnv, GprLatencyCfg(), GprLatencyCfgPPO())
+task_registry.register("gpr_latency_standing", GprLatencyEnv, GprLatencyStandingCfg(), GprLatencyCfgPPO())
+task_registry.register(
+    "gpr_headless_latency", GprHeadlessLatencyEnv, GprHeadlessLatencyCfg(), GprHeadlessLatencyCfgPPO()
+)
+task_registry.register("gpr_vel", GprVelEnv, GprVelCfg(), GprVelCfgPPO())
+task_registry.register("gpr_headless_pos", GprHeadlessPosEnv, GprHeadlessPosCfg(), GprHeadlessPosCfgPPO())
+task_registry.register(
+    "gpr_headless_pos_standing", GprHeadlessPosEnv, GprHeadlessPosStandingCfg(), GprHeadlessPosCfgPPO()
+)

sim/envs/base/base_task.py

@@ -64,6 +64,7 @@ def __init__(self, cfg, sim_params, physics_engine, sim_device, headless):
         self.num_obs = cfg.env.num_observations
         self.num_privileged_obs = cfg.env.num_privileged_obs
         self.num_actions = cfg.env.num_actions
+        self.num_joints = cfg.env.num_joints
 
         # optimization flags for pytorch JIT
         torch._C._jit_set_profiling_mode(False)

sim/envs/base/legged_robot.py

@@ -82,7 +82,7 @@ def reset(self):
         """Reset all robots"""
 
         self.reset_idx(torch.arange(self.num_envs, device=self.device))
-        
+
         obs, privileged_obs, _, _, _ = self.step(
             torch.zeros(self.num_envs, self.num_actions, device=self.device, requires_grad=False)
         )
@@ -412,7 +412,9 @@ def _reset_root_states(self, env_ids):
             self.root_states[env_ids] = self.base_init_state
             self.root_states[env_ids, :3] += self.env_origins[env_ids]
         # base velocities
-        # self.root_states[env_ids, 7:13] = torch_rand_float(-0.05, 0.05, (len(env_ids), 6), device=self.device) # [7:10]: lin vel, [10:13]: ang vel
+        self.root_states[env_ids, 7:13] = torch_rand_float(
+            -0.05, 0.05, (len(env_ids), 6), device=self.device
+        )  # [7:10]: lin vel, [10:13]: ang vel
         if self.cfg.asset.fix_base_link:
             self.root_states[env_ids, 7:13] = 0
             self.root_states[env_ids, 2] += 1.8
@@ -521,13 +523,13 @@ def _init_buffers(self):
         )
         self.forward_vec = to_torch([1.0, 0.0, 0.0], device=self.device).repeat((self.num_envs, 1))
         self.torques = torch.zeros(
-            self.num_envs, self.num_actions, dtype=torch.float, device=self.device, requires_grad=False
+            self.num_envs, self.num_joints, dtype=torch.float, device=self.device, requires_grad=False
         )
         self.p_gains = torch.zeros(
-            self.num_envs, self.num_actions, dtype=torch.float, device=self.device, requires_grad=False
+            self.num_envs, self.num_joints, dtype=torch.float, device=self.device, requires_grad=False
         )
         self.d_gains = torch.zeros(
-            self.num_envs, self.num_actions, dtype=torch.float, device=self.device, requires_grad=False
+            self.num_envs, self.num_joints, dtype=torch.float, device=self.device, requires_grad=False
         )
         self.actions = torch.zeros(
             self.num_envs, self.num_actions, dtype=torch.float, device=self.device, requires_grad=False
@@ -612,11 +614,13 @@ def _prepare_reward_function(self):
 
         # remove zero scales + multiply non-zero ones by dt
         for key in list(self.reward_scales.keys()):
-            scale = self.reward_scales[key]
-            if scale == 0:
-                self.reward_scales.pop(key)
-            else:
-                self.reward_scales[key] *= self.dt
+            if key != "termination":
+                scale = self.reward_scales[key]
+                if scale == 0:
+                    self.reward_scales.pop(key)
+                else:
+                    self.reward_scales[key] *= self.dt
+
         # prepare list of functions
         self.reward_functions = []
         self.reward_names = []