zbot test

sim/envs/__init__.py

@@ -18,6 +18,8 @@
 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
@@ -29,4 +31,6 @@
 task_registry.register("h1", H1FreeEnv, H1Cfg(), H1CfgPPO())
 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())

sim/envs/base/legged_robot.py

@@ -80,8 +80,9 @@ def step(self, actions):
 
     def reset(self):
         """Reset all robots"""
+
         self.reset_idx(torch.arange(self.num_envs, device=self.device))
-        # self._resample_default_positions()
+
         obs, privileged_obs, _, _, _ = self.step(
             torch.zeros(self.num_envs, self.num_actions, device=self.device, requires_grad=False)
         )
@@ -161,6 +162,11 @@ def reset_idx(self, env_ids):
         if self.cfg.commands.curriculum and (self.common_step_counter % self.max_episode_length == 0):
             self.update_command_curriculum(env_ids)
 
+        # Add noise to the PD gains
+        if self.cfg.domain_rand.randomize_pd_gains:
+            self.p_gains[env_ids] = self.original_p_gains[env_ids] + torch.randn_like(self.p_gains[env_ids]) * 7
+            self.d_gains[env_ids] = self.original_d_gains[env_ids] + torch.randn_like(self.d_gains[env_ids]) * 0.3
+
         # reset robot states
         self._reset_dofs(env_ids)
 
@@ -577,7 +583,7 @@ def _init_buffers(self):
         self.default_dof_pos = torch.zeros(self.num_dof, dtype=torch.float, device=self.device, requires_grad=False)
         for i in range(self.num_dofs):
             name = self.dof_names[i]
-            print(name)
+            print(i, name)
             self.default_dof_pos[i] = self.cfg.init_state.default_joint_angles[name]
             found = False
 
@@ -591,6 +597,9 @@ def _init_buffers(self):
                 self.d_gains[:, i] = 0.0
                 raise ValueError(f"PD gain of joint {name} were not defined, setting them to zero")
 
+        self.original_p_gains = self.p_gains.clone()
+        self.original_d_gains = self.d_gains.clone()
+
         self.rand_push_force = torch.zeros((self.num_envs, 3), dtype=torch.float32, device=self.device)
         self.rand_push_torque = torch.zeros((self.num_envs, 3), dtype=torch.float32, device=self.device)
         self.default_dof_pos = self.default_dof_pos.unsqueeze(0)

sim/envs/base/legged_robot_config.py

@@ -218,6 +218,7 @@ class sim:
         substeps = 1
         gravity = [0.0, 0.0, -9.81]  # [m/s^2]
         up_axis = 1  # 0 is y, 1 is z
+        use_projected_gravity = False
 
         class physx:
             num_threads = 10

sim/envs/humanoids/gpr_config.py

@@ -1,6 +1,6 @@
 """Defines the environment configuration for the Getting up task"""
 
