1.elf12 good

03eab9af · liufq · acced63a · 03eab9af · 03eab9af · 03eab9af
Commit 03eab9af authored Mar 25, 2025 by liufq
--- a/humanoid/envs/__init__.py
+++ b/humanoid/envs/__init__.py
@@ -38,6 +38,12 @@ from .x1.x1_dh_stand_config  import X1DHStandCfg, X1DHStandCfgPPO
 from .x1.x1_dh_stand_env import X1DHStandEnv
+from .elf12.elf12_dh_stand_config  import Elf12DHStandCfg, Elf12DHStandCfgPPO
+from .elf12.elf12_dh_stand_env import Elf12DHStandEnv
 from humanoid.utils.task_registry import task_registry
 task_registry.register( "x1_dh_stand", X1DHStandEnv, X1DHStandCfg(), X1DHStandCfgPPO() )
+task_registry.register( "elf12_dh_stand", Elf12DHStandEnv, Elf12DHStandCfg(), Elf12DHStandCfgPPO() )
--- a/humanoid/envs/elf12/elf12_dh_stand_config.py
+++ b/humanoid/envs/elf12/elf12_dh_stand_config.py
@@ -32,7 +32,7 @@
 from humanoid.envs.base.legged_robot_config import LeggedRobotCfg, LeggedRobotCfgPPO
-class X1DHStandCfg(LeggedRobotCfg):
+class Elf12DHStandCfg(LeggedRobotCfg):
    """
    Configuration class for the XBotL humanoid robot.
    """
@@ -60,16 +60,16 @@ class X1DHStandCfg(LeggedRobotCfg):
    class asset(LeggedRobotCfg.asset):
-        file = '{LEGGED_GYM_ROOT_DIR}/resources/robots/x1/urdf/x1.urdf'
+        file = '{LEGGED_GYM_ROOT_DIR}/resources/robots/elf12/urdf/elf12.urdf'
-        xml_file = '{LEGGED_GYM_ROOT_DIR}/resources/robots/x1/mjcf/xyber_x1_flat.xml'
+        xml_file = '{LEGGED_GYM_ROOT_DIR}/resources/robots/xxxx'
-        name = "x1"
+        name = "elf12"
-        foot_name = "ankle_roll"
+        foot_name = "ankle_x_link"
-        knee_name = "knee_pitch"
+        knee_name = "knee_y_link"
-        terminate_after_contacts_on = ['base_link']
+        terminate_after_contacts_on = ['base_link', 'hip']
-        penalize_contacts_on = ["base_link"]
+        penalize_contacts_on = ["base_link", "hip"]
-        self_collisions = 0  # 1 to disable, 0 to enable...bitwise filter
+        self_collisions = 1  # 1 to disable, 0 to enable...bitwise filter
        flip_visual_attachments = False
        replace_cylinder_with_capsule = False
        fix_base_link = False
@@ -123,31 +123,30 @@ class X1DHStandCfg(LeggedRobotCfg):
    class init_state(LeggedRobotCfg.init_state):
-        pos = [0.0, 0.0, 0.7]
+        pos = [0.0, 0.0, 0.78]
        default_joint_angles = {  # = target angles [rad] when action = 0.0
-            'left_hip_pitch_joint': 0.4,
+            'l_hip_z_joint': 0.0,
-            'left_hip_roll_joint': 0.05,
+            'l_hip_x_joint': 0.0,
-            'left_hip_yaw_joint': -0.31,
+            'l_hip_y_joint': -0.3,
-            'left_knee_pitch_joint': 0.49,
+            'l_knee_y_joint': 0.6,
-            'left_ankle_pitch_joint': -0.21,
+            'l_ankle_y_joint': -0.3,
-            'left_ankle_roll_joint': 0.0,
+            'l_ankle_x_joint': 0.0,
-            'right_hip_pitch_joint': -0.4,
-            'right_hip_roll_joint': -0.05,
+            'r_hip_z_joint': 0.0,
-            'right_hip_yaw_joint': 0.31,
+            'r_hip_x_joint': 0.0,
-            'right_knee_pitch_joint': 0.49,
+            'r_hip_y_joint': -0.3,
-            'right_ankle_pitch_joint': -0.21, 
+            'r_knee_y_joint': 0.6,
-            'right_ankle_roll_joint': 0.0,
+            'r_ankle_y_joint': -0.3,
+            'r_ankle_x_joint': 0.0,
        }
    class control(LeggedRobotCfg.control):
        # PD Drive parameters:
        control_type = 'P'
