Fase 4: Cambio de terreno

Objetivos de la Fase 4

En esta fase cambiaré el plano utilizado hasta ahora como terreno para el entrenamiento por un terreno con inclinaciones. El robot deberá aprender a balancearse en este tipo de terrenos inclinados sin olvidar lo aprendido en las fases anteriores.

• Que el robot sea capaz de mantenerse en equilibrio estático cuando el comando de movimiento es cero.

• Que el robot sea capaz de desplazarse recto a la velocidad lineal solicitada cuando el comando de velocidad no tiene componente angular, manteniendo el equilibrio cuando se encuentre sobre un terreno inclinado.

• Que el robot sea capaz de realizar movimientos curvilineos, adaptando su velocidad linear y angular a la solicitada cuando el comando tiene tanto componente lineal como angular. Todo ello mientras mantiene el equilibrio cuando se encuentre sobre un terreno inclinado.

Estrategia de entrenamiento

En esta fase no vamos a cambiar el vector de observaciones ni de acciones, sólamente el terreno. Así que podremos utilizar la política obtenida en la fase 3 directamente para realizar el entrenamiento.

Creación del nuevo terreno

Utilizando blender he creado un plano de 75x75m:

En el centro del plano he creado una serie de rampas:

El resto del terreno es plano, para que el robot no olvide cómo mantener el equilibrio cuando no existe inclinación.

Exporto el archivo en formato STL, abro un proyecto vacio de IsaacSim e importo el STL del terreno:

Una vez importado, hay que agregar el Colliders Preset al terreno: Clic derecho sobre el terreno > Add > Physics > Colliders Preset

Una vez hecho esto, IsaacSim debería mostrar una malla sobre el terreno, que es la mesh de colisión que utilizará para calcular las físicas (el contacto del robot sobre el terreno).

Si esto no sucede, hay que activar su visualización:

Para comprobar que la malla de colisión está bien, a mi me gusta comprobarlo creando una esfera a la que le agrego "Rigid Body with colliders preset". Situo la esfera sobre el plano y ejecuto la simulación.

Una vez comprobado que todo funciona como debe, en el Stage Tree pulso sobre el objeto del terreno con el botón derecho y selecciono la opción Save Selected y guardo el USD del terreno en mi carpeta del proyecto.

Una vez hecho esto, ya podemos utilizar este USD para cargar el nuevo terreno en el entrenamiento.

Cambios en __init__.py

__init__.py

# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause

import gymnasium as gym

from . import agents

##
# Register Gym environments.
##


gym.register(
    id="Template-Simplerobot-Direct-v0",
    entry_point=f"{__name__}.simplerobot_env:SimplerobotEnv",
    disable_env_checker=True,
    kwargs={
        "env_cfg_entry_point": f"{__name__}.simplerobot_env_cfg:SimplerobotEnvCfg",
        "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:PPORunnerCfg",
    },
)

gym.register(
    id="Template-Simplerobot-Direct-Phase1-v0",
    entry_point=f"{__name__}.simplerobot_env:SimplerobotEnv",
    disable_env_checker=True,
    kwargs={
        "env_cfg_entry_point": f"{__name__}.simplerobot_env_cfg:SimplerobotEnvCfgPhase1",
        "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:PPORunnerCfg",
    },
)

gym.register(
    id="Template-Simplerobot-Direct-Phase2-v0",
    entry_point=f"{__name__}.simplerobot_env:SimplerobotEnv",
    disable_env_checker=True,
    kwargs={
        "env_cfg_entry_point": f"{__name__}.simplerobot_env_cfg:SimplerobotEnvCfgPhase2",
        "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:PPORunnerCfg",
    },
)

gym.register(
    id="Template-Simplerobot-Direct-Phase3-v0",
    entry_point=f"{__name__}.simplerobot_env:SimplerobotEnv",
    disable_env_checker=True,
    kwargs={
        "env_cfg_entry_point": f"{__name__}.simplerobot_env_cfg:SimplerobotEnvCfgPhase3",
        "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:PPORunnerCfg",
    },
)

