[Question] Interactive simulation for Anymal velocity command

[AvisP]

Question

I am trying to creative an intteractive demo similar to the humanoid robot as shown in https://github.com/isaac-sim/IsaacLab/blob/main/scripts/demos/h1_locomotion.py but for Anymal on Rough terrain. I modified and have the following script. Although it is working properly, selecting a robot and pressing the the UP button it is proceeding forward. But when and trying to go left or right by pressing LEFT and RIGHT buttons it is falling down.

I am guessing it has something to do with how the direction commands are specified here

IsaacLab/scripts/demos/h1_locomotion.py

Line 134 in 6183e15

self._key_to_control = {

. I am not able to understand why for the H1 it was set like this and what it should be for the quadruped robot Anymal. Any tips would be appreciated.

import argparse
import os
import sys

sys.path.append(os.path.join(os.path.dirname(os.path.abspath(__file__)), "../.."))
import scripts.reinforcement_learning.rsl_rl.cli_args as cli_args  # isort: skip


from isaaclab.app import AppLauncher

# add argparse arguments
parser = argparse.ArgumentParser(
    description="This script demonstrates an interactive demo with the H1 rough terrain environment."
)
# append RSL-RL cli arguments
cli_args.add_rsl_rl_args(parser)
# append AppLauncher cli args
AppLauncher.add_app_launcher_args(parser)
# parse the arguments
args_cli = parser.parse_args()

# launch omniverse app
app_launcher = AppLauncher(args_cli)
simulation_app = app_launcher.app

"""Rest everything follows."""
import torch

import carb
import omni
from omni.kit.viewport.utility import get_viewport_from_window_name
from omni.kit.viewport.utility.camera_state import ViewportCameraState
from pxr import Gf, Sdf
from rsl_rl.runners import OnPolicyRunner

from isaaclab.envs import ManagerBasedRLEnv
from isaaclab.utils.math import quat_apply
from isaaclab.utils.pretrained_checkpoint import get_published_pretrained_checkpoint

from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlVecEnvWrapper

from isaaclab_tasks.manager_based.locomotion.velocity.config.anymal_c.rough_env_cfg import AnymalCRoughEnvCfg_PLAY
TASK = "Isaac-Velocity-Rough-Anymal-C-v0"
RL_LIBRARY = "rsl_rl"


class AnymalRoughDemo:
    """This class provides an interactive demo for the H1 rough terrain environment.
    It loads a pre-trained checkpoint for the Isaac-Velocity-Rough-H1-v0 task, trained with RSL RL
    and defines a set of keyboard commands for directing motion of selected robots.

    A robot can be selected from the scene through a mouse click. Once selected, the following
    keyboard controls can be used to control the robot:

    * UP: go forward
    * LEFT: turn left
    * RIGHT: turn right
    * DOWN: stop
    * C: switch between third-person and perspective views
    * ESC: exit current third-person view"""

    def __init__(self):
        """Initializes environment config designed for the interactive model and sets up the environment,
        loads pre-trained checkpoints, and registers keyboard events."""
        agent_cfg: RslRlOnPolicyRunnerCfg = cli_args.parse_rsl_rl_cfg(TASK, args_cli)
        # load the trained jit policy
        checkpoint = get_published_pretrained_checkpoint(RL_LIBRARY, TASK)
        # create envionrment
        env_cfg = AnymalCRoughEnvCfg_PLAY()
        env_cfg.scene.num_envs = 25
        env_cfg.episode_length_s = 1000000
        env_cfg.curriculum = None
        env_cfg.commands.base_velocity.ranges.lin_vel_x = (0.0, 1.0)
        env_cfg.commands.base_velocity.ranges.heading = (-1.0, 1.0)
        # wrap around environment for rsl-rl
        self.env = RslRlVecEnvWrapper(ManagerBasedRLEnv(cfg=env_cfg))
        self.device = self.env.unwrapped.device
        # load previously trained model
        ppo_runner = OnPolicyRunner(self.env, agent_cfg.to_dict(), log_dir=None, device=self.device)
        ppo_runner.load(checkpoint)
        # obtain the trained policy for inference
        self.policy = ppo_runner.get_inference_policy(device=self.device)

        self.create_camera()
        self.commands = torch.zeros(env_cfg.scene.num_envs, 4, device=self.device)
        self.commands[:, 0:3] = self.env.unwrapped.command_manager.get_command("base_velocity")
        self.set_up_keyboard()
        self._prim_selection = omni.usd.get_context().get_selection()
        self._selected_id = None
        self._previous_selected_id = None
        self._camera_local_transform = torch.tensor([-2.5, 0.0, 0.8], device=self.device)

    def create_camera(self):
        """Creates a camera to be used for third-person view."""
        stage = omni.usd.get_context().get_stage()
        self.viewport = get_viewport_from_window_name("Viewport")
        # Create camera
        self.camera_path = "/World/Camera"
        self.perspective_path = "/OmniverseKit_Persp"
        camera_prim = stage.DefinePrim(self.camera_path, "Camera")
        camera_prim.GetAttribute("focalLength").Set(8.5)
        coi_prop = camera_prim.GetProperty("omni:kit:centerOfInterest")
        if not coi_prop or not coi_prop.IsValid():
            camera_prim.CreateAttribute(
                "omni:kit:centerOfInterest", Sdf.ValueTypeNames.Vector3d, True, Sdf.VariabilityUniform
            ).Set(Gf.Vec3d(0, 0, -10))
        self.viewport.set_active_camera(self.perspective_path)

    def set_up_keyboard(self):
        """Sets up interface for keyboard input and registers the desired keys for control."""
        self._input = carb.input.acquire_input_interface()
        self._keyboard = omni.appwindow.get_default_app_window().get_keyboard()
        self._sub_keyboard = self._input.subscribe_to_keyboard_events(self._keyboard, self._on_keyboard_event)
        T = 0.5
        R = 0.5
        self._key_to_control = {
            "UP": torch.tensor([T, 0.0, 0.0, 0.0], device=self.device),
            "DOWN": torch.tensor([0.0, 0.0, 0.0, 0.0], device=self.device),
            "LEFT": torch.tensor([T, 0.0, 0.0, -R], device=self.device),
            "RIGHT": torch.tensor([T, 0.0, 0.0, R], device=self.device),
            "ZEROS": torch.tensor([0.0, 0.0, 0.0, 0.0], device=self.device),
        }

    def _on_keyboard_event(self, event):
        """Checks for a keyboard event and assign the corresponding command control depending on key pressed."""
        print("Key pressed ", event.input.name)
        if event.type == carb.input.KeyboardEventType.KEY_PRESS:
            # Arrow keys map to pre-defined command vectors to control navigation of robot
            if event.input.name in self._key_to_control:
                if self._selected_id:
                    self.commands[self._selected_id] = self._key_to_control[event.input.name]
            # Escape key exits out of the current selected robot view
            elif event.input.name == "ESCAPE":
                self._prim_selection.clear_selected_prim_paths()
            # C key swaps between third-person and perspective views
            elif event.input.name == "C":
                if self._selected_id is not None:
                    if self.viewport.get_active_camera() == self.camera_path:
                        self.viewport.set_active_camera(self.perspective_path)
                    else:
                        self.viewport.set_active_camera(self.camera_path)
        # On key release, the robot stops moving
        elif event.type == carb.input.KeyboardEventType.KEY_RELEASE:
            if self._selected_id:
                self.commands[self._selected_id] = self._key_to_control["ZEROS"]

    def update_selected_object(self):
        """Determines which robot is currently selected and whether it is a valid H1 robot.
        For valid robots, we enter the third-person view for that robot.
        When a new robot is selected, we reset the command of the previously selected
        to continue random commands."""

        self._previous_selected_id = self._selected_id
        selected_prim_paths = self._prim_selection.get_selected_prim_paths()
        if len(selected_prim_paths) == 0:
            self._selected_id = None
            self.viewport.set_active_camera(self.perspective_path)
        elif len(selected_prim_paths) > 1:
            print("Multiple prims are selected. Please only select one!")
        else:
            prim_splitted_path = selected_prim_paths[0].split("/")
            # a valid robot was selected, update the camera to go into third-person view
            if len(prim_splitted_path) >= 4 and prim_splitted_path[3][0:4] == "env_":
                self._selected_id = int(prim_splitted_path[3][4:])
                if self._previous_selected_id != self._selected_id:
                    self.viewport.set_active_camera(self.camera_path)
                self._update_camera()
            else:
                print("The selected prim was not a Anymal robot")

        # Reset commands for previously selected robot if a new one is selected
        if self._previous_selected_id is not None and self._previous_selected_id != self._selected_id:
            self.env.unwrapped.command_manager.reset([self._previous_selected_id])
            self.commands[:, 0:3] = self.env.unwrapped.command_manager.get_command("base_velocity")

    def _update_camera(self):
        """Updates the per-frame transform of the third-person view camera to follow
        the selected robot's torso transform."""

        base_pos = self.env.unwrapped.scene["robot"].data.root_pos_w[self._selected_id, :]  # - env.scene.env_origins
        base_quat = self.env.unwrapped.scene["robot"].data.root_quat_w[self._selected_id, :]

        camera_pos = quat_apply(base_quat, self._camera_local_transform) + base_pos

        camera_state = ViewportCameraState(self.camera_path, self.viewport)
        eye = Gf.Vec3d(camera_pos[0].item(), camera_pos[1].item(), camera_pos[2].item())
        target = Gf.Vec3d(base_pos[0].item(), base_pos[1].item(), base_pos[2].item() + 0.6)
        camera_state.set_position_world(eye, True)
        camera_state.set_target_world(target, True)


def main():
    """Main function."""
    demo_anymal = AnymalRoughDemo()
    obs, _ = demo_anymal.env.reset()
    while simulation_app.is_running():
        # check for selected robots
        demo_anymal.update_selected_object()
        with torch.inference_mode():
            action = demo_anymal.policy(obs)
            obs, _, _, _ = demo_anymal.env.step(action)
            print("Action ", action)
            print("Obserrvation ", obs)
            # overwrite command based on keyboard input
            obs[:, 9:13] = demo_anymal.commands


if __name__ == "__main__":
    main()
    simulation_app.close()

[RandomOakForest]

Thanks for posting this. Could you verify that the input of the command manager is correct, verify the mapping for the keys is ok?

[AvisP]

@RandomOakForest Thanks for your suggestion. By command_manager are you referring to these line?

self.env.unwrapped.command_manager.reset([self._previous_selected_id])
self.commands[:, 0:3] = self.env.unwrapped.command_manager.get_command("base_velocity")

The values get set in self.commands when I am selecting a bot and pressing buttons they get set on this line to predefined fixed values

IsaacLab/scripts/demos/h1_locomotion.py

Line 148 in 6183e15

self.commands[self._selected_id] = self._key_to_control[event.input.name]

I managed to stabilize the movement a bit by changing these lines. I don't fully understand what values to put where and they take values from 1 to -1. Is it possible to pass a vector of floating points instead of binding the buttons to a fixed vector? Kindly advise, I am new to this

T = 1.0
R = 0.5
F = 0.8
self._key_to_control = {
    "UP": torch.tensor([T, 0.0, 0.0, 0.0], device=self.device),
    "DOWN": torch.tensor([-T, 0.0, 0.0, -R], device=self.device),
    "LEFT": torch.tensor([T, 0.6, -0.6, 0], device=self.device),
    "RIGHT": torch.tensor([T, -0.6, 0.6, 0], device=self.device),
    "ZEROS": torch.tensor([0.0, 0.0, 0.0, 0.0], device=self.device),
}

[RandomOakForest]

Thank you for following up. I will move this to our Discussions section. A summary that I'm still reviewing with a few things to consider follows.

The original H1 control tensor [T, 0.0, 0.0, ±R] corresponds to:

# [forward_vel, lateral_vel, vertical_vel, yaw_rate]
self._key_to_control = {
    "UP": [T, 0, 0, 0],    # Forward
    "LEFT": [T, 0, 0, -R], # Forward + CCW rotation
    "RIGHT": [T, 0, 0, R]  # Forward + CW rotation
}

This works for bipeds because:

1. Primary stability comes from sagittal plane motion ¹²
2. Turning is achieved through upper body rotation + foot placement ³¹

For stable quadruped turning:

# Recommended structure for ANYmal:
# [lin_vel_x, lin_vel_y, lin_vel_z, ang_vel_z]
self._key_to_control = {
    "UP": [T, 0, 0, 0],    # Forward
    "LEFT": [0, S, 0, -R], # Lateral left + CCW rotation
    "RIGHT": [0, -S, 0, R] # Lateral right + CW rotation
}

Where:

• S = Lateral velocity command (0.1-0.3 m/s)
• R = Reduced yaw rate (0.1-0.2 rad/s vs H1's 0.3 rad/s) ¹²

Critical Implementation Differences

1. Separate Lateral/Yaw Commands
   Quadrupeds need explicit lateral velocity for crab walking:

# Bad (H1 style):
"LEFT": [T, 0, 0, -R]  # Combines forward + yaw

# Good (Anymal):
"LEFT": [0, S, 0, -R]  # Pure lateral + yaw

2. Velocity Scaling Factors
   Use values from the ANYmal policy's env.yaml: ¹²

# Example from ANYmal rough terrain policy
command:
  lin_vel_x: [-1.0, 1.0]  # Forward/backward
  lin_vel_y: [-0.5, 0.5]  # Lateral 
  ang_vel_yaw: [-1.0, 1.0] # Rotation

Scale commands accordingly:

T = 0.5  # 50% of max forward velocity
S = 0.3  # 60% of max lateral velocity 
R = 0.4  # 40% of max yaw rate

3. Terrain Adaptation
   Add terrain height sampling for rough terrain:

# Required for ANYmal rough terrain policy [^7][^9]
obs = [
    terrain_height,  # Missing in H1 implementation
    base_lin_vel,
    base_ang_vel,
    joint_positions
]

Recommended Fixes

1. Modify Control Mapping:

self._key_to_control = {
    "UP": torch.tensor([T, 0.0, 0.0, 0.0], device=device),
    "LEFT": torch.tensor([0.0, S, 0.0, -R], device=device),
    "RIGHT": torch.tensor([0.0, -S, 0.0, R], device=device),
    "DOWN": torch.tensor([-T, 0.0, 0.0, 0.0], device=device),
    "ZEROS": torch.zeros(4, device=device)
}

2. Adjust Velocity Limits (from ¹²):

# Rough terrain recommended limits
MAX_LIN_VEL = 0.8  # m/s (vs H1's 1.5 m/s)
MAX_ANG_VEL = 0.6  # rad/s (vs H1's 1.0 rad/s)

3. Implement Terrain Sampling:

# Add to observation computation
terrain_height = self._get_terrain_height_samples()
obs = torch.cat([base_vel, ang_vel, terrain_height, joint_pos])

These changes account for the ANYmal's quadrupedal dynamics and rough terrain requirements. Monitor the lateral foot forces during turns - they should stay below 150N for stable operation ³¹.

References

Footnotes

1. https://docs.isaacsim.omniverse.nvidia.com/4.5.0/robot_simulation/ext_isaacsim_robot_policy_example.html ↩ ↩² ↩³ ↩⁴ ↩⁵ ↩⁶

2. https://isaac-sim.github.io/IsaacLab/main/source/overview/environments.html ↩ ↩² ↩³ ↩⁴

3. https://docs.isaacsim.omniverse.nvidia.com/4.5.0/isaac_lab_tutorials/tutorial_policy_deployment.html ↩ ↩²