-        stiffness = {'hip_pitch_joint': 30, 'hip_roll_joint': 40,'hip_yaw_joint': 35,
+        stiffness = {'hip_z': 150, 'hip_x': 150, 'hip_y': 150, 'knee_y': 150, 'ankle_y': 20, 'ankle_x': 20}
-                     'knee_pitch_joint': 100, 'ankle_pitch_joint': 35, 'ankle_roll_joint': 35}
+        damping = {'hip_z': 3, 'hip_x': 3, 'hip_y': 3, 'knee_y': 3, 'ankle_y': 1, 'ankle_x': 1}
-        damping = {'hip_pitch_joint': 3, 'hip_roll_joint': 3.0,'hip_yaw_joint': 4, 
-                   'knee_pitch_joint': 10, 'ankle_pitch_joint': 0.5, 'ankle_roll_joint': 0.5}
        # action scale: target angle = actionScale * action + defaultAngle
        action_scale = 0.5
@@ -183,8 +182,8 @@ class X1DHStandCfg(LeggedRobotCfg):
        push_interval_s = 4 # every this second, push robot
        update_step = 2000 * 24 # after this count, increase push_duration index
        push_duration = [0, 0.05, 0.1, 0.15, 0.2, 0.25] # increase push duration during training
-        max_push_vel_xy = 0.2
+        max_push_vel_xy = 0.5
-        max_push_ang_vel = 0.2
+        max_push_ang_vel = 0.3
        randomize_base_mass = True
        added_mass_range = [-3, 3] # base mass rand range, base mass is all fix link sum mass
@@ -295,7 +294,7 @@ class X1DHStandCfg(LeggedRobotCfg):
        sw_switch = True # use stand_com_threshold or not
        class ranges:
-            lin_vel_x = [-0.4, 1.2] # min max [m/s] 
+            lin_vel_x = [-0.4, 0.8] # min max [m/s] 
            lin_vel_y = [-0.4, 0.4]   # min max [m/s]
            ang_vel_yaw = [-0.6, 0.6]    # min max [rad/s]
            heading = [-3.14, 3.14]
@@ -304,16 +303,18 @@ class X1DHStandCfg(LeggedRobotCfg):
        soft_dof_pos_limit = 0.98
        soft_dof_vel_limit = 0.9
        soft_torque_limit = 0.9
-        base_height_target = 0.61
+        base_height_target = 0.68
-        foot_min_dist = 0.2
+        foot_min_dist = 0.15
        foot_max_dist = 1.0
        # final_swing_joint_pos = final_swing_joint_delta_pos + default_pos
-        final_swing_joint_delta_pos = [0.25, 0.05, -0.11, 0.35, -0.16, 0.0, -0.25, -0.05, 0.11, 0.35, -0.16, 0.0]
+        final_swing_joint_delta_pos = [0.0, 0.0, -0.16, 0.32, -0.16, 0.0, 
+                                       0.0, 0.0, -0.16, 0.32, -0.16, 0.0]
        target_feet_height = 0.03 
-        target_feet_height_max = 0.06
+        target_feet_height_max = 0.08
-        feet_to_ankle_distance = 0.041
+        feet_to_ankle_distance = 0.04
        cycle_time = 0.7
+        double_suport = 0.4
        # if true negative total rewards are clipped at zero (avoids early termination problems)
        only_positive_rewards = True
        # tracking reward = exp(-error*sigma)
@@ -341,7 +342,7 @@ class X1DHStandCfg(LeggedRobotCfg):
            default_joint_pos = 1.0
            orientation = 1.
            feet_rotation = 0.3
-            base_height = 0.2
+            # base_height = 0.2
            base_acc = 0.2
            # energy
            action_smoothness = -0.002
@@ -367,7 +368,7 @@ class X1DHStandCfg(LeggedRobotCfg):
        clip_actions = 100.