-from kinfer import proto as P
+# from kinfer import proto as P
 
 from sim.env import robot_urdf_path
 from sim.envs.base.legged_robot_config import (  # type: ignore
@@ -28,92 +28,92 @@ class env(LeggedRobotCfg.env):
         episode_length_s = 24  # episode length in seconds
         use_ref_actions = False
 
-        input_schema = P.IOSchema(
-            values=[
-                P.ValueSchema(
-                    value_name="vector_command",
-                    vector_command=P.VectorCommandSchema(
-                        dimensions=3,  # x_vel, y_vel, rot
-                    ),
-                ),
-                P.ValueSchema(
-                    value_name="timestamp",
-                    timestamp=P.TimestampSchema(
-                        start_seconds=0,
-                    ),
-                ),
-                P.ValueSchema(
-                    value_name="dof_pos",
-                    joint_positions=P.JointPositionsSchema(
-                        joint_names=Robot.joint_names(),
-                        unit=P.JointPositionUnit.RADIANS,
-                    ),
-                ),
-                P.ValueSchema(
-                    value_name="dof_vel",
-                    joint_velocities=P.JointVelocitiesSchema(
-                        joint_names=Robot.joint_names(),
-                        unit=P.JointVelocityUnit.RADIANS_PER_SECOND,
-                    ),
-                ),
-                P.ValueSchema(
-                    value_name="prev_actions",
-                    joint_positions=P.JointPositionsSchema(
-                        joint_names=Robot.joint_names(), unit=P.JointPositionUnit.RADIANS
-                    ),
-                ),
-                # Abusing the IMU schema to pass in euler and angular velocity instead of raw sensor data
-                P.ValueSchema(
-                    value_name="imu_ang_vel",
-                    imu=P.ImuSchema(
-                        use_accelerometer=False,
-                        use_gyroscope=True,
-                        use_magnetometer=False,
-                    ),
-                ),
-                P.ValueSchema(
-                    value_name="imu_euler_xyz",
-                    imu=P.ImuSchema(
-                        use_accelerometer=True,
-                        use_gyroscope=False,
-                        use_magnetometer=False,
-                    ),
-                ),
-                P.ValueSchema(
-                    value_name="hist_obs",
-                    state_tensor=P.StateTensorSchema(
-                        # 11 is the number of single observation features - 6 from IMU, 5 from command input
-                        # 3 comes from the number of times num_actions is repeated in the observation (dof_pos, dof_vel, prev_actions)
-                        shape=[frame_stack * (11 + NUM_JOINTS * 3)],
-                        dtype=P.DType.FP32,
-                    ),
-                ),
-            ]
-        )
-
-        output_schema = P.IOSchema(
-            values=[
-                P.ValueSchema(
-                    value_name="actions",
-                    joint_positions=P.JointPositionsSchema(
-                        joint_names=Robot.joint_names(), unit=P.JointPositionUnit.RADIANS
-                    ),
-                ),
-                P.ValueSchema(
-                    value_name="actions_raw",
-                    joint_positions=P.JointPositionsSchema(
-                        joint_names=Robot.joint_names(), unit=P.JointPositionUnit.RADIANS
-                    ),
-                ),
-                P.ValueSchema(
-                    value_name="new_x",
-                    state_tensor=P.StateTensorSchema(
-                        shape=[frame_stack * (11 + NUM_JOINTS * 3)],
-                        dtype=P.DType.FP32,
-                    ),
-                ),
-            ]
-        )
+        # input_schema = P.IOSchema(
+        #     values=[
+        #         P.ValueSchema(
+        #             value_name="vector_command",
+        #             vector_command=P.VectorCommandSchema(
+        #                 dimensions=3,  # x_vel, y_vel, rot
+        #             ),
+        #         ),
+        #         P.ValueSchema(
+        #             value_name="timestamp",
+        #             timestamp=P.TimestampSchema(
+        #                 start_seconds=0,
+        #             ),
+        #         ),
+        #         P.ValueSchema(
+        #             value_name="dof_pos",
+        #             joint_positions=P.JointPositionsSchema(
+        #                 joint_names=Robot.joint_names(),
+        #                 unit=P.JointPositionUnit.RADIANS,
+        #             ),
+        #         ),
+        #         P.ValueSchema(
+        #             value_name="dof_vel",
+        #             joint_velocities=P.JointVelocitiesSchema(
+        #                 joint_names=Robot.joint_names(),
+        #                 unit=P.JointVelocityUnit.RADIANS_PER_SECOND,
+        #             ),
+        #         ),
+        #         P.ValueSchema(
+        #             value_name="prev_actions",
+        #             joint_positions=P.JointPositionsSchema(
+        #                 joint_names=Robot.joint_names(), unit=P.JointPositionUnit.RADIANS
+        #             ),
+        #         ),
+        #         # Abusing the IMU schema to pass in euler and angular velocity instead of raw sensor data
+        #         P.ValueSchema(
+        #             value_name="imu_ang_vel",
+        #             imu=P.ImuSchema(
+        #                 use_accelerometer=False,
+        #                 use_gyroscope=True,
+        #                 use_magnetometer=False,
+        #             ),
+        #         ),
+        #         P.ValueSchema(
+        #             value_name="imu_euler_xyz",
+        #             imu=P.ImuSchema(
+        #                 use_accelerometer=True,
+        #                 use_gyroscope=False,
+        #                 use_magnetometer=False,
+        #             ),
+        #         ),
+        #         P.ValueSchema(
+        #             value_name="hist_obs",
+        #             state_tensor=P.StateTensorSchema(
+        #                 # 11 is the number of single observation features - 6 from IMU, 5 from command input
+        #                 # 3 comes from the number of times num_actions is repeated in the observation (dof_pos, dof_vel, prev_actions)
+        #                 shape=[frame_stack * (11 + NUM_JOINTS * 3)],
+        #                 dtype=P.DType.FP32,
+        #             ),
+        #         ),
+        #     ]
+        # )
+
+        # output_schema = P.IOSchema(
+        #     values=[
+        #         P.ValueSchema(
+        #             value_name="actions",
+        #             joint_positions=P.JointPositionsSchema(
+        #                 joint_names=Robot.joint_names(), unit=P.JointPositionUnit.RADIANS
+        #             ),
+        #         ),
+        #         P.ValueSchema(
+        #             value_name="actions_raw",
+        #             joint_positions=P.JointPositionsSchema(
+        #                 joint_names=Robot.joint_names(), unit=P.JointPositionUnit.RADIANS
+        #             ),
+        #         ),
+        #         P.ValueSchema(
+        #             value_name="new_x",
+        #             state_tensor=P.StateTensorSchema(
+        #                 shape=[frame_stack * (11 + NUM_JOINTS * 3)],
+        #                 dtype=P.DType.FP32,
+        #             ),
+        #         ),
+        #     ]
+        # )
 
     class safety(LeggedRobotCfg.safety):
         # safety factors