gym.register(
    id="Template-Simplerobot-Direct-Phase4-v0",
    entry_point=f"{__name__}.simplerobot_env:SimplerobotEnv",
    disable_env_checker=True,
    kwargs={
        "env_cfg_entry_point": f"{__name__}.simplerobot_env_cfg:SimplerobotEnvCfgPhase4",
        "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:PPORunnerCfg",
    },
)

He registrado una nueva fase para incluir la 4.

Cambios en simplerobot_env_cfg.py

simplerobot_env_cfg.py

# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause

from isaaclab_assets.robots.simplerobot import SIMPLE_ROBOT_CFG

from isaaclab.assets import ArticulationCfg
from isaaclab.envs import DirectRLEnvCfg
from isaaclab.scene import InteractiveSceneCfg
from isaaclab.sim import SimulationCfg
from isaaclab.utils import configclass


@configclass
class SimplerobotEnvCfg(DirectRLEnvCfg):
    # env
    decimation = 2
    episode_length_s = 20
    actions_scale = 0.25

    # - spaces definition
    action_space = 2  # two wheel velocities: [left_wheel_velocity, right_wheel_velocity]
    observation_space = 3 + 2 + 2 + 1 + 1 + 2 # gravity vector (3), angular velocity pitch y yaw (2), wheel velocities (2), linear velocity (1), angular yaw (1),  command velocity linear & yaw (2)
    state_space = 0

    # simulation
    sim: SimulationCfg = SimulationCfg(dt=1 / 120, render_interval=decimation)

    # robot(s)
    robot_cfg: ArticulationCfg = SIMPLE_ROBOT_CFG.replace(prim_path="/World/envs/env_.*/Robot")

    # scene
    # 100 environments in a grid, spaced by 4 meters. Each env has its own physics scene so interactions are independent.
    scene: InteractiveSceneCfg = InteractiveSceneCfg(num_envs=400, env_spacing=20.0, replicate_physics=True)

    dof_names = ["left_joint", "right_joint"] # as this configuration file defines topology, the names of the dofs should be specified here

    phase = 0
    # reward weights (defaults / neutral)
    upright_reward_weight = 0.0
    alive_reward_weight = 0.0
    ang_vel_penalty_weight = 0.0
    vel_penalty_weight = 0.0
    no_still_penalty_weight = 0.0
    diff_penalty_weight = 0.0
    yaw_penalty_weight = 0.0
    max_lin_vel = 1.0
    max_yaw_vel = 1.0


@configclass
class SimplerobotEnvCfgPhase1(SimplerobotEnvCfg):
    """Phase 1: stabilization / balance"""
    phase = 1
    upright_reward_weight = 2.0
    alive_reward_weight = 0.2
    ang_vel_penalty_weight = -0.5
    vel_penalty_weight = 0.0
    no_still_penalty_weight = 0.0
    diff_penalty_weight = 0.0
    yaw_penalty_weight = 0.0

@configclass
class SimplerobotEnvCfgPhase2(SimplerobotEnvCfg):
    """Phase 2: velocity tracking"""
    phase = 2
    upright_reward_weight = 0.2
    alive_reward_weight = 0.2
    ang_vel_penalty_weight = -0.05
    vel_penalty_weight = -2.0
    no_still_penalty_weight = -5.0
    diff_penalty_weight = -0.2
    yaw_penalty_weight = 0.0

@configclass
class SimplerobotEnvCfgPhase3(SimplerobotEnvCfg):
    """Phase 3: yaw"""
    phase = 3
    upright_reward_weight = 0.2
    alive_reward_weight = 0.2
    ang_vel_penalty_weight = -0.05
    vel_penalty_weight = -2.0
    no_still_penalty_weight = -5.0
    diff_penalty_weight = 0.0
    yaw_penalty_weight = -1.0