-class X1DHStandCfgPPO(LeggedRobotCfgPPO):
+class Elf12DHStandCfgPPO(LeggedRobotCfgPPO):
    seed = 5
    runner_class_name = 'DHOnPolicyRunner'   # DWLOnPolicyRunner
@@ -382,7 +383,7 @@ class X1DHStandCfgPPO(LeggedRobotCfgPPO):
        filter_size=[32, 16]
        stride_size=[3, 2]
        lh_output_dim= 64   #long history output dim
-        in_channels = X1DHStandCfg.env.frame_stack
+        in_channels = Elf12DHStandCfg.env.frame_stack
    class algorithm(LeggedRobotCfgPPO.algorithm):
        entropy_coef = 0.001
@@ -391,20 +392,20 @@ class X1DHStandCfgPPO(LeggedRobotCfgPPO):
        gamma = 0.994
        lam = 0.9
        num_mini_batches = 4
-        if X1DHStandCfg.terrain.measure_heights:
+        if Elf12DHStandCfg.terrain.measure_heights:
-            lin_vel_idx = (X1DHStandCfg.env.single_num_privileged_obs + X1DHStandCfg.terrain.num_height) * (X1DHStandCfg.env.c_frame_stack - 1) + X1DHStandCfg.env.single_linvel_index
+            lin_vel_idx = (Elf12DHStandCfg.env.single_num_privileged_obs + Elf12DHStandCfg.terrain.num_height) * (Elf12DHStandCfg.env.c_frame_stack - 1) + Elf12DHStandCfg.env.single_linvel_index
        else:
-            lin_vel_idx = X1DHStandCfg.env.single_num_privileged_obs * (X1DHStandCfg.env.c_frame_stack - 1) + X1DHStandCfg.env.single_linvel_index
+            lin_vel_idx = Elf12DHStandCfg.env.single_num_privileged_obs * (Elf12DHStandCfg.env.c_frame_stack - 1) + Elf12DHStandCfg.env.single_linvel_index
    class runner:
        policy_class_name = 'ActorCriticDH'
        algorithm_class_name = 'DHPPO'
        num_steps_per_env = 24  # per iteration
-        max_iterations = 20000  # number of policy updates
+        max_iterations = 50000  # number of policy updates
        # logging
-        save_interval = 100  # check for potential saves every this many iterations
+        save_interval = 500  # check for potential saves every this many iterations
-        experiment_name = 'x1_dh_stand'
+        experiment_name = 'elf12_dh_stand'
        run_name = ''
        # load and resume
        resume = False

--- a/humanoid/envs/elf12/elf12_dh_stand_env.py
+++ b/humanoid/envs/elf12/elf12_dh_stand_env.py
@@ -76,7 +76,7 @@ def get_euler_xyz_tensor(quat):
    euler_xyz[euler_xyz > np.pi] -= 2 * np.pi
    return euler_xyz