@configclass
class SimplerobotEnvCfgPhase4(SimplerobotEnvCfg):
    """Phase 4: Difficult terrains"""
    phase = 4
    upright_reward_weight = 0.2
    alive_reward_weight = 0.2
    ang_vel_penalty_weight = -0.05
    vel_penalty_weight = -2.0
    no_still_penalty_weight = -5.0
    diff_penalty_weight = 0.0
    yaw_penalty_weight = -1.0

scene: InteractiveSceneCfg = InteractiveSceneCfg(num_envs=400, env_spacing=20.0, replicate_physics=True)

He separado a los robots 20 metros entre sí para hacer más fácil la visualización. Esto sólamente afecta a nivel visual. El terreno es de 70x70m, lo que significa que los terrenos van a solaparse unos con otros, pero solo visualmente, ya que cada instancia de robot tiene asociado su propio terreno y calcula las colisiones únicamente con este.

@configclass
class SimplerobotEnvCfgPhase4(SimplerobotEnvCfg):
    """Phase 4: Difficult terrains"""
    phase = 4
    upright_reward_weight = 0.2
    alive_reward_weight = 0.2
    ang_vel_penalty_weight = -0.05
    vel_penalty_weight = -2.0
    no_still_penalty_weight = -5.0
    diff_penalty_weight = 0.0
    yaw_penalty_weight = -1.0

He creado una nueva clase para la fase 4, manteniendo los mismos valores que en la fase 3 a excepción del valor de la variable phase.

Cambios en simplerobot_env.py

simplerobot_env.py

# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause

from __future__ import annotations

import torch
from collections.abc import Sequence

import isaaclab.sim as sim_utils
from isaaclab.assets import Articulation
from isaaclab.envs import DirectRLEnv
from isaaclab.sim.spawners.from_files import GroundPlaneCfg, spawn_ground_plane, UsdFileCfg, spawn_from_usd


from .simplerobot_env_cfg import SimplerobotEnvCfg
import random