-class X1DHStandEnv(LeggedRobot):
+class Elf12DHStandEnv(LeggedRobot):
    '''
    X1DHStandEnv is a class that represents a custom environment for a legged robot.
@@ -157,7 +157,7 @@ class X1DHStandEnv(LeggedRobot):
        # right foot stance
        stance_mask[:, 1] = sin_pos < 0
        # Add double support phase
-        stance_mask[torch.abs(sin_pos) < 0.1] = 1
+        stance_mask[torch.abs(sin_pos) < self.cfg.rewards.double_suport] = 1
        # stand mask == 1 means stand leg 
        return stance_mask
@@ -295,7 +295,7 @@ class X1DHStandEnv(LeggedRobot):
        self.ref_dof_pos[:, 10] = sin_pos_r * self.cfg.rewards.final_swing_joint_delta_pos[10]
        self.ref_dof_pos[:, 11] = sin_pos_r * self.cfg.rewards.final_swing_joint_delta_pos[11]
-        self.ref_dof_pos[torch.abs(sin_pos) < 0.1] = 0.
+        self.ref_dof_pos[torch.abs(sin_pos) < self.cfg.rewards.double_suport] = 0.
        # if use_ref_actions=True, action += ref_action
        self.ref_action = 2 * self.ref_dof_pos
@@ -610,7 +610,8 @@ class X1DHStandEnv(LeggedRobot):
        with the ground. The speed of the foot is calculated and scaled by the contact conditions.
        """
        contact = self.contact_forces[:, self.feet_indices, 2] > 5.
-        foot_speed_norm = torch.norm(self.rigid_state[:, self.feet_indices, 10:12], dim=2)
+        # foot_speed_norm = torch.norm(self.rigid_state[:, self.feet_indices, 10:12], dim=2)
+        foot_speed_norm = torch.norm(self.rigid_state[:, self.feet_indices, 7:9], dim=2)
        rew = torch.sqrt(foot_speed_norm)
        rew *= contact
        return torch.sum(rew, dim=1)
@@ -665,8 +666,8 @@ class X1DHStandEnv(LeggedRobot):
        on penalizing deviation in yaw and roll directions. Excludes yaw and roll from the main penalty.
        """
        joint_diff = self.dof_pos - self.default_joint_pd_target
-        left_yaw_roll = joint_diff[:, [1,2,5]]
+        left_yaw_roll = joint_diff[:, [0,1,5]]
-        right_yaw_roll = joint_diff[:, [7,8,11]]
+        right_yaw_roll = joint_diff[:, [6,7,11]]
        yaw_roll = torch.norm(left_yaw_roll, dim=1) + torch.norm(right_yaw_roll, dim=1)
        yaw_roll = torch.clamp(yaw_roll - 0.1, 0, 50)
        return torch.exp(-yaw_roll * 100) - 0.01 * torch.norm(joint_diff, dim=1)

--- a/humanoid/export.sh
+++ b/humanoid/export.sh
+python scripts/export_policy_dh.py --task=elf12_dh_stand #--load_run=<date_time><run_name>
\ No newline at end of file
--- a/humanoid/play.sh
+++ b/humanoid/play.sh
+# python scripts/play.py --task=bot_elf_dh_stand #--load_run=<date_time><run_name>
+python scripts/play.py --task=elf12_dh_stand #--load_run=<date_time><run_name>
\ No newline at end of file
--- a/humanoid/scripts/play.py
+++ b/humanoid/scripts/play.py
@@ -132,7 +132,7 @@ def play(args):
    logger = Logger(env_cfg.sim.dt * env_cfg.control.decimation)
    robot_index = 0 # which robot is used for logging
    joint_index = 5 # which joint is used for logging
-    stop_state_log = 1000 # number of steps before plotting states
+    stop_state_log = 500 # number of steps before plotting states
    if RENDER:
        camera_properties = gymapi.CameraProperties()
        camera_properties.width = 1920
@@ -193,8 +193,8 @@ def play(args):
                    'base_height' : env.root_states[robot_index, 2].item(),
                    'foot_z_l' : env.rigid_state[robot_index,4,2].item(),
                    'foot_z_r' : env.rigid_state[robot_index,9,2].item(),
-                    'foot_forcez_l' : env.contact_forces[robot_index,4,2].item(),
+                    'foot_forcez_l' : env.contact_forces[robot_index,6,2].item(),
-                    'foot_forcez_r' : env.contact_forces[robot_index,9,2].item(),
+                    'foot_forcez_r' : env.contact_forces[robot_index,12,2].item(),
                    'base_vel_x': env.base_lin_vel[robot_index, 0].item(),
                    'command_x': x_vel_cmd,
                    'base_vel_y':  env.base_lin_vel[robot_index, 1].item(),
@@ -245,6 +245,6 @@ def play(args):
 if __name__ == '__main__':
    EXPORT_POLICY = False
    RENDER = False
-    FIX_COMMAND = False
+    FIX_COMMAND = True #False
    args = get_args()
    play(args)
--- a/humanoid/train.sh
+++ b/humanoid/train.sh
+# python scripts/train.py --task=x1_dh_stand --run_name=test #--headless #--resume
+# python ./scripts/train.py --task=bot_elf_dh_stand --run_name=test #--num_envs=3 #--resume #--headless 
+python ./scripts/train.py --task=elf12_dh_stand --run_name=test #--num_envs=3 #--resume #--headless 
\ No newline at end of file
--- a/humanoid/utils/logger.py
+++ b/humanoid/utils/logger.py
@@ -74,6 +74,7 @@ class Logger:
        # self.plot_process10 = Process(target=self._plot_tn_rms1)
        # self.plot_process11 = Process(target=self._plot_tn1)
        # self.plot_process12 = Process(target=self._plot_torque_vel1)
+        self.plot_process13 = Process(target=self._plot_force)
        self.plot_process.start()
        self.plot_process1.start()
        # self.plot_process2.start()
@@ -87,6 +88,7 @@ class Logger:
        # self.plot_process10.start()
        # self.plot_process11.start()
        # self.plot_process12.start()
+        self.plot_process13.start()
    def _plot(self):
        nb_rows = 3
@@ -128,6 +130,40 @@ class Logger:
        # a.legend()
        plt.show()
+    def _plot_force(self):
+        for key, value in self.state_log.items():
+            time = np.linspace(0, len(value)*self.dt, len(value))
+            break
+        log= self.state_log
+        fig,ax=plt.subplots(1,1)
+        if log["foot_forcez_l"]: 
+            forcez_l = np.array(log["foot_forcez_l"])
+            forcez_l /= 100.0
+            smothf_l = np.convolve(forcez_l, np.ones(3)/3, mode='same') #'valid'
+            # smothf_l = forcez_l
+            ax.plot(time, smothf_l+1.1, label='measured_l', color='y')
+        if log["foot_forcez_r"]: 
+            forcez_r = np.array(log["foot_forcez_r"])
+            forcez_r /= 100.0
+            smothf_r = np.convolve(forcez_r, np.ones(3)/3, mode='same')
+            # smothf_r = forcez_r
+            ax.plot(time, smothf_r+1.1, label='measured_r', color='b') 
+        # if log["contact_mask_r"]:
+        #     ax.plot(time, log["contact_mask_r"], label='contact_mask_r', color='b')
+        # if log["ref_r_3"]:
+        #     ax.plot(time, log["ref_r_3"], label='ref_r_3', color='b')
+        if log["command_sin"]: ax.plot(time, log["command_sin"], label='commanded_sin', color='y')
+        if log["command_cos"]: ax.plot(time, log["command_cos"], label='commanded_cos', color='b')
+        ax.set(xlabel='time [s]', ylabel='command cos', title='Command Cos')
+        plt.show()
    def _plot_position(self):
        nb_rows = 2
        nb_cols = 3

--- a/resources/robots/elf12/urdf/elf12.urdf
+++ b/resources/robots/elf12/urdf/elf12.urdf
@@ -361,7 +361,7 @@
          rgba="0.752941176470588 0.752941176470588 0.752941176470588 1" />
      </material>
    </visual>
-    <!-- <collision>
+    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
@@ -369,8 +369,8 @@
        <mesh
          filename="../meshes/l_ankle_x_link.STL" />
      </geometry>
-    </collision> -->
+    </collision>
-    <collision name="l_foot_1">
+    <!-- <collision name="l_foot_1">
      <origin rpy="0 1.5708 0" xyz="0.0 0.03 -0.03"/>
      <geometry>
          <cylinder length="0.185" radius="0.01"/>
@@ -381,7 +381,7 @@
      <geometry>
          <cylinder length="0.185" radius="0.01"/>
      </geometry>
-    </collision>
+    </collision> -->
  </link>
  <joint
    name="l_ankle_x_joint"
@@ -721,7 +721,7 @@
          rgba="0.752941176470588 0.752941176470588 0.752941176470588 1" />
      </material>
    </visual>
-    <!-- <collision>
+    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
@@ -729,8 +729,8 @@
        <mesh
          filename="../meshes/r_ankle_x_link.STL" />
      </geometry>
-    </collision> -->
+    </collision>
-    <collision name="r_foot_1">
+    <!-- <collision name="r_foot_1">
      <origin rpy="0 1.5708 0" xyz="0.0 0.03 -0.03"/>
      <geometry>
          <cylinder length="0.185" radius="0.01"/>
@@ -741,7 +741,7 @@
      <geometry>
          <cylinder length="0.185" radius="0.01"/>
      </geometry>
-    </collision>
+    </collision> -->
  </link>
  <joint
    name="r_ankle_x_joint"