class SimplerobotEnv(DirectRLEnv):
    cfg: SimplerobotEnvCfg

    def __init__(self, cfg: SimplerobotEnvCfg, render_mode: str | None = None, **kwargs):
        super().__init__(cfg, render_mode, **kwargs) # this super call will invoke _setup_scene()

        self.dof_idx, _ = self.robot.find_joints(self.cfg.dof_names) # get the indices of the controlled dofs


    def _setup_scene(self):
        self.robot = Articulation(self.cfg.robot_cfg)
        # add ground plane
        if self.cfg.phase != 4:
            spawn_ground_plane(prim_path="/World/ground", cfg=GroundPlaneCfg())
        else:
            terrain_cfg = UsdFileCfg(
                usd_path="/home/angellm/SimpleRobot/other/SimpleRobot/Inclinaciones1.usd",
                visible=True,
                copy_from_source=True,
            )
            spawn_from_usd(prim_path="/World/envs/env_.*/ground", cfg=terrain_cfg)

        # clone and replicate
        self.scene.clone_environments(copy_from_source=False) # copy_from_source=False will use instanceable references for better performance. 
        # add articulation to scene
        self.scene.articulations["robot"] = self.robot
        # add lights
        light_cfg = sim_utils.DomeLightCfg(intensity=2000.0, color=(0.75, 0.75, 0.75))
        light_cfg.func("/World/Light", light_cfg)

        self.commands = torch.zeros((self.cfg.scene.num_envs, 2)).cuda()  # initialize commands buffer

    # Both _pre_physics_step and _apply_action are not called every simulation step, but only at the steps when actions are applied (according to the decimation factor).
    # F.g., if decimation=2, these methods are called every 2 simulation steps: _pre_physics_step -> _apply_action -> physics step -> physics step -> _pre_physics_step -> _apply_action -> physics step -> physics step -> ...
    def _pre_physics_step(self, actions: torch.Tensor) -> None:
        # This method is called before the physics step. We store the actions to be applied later in _apply_action()
        self.actions = actions.clone() # Copy the actions and store them for use in _apply_action(). It acts as a buffer between the policy and the physics step.
        self.actions = self.actions * self.cfg.actions_scale # scale the actions to reasonable values

    def _apply_action(self) -> None:
        # This method is called after the _pre_physics_step() and before the physics step. Here we apply the stored actions to the robot.
        self.robot.set_joint_velocity_target(self.actions, joint_ids=self.dof_idx) # set the wheel velocities according to the actions
    
    def _get_observations(self) -> dict:
        self.projected_gravity = self.robot.data.projected_gravity_b # Shape (N,3)
        self.angular_velocity = self.robot.data.root_ang_vel_b[:, :2] # Shape (N,2) pitch y roll
        self.wheel_vel = self.robot.data.joint_vel #Shape (N, num_joints)
        self.vx = self.robot.data.root_lin_vel_b[:, 0:1]   # frontal
        self.yaw_vel = self.robot.data.root_ang_vel_b[:, 2:3]
        self.cmd = self.commands[:,0:2]

        obs = torch.cat(
            [
                self.projected_gravity,
                self.angular_velocity,
                self.wheel_vel,
                self.vx,
                self.yaw_vel,
                self.cmd,
            ],
            dim=-1,
        )

        observations = {"policy": obs}

        # print(f"Cmd: ({self.commands[0,0].item():.2f}, {self.commands[0,1].item():.2f}) | Robot: ({self.robot.data.root_lin_vel_b[0,0].item():.2f}, {self.robot.data.root_ang_vel_b[0,2].item():.2f})")
        
        return observations

    def _get_rewards(self) -> torch.Tensor:

        # --- Inclinación ---
        # projected_gravity_b ≈ [0, 0, -1] cuando está vertical
        tilt_error = self.projected_gravity[:, 0]**2 # Solo nos importa el eje X (adelante/atrás)
        upright_reward = torch.exp(-5.0 * tilt_error)

        # --- Velocidad angular (evitar oscilaciones) ---
        ang_vel_penalty = torch.sum(self.angular_velocity[:, :2] ** 2, dim=1) # penaliza pitch y roll, no yaw

        # --- Alive ---
        alive_reward = (~self.reset_buf).float()

        # --- Penalización por no moverse a la velocidad del comando ---
        vel_cmd = self.commands[:, 0:1]
        vel_cmd_mask = torch.abs(vel_cmd) > 1e-3
        vel_error = torch.zeros_like(vel_cmd)
        vel_error[vel_cmd_mask] = torch.abs(self.vx - vel_cmd)[vel_cmd_mask]
        vel_penalty = torch.clamp(vel_error / self.cfg.max_lin_vel, 0.0, 1.0).squeeze(-1)

        # --- Penalización por no estar quieto cuando el comando es 0 ---
        still_mask = torch.abs(vel_cmd) <= 1e-3
        no_still_error = torch.zeros_like(vel_cmd)
        no_still_error[still_mask] = torch.abs(self.vx - 0.0)[still_mask]
        no_still_penalty = torch.clamp(no_still_error / self.cfg.max_lin_vel, 0.0, 1.0).squeeze(-1)

        # --- Penalización por diferencia de velocidad entre ruedas (evitar giros) ---
        diff_penalty = (self.wheel_vel[:, 0] - self.wheel_vel[:, 1])**2

        # --- Penalización por no moverse a la velocidad angular del comando ---
        yaw_cmd = self.commands[:, 1:2]
        yaw_cmd_mask = torch.abs(yaw_cmd) > 1e-3
        yaw_error = torch.zeros_like(yaw_cmd)
        yaw_error[yaw_cmd_mask]= torch.abs(self.yaw_vel - yaw_cmd)[yaw_cmd_mask]
        yaw_penalty = torch.clamp(yaw_error / self.cfg.max_yaw_vel, 0.0, 1.0).squeeze(-1)

        # --- Reward final ---
        reward = (
            self.cfg.upright_reward_weight * upright_reward
            + self.cfg.ang_vel_penalty_weight * ang_vel_penalty
            + self.cfg.alive_reward_weight * alive_reward
            + self.cfg.vel_penalty_weight * vel_penalty
            + self.cfg.no_still_penalty_weight * no_still_penalty
            + self.cfg.diff_penalty_weight * diff_penalty
            + self.cfg.yaw_penalty_weight * yaw_penalty
        )

        return reward

    def _get_dones(self) -> tuple[torch.Tensor, torch.Tensor]:
        time_out = self.episode_length_buf >= self.max_episode_length - 1 # If the episode length buffer exceeds the max length, we time out

        # Si el robot se inclina más de ~50° en el eje X o Y, se considera que ha caído
        fallen = torch.any(torch.abs(self.projected_gravity[:, :2]) > 0.8727, dim=1)

        return fallen, time_out # for now we only terminate episodes on timeout, forgetting about other termination conditions

    def _reset_idx(self, env_ids: Sequence[int] | None):
        if env_ids is None:
            env_ids = self.robot._ALL_INDICES
        super()._reset_idx(env_ids)

        default_root_state = self.robot.data.default_root_state[env_ids] # get the default root state (position and orientation in World frame)
        default_root_state[:, :3] += self.scene.env_origins[env_ids]     # offset the position according to the environment origin
        default_root_state[:, 2] += 0.3  # SUBIR ROBOT

        # pick new commands for reset envs and normalize them just like in the setup
        if self.cfg.phase <= 1:
            self.commands[env_ids] = torch.zeros((len(env_ids), 2)).cuda()
        elif self.cfg.phase == 2:
            if random.random() < 0.3:
                self.commands[env_ids] = torch.zeros((len(env_ids), 2)).cuda()
            else:
                self.commands[env_ids, 0] = 0.3
                self.commands[env_ids, 1] = 0
        elif self.cfg.phase >= 3:
            num = len(env_ids)
            rnd = torch.rand(num, device=self.device)
            self.commands[env_ids] = 0.0
            # 20% robots quietos
            mask_zero = rnd <= 0.2 # con estos no hay que hacer nada porque ya estan a cero
            # 20% robots solo con velocidad lineal
            mask_lin = (rnd > 0.2) & (rnd <= 0.4)
            self.commands[env_ids[mask_lin], 0] = (torch.rand(mask_lin.sum(), device=self.device) * 2 - 1)
            # 60% robots velocidad lineal y angular (yaw)
            mask_full = rnd > 0.4
            self.commands[env_ids[mask_full], 0] = (torch.rand(mask_full.sum(), device=self.device) * 2 - 1)
            self.commands[env_ids[mask_full], 1] = (torch.rand(mask_full.sum(), device=self.device) * 2 - 1)

        self.robot.write_root_state_to_sim(default_root_state, env_ids)  # reset the root state of the robot

Cambios en _setup_scene

# add ground plane
        if self.cfg.phase != 4:
            spawn_ground_plane(prim_path="/World/ground", cfg=GroundPlaneCfg())
        else:
            terrain_cfg = UsdFileCfg(
                usd_path="/home/angellm/SimpleRobot/other/SimpleRobot/TerrainRamps.usd",
                visible=True,
                copy_from_source=True,
            )
            spawn_from_usd(prim_path="/World/envs/env_.*/ground", cfg=terrain_cfg)

He cambiado la manera en la que se spawnea el terreno. Ahora, si la fase de entrenamiento es la 4, utilizaré el USD que he creado antes para el terreno con rampas.

Cambios en _reset_idx

def _reset_idx(self, env_ids: Sequence[int] | None):
        if env_ids is None:
            env_ids = self.robot._ALL_INDICES
        super()._reset_idx(env_ids)

        default_root_state = self.robot.data.default_root_state[env_ids] # get the default root state (position and orientation in World frame)
        default_root_state[:, :3] += self.scene.env_origins[env_ids]     # offset the position according to the environment origin
        default_root_state[:, 2] += 0.3  # SUBIR ROBOT

        # pick new commands for reset envs and normalize them just like in the setup
        if self.cfg.phase <= 1:
            self.commands[env_ids] = torch.zeros((len(env_ids), 2)).cuda()
        elif self.cfg.phase == 2:
            if random.random() < 0.3:
                self.commands[env_ids] = torch.zeros((len(env_ids), 2)).cuda()
            else:
                self.commands[env_ids, 0] = 0.3
                self.commands[env_ids, 1] = 0
        elif self.cfg.phase >= 3:
            num = len(env_ids)
            rnd = torch.rand(num, device=self.device)
            self.commands[env_ids] = 0.0
            # 20% robots quietos
            mask_zero = rnd <= 0.2 # con estos no hay que hacer nada porque ya estan a cero
            # 20% robots solo con velocidad lineal
            mask_lin = (rnd > 0.2) & (rnd <= 0.4)
            self.commands[env_ids[mask_lin], 0] = (torch.rand(mask_lin.sum(), device=self.device) * 2 - 1)
            # 60% robots velocidad lineal y angular (yaw)
            mask_full = rnd > 0.4
            self.commands[env_ids[mask_full], 0] = (torch.rand(mask_full.sum(), device=self.device) * 2 - 1)
            self.commands[env_ids[mask_full], 1] = (torch.rand(mask_full.sum(), device=self.device) * 2 - 1)

        self.robot.write_root_state_to_sim(default_root_state, env_ids)  # reset the root state of the robot

He hecho dos cambios. El primero de ellos ha sido subir la posición inicial del robot 0.3m sobre el nivel del suelo. El robot comienza en el aire y cae hasta colisionar con el terreno. En las primeras pruebas tuve problemas con las colisiones y algunos robots spawneaban por debajo del terreno y no se calculaban bien las colisiones, así que decidí levantarlo un poco.

El otro cambio ha sido este: elif self.cfg.phase >= 3: para incluir tanto la fase 3 como la 4.

Entrenamiento Fase 4

Como no hemos tocado ningún parámetro del entrenamiento más allá del terreno y la posición inicial del robot, podemos utilizar como base la política entrenada en la Fase 3 directamente:

~/IsaacLab/isaaclab.sh -p ~/SimpleRobot/scripts/rsl_rl/train_mod.py --task=Template-Simplerobot-Direct-Phase4-v0 --load_policy /home/angellm/logs/rsl_rl/simplerobot_direct/2026-01-24_21-59-22/model_9999.pt --max_iterations 2500 --headless

Run: 2026-01-29_19-54-12

En la gráfica de la izquierda puedo ver que, desde casi las primeras iteraciones, la duración de los episodios se aproxima al máximo. Esto es debido a que estamos utilizando la política de la fase 3 directamente como base para entrenar esta. Por otro lado, en la gráfica de la derecha puedo ver que, tras unas primeras iteraciones prometedoras, a partir de la número 500, la media de las recompensas cae. Probablemente esto significa que ha entrado en un espacio de soluciones que no es muy óptimo y que, seguramente, con más iteraciones logrará salir de ese espacio de soluciones y llegar a uno más óptimo. Pero como veo que la iteración 100 tiene una buena relación de tiempo por episodio y recompensa, lo elijo como checkpoint para visualizar el comportamiento de la política:

~/IsaacLab/isaaclab.sh -p ~/SimpleRobot/scripts/rsl_rl/play.py --task=Template-Simplerobot-Direct-Phase4-v0 --checkpoint=/home/angellm/logs/rsl_rl/simplerobot_direct/2026-01-29_19-54-12/model_100.pt

En la simulación veo que existen:

• Robots en equilibrio estático

• Robots que se mueven en linea recta sin girar, "escalando" planos inclinados sin caerse

• Robots que se mueven con velocidad linear y angular, navegando por planos inclinados sin caerse.

Por lo que considero que este checkpoint es válido.

Evaluación

La política entrenada se comporta como cabría esperar y se cumplen todos los objetivos marcados para esta fase.

¡Fase 4 completada con éxito!