diff --git a/.github/workflows/linting_auto.yml b/.github/workflows/linting_auto.yml index c4a6700..46d19da 100644 --- a/.github/workflows/linting_auto.yml +++ b/.github/workflows/linting_auto.yml @@ -26,7 +26,7 @@ jobs: - name: Set up Python uses: actions/setup-python@v1 with: - python-version: 3.9.22 + python-version: 3.10.18 - name: Install Python dependencies run: pip install autopep8 clang-format diff --git a/autonomy/simulation/CMakeLists.txt b/autonomy/simulation/CMakeLists.txt new file mode 100644 index 0000000..7a11161 --- /dev/null +++ b/autonomy/simulation/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_minimum_required(VERSION 3.10) +project(sample_msgs) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +ament_export_dependencies(rosidl_default_runtime) +ament_package() \ No newline at end of file diff --git a/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/config/extension.toml b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/config/extension.toml new file mode 100644 index 0000000..686f819 --- /dev/null +++ b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/config/extension.toml @@ -0,0 +1,22 @@ +[package] + +# Note: Semantic Versioning is used: https://semver.org/ +version = "0.10.31" + +# Description +title = "Isaac Lab Environments" +description="Extension containing suite of environments for robot learning." +readme = "docs/README.md" +repository = "https://github.com/isaac-sim/IsaacLab" +category = "robotics" +keywords = ["robotics", "rl", "il", "learning"] + +[dependencies] +"isaaclab" = {} +"isaaclab_assets" = {} + +[core] +reloadable = false + +[[python.module]] +name = "isaaclab_tasks" diff --git a/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/modelCfg/humanoid.py b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/modelCfg/humanoid.py new file mode 100644 index 0000000..a6840b6 --- /dev/null +++ b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/modelCfg/humanoid.py @@ -0,0 +1,44 @@ +# Code for specifying custom model parameters + +import isaaclab.sim as sim_utils +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.assets.articulation import ArticulationCfg + +HAND_CFG = ArticulationCfg( + spawn=sim_utils.UsdFileCfg( + usd_path="/home/hy/Downloads/Humanoid_Wato/ModelAssets/hand.usd", + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=False, + max_depenetration_velocity=5.0, + ), + activate_contact_sensors=False, + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + "Revolute_1": 0.0, + "Revolute_2": 0.0, + "Revolute_3": 0.0, + "Revolute_4": 0.0, + "Revolute_5": 0.0, + "Revolute_6": 0.0, + "Revolute_7": 0.0, + "Revolute_8": 0.0, + "Revolute_9": 0.0, + "Revolute_10": 0.0, + "Revolute_11": 0.0, + "Revolute_12": 0.0, + "Revolute_13": 0.0, + "Revolute_14": 0.0, + "Revolute_15": 0.0, + } + ), + actuators={ + "arm": ImplicitActuatorCfg( + joint_names_expr=[".*"], + # velocity_limit=100.0, + # effort_limit=87.0, + stiffness=0.5, + damping=0.5, + ), + }, +) diff --git a/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/setup.py b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/setup.py new file mode 100644 index 0000000..a37d989 --- /dev/null +++ b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/setup.py @@ -0,0 +1,55 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Installation script for the 'isaaclab_tasks' python package.""" + +import os +import toml + +from setuptools import setup + +# Obtain the extension data from the extension.toml file +EXTENSION_PATH = os.path.dirname(os.path.realpath(__file__)) +# Read the extension.toml file +EXTENSION_TOML_DATA = toml.load(os.path.join(EXTENSION_PATH, "config", "extension.toml")) + +# Minimum dependencies required prior to installation +INSTALL_REQUIRES = [ + # generic + "numpy", + "torch==2.5.1", + "torchvision>=0.14.1", # ensure compatibility with torch 1.13.1 + # 5.26.0 introduced a breaking change, so we restricted it for now. + # See issue https://github.com/tensorflow/tensorboard/issues/6808 for details. + "protobuf>=3.20.2, < 5.0.0", + # basic logger + "tensorboard", +] + +PYTORCH_INDEX_URL = ["https://download.pytorch.org/whl/cu118"] + +# Installation operation +setup( + name="skyentific_poclegs", + author="Isaac Lab Project Developers", + maintainer="Isaac Lab Project Developers", + url=EXTENSION_TOML_DATA["package"]["repository"], + version=EXTENSION_TOML_DATA["package"]["version"], + description=EXTENSION_TOML_DATA["package"]["description"], + keywords=EXTENSION_TOML_DATA["package"]["keywords"], + include_package_data=True, + python_requires=">=3.10", + install_requires=INSTALL_REQUIRES, + dependency_links=PYTORCH_INDEX_URL, + packages=["skyentific_poclegs"], + classifiers=[ + "Natural Language :: English", + "Programming Language :: Python :: 3.10", + "Isaac Sim :: 4.5.0", + ], + zip_safe=False, +) + +# From Isaac Lab source/isaaclab_tasks/setup.py diff --git a/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/__init__.py b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/__init__.py new file mode 100644 index 0000000..acca6c4 --- /dev/null +++ b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/__init__.py @@ -0,0 +1,21 @@ +# Copyright (c) 2022-2024, The Berkeley Humanoid Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Package containing task implementations for various robotic environments.""" + +import os +import toml + +from isaaclab_tasks.utils import import_packages + +## +# Register Gym environments. +## + + +# The blacklist is used to prevent importing configs from sub-packages +_BLACKLIST_PKGS = ["utils"] +# Import all configs in this package +import_packages(__name__, _BLACKLIST_PKGS) diff --git a/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/manipulation/__init__.py b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/manipulation/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/manipulation/config/HumanoidRLEnv/__init__.py b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/manipulation/config/HumanoidRLEnv/__init__.py new file mode 100644 index 0000000..37d9b69 --- /dev/null +++ b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/manipulation/config/HumanoidRLEnv/__init__.py @@ -0,0 +1,26 @@ +import gymnasium as gym +from . import agents + +gym.register( + id="Isaac-Reach-Humanoid-Hand-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.joint_pos_env_cfg:HumanoidHandReachEnvCfg", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:HumanoidHandReachPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Reach-UR10-Play-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.joint_pos_env_cfg:HumanoidHandReachEnvCfg_PLAY", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:HumanoidHandReachPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_ppo_cfg.yaml", + }, +) diff --git a/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/manipulation/config/HumanoidRLEnv/agents/__init__.py b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/manipulation/config/HumanoidRLEnv/agents/__init__.py new file mode 100644 index 0000000..732feda --- /dev/null +++ b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/manipulation/config/HumanoidRLEnv/agents/__init__.py @@ -0,0 +1,6 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# From Isaac Lab source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config//agents/__init__.py diff --git a/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/manipulation/config/HumanoidRLEnv/agents/rsl_rl_ppo_cfg.py b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/manipulation/config/HumanoidRLEnv/agents/rsl_rl_ppo_cfg.py new file mode 100644 index 0000000..ec8a196 --- /dev/null +++ b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/manipulation/config/HumanoidRLEnv/agents/rsl_rl_ppo_cfg.py @@ -0,0 +1,33 @@ +from isaaclab.utils import configclass +from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg + + +@configclass +class HumanoidHandReachPPORunnerCfg(RslRlOnPolicyRunnerCfg): + num_steps_per_env = 24 + max_iterations = 1000 + save_interval = 50 + experiment_name = "reach_humanoid_hand" + run_name = "" + resume = False + empirical_normalization = False + policy = RslRlPpoActorCriticCfg( + init_noise_std=1.0, + actor_hidden_dims=[64, 64], + critic_hidden_dims=[64, 64], + activation="elu", + ) + algorithm = RslRlPpoAlgorithmCfg( + value_loss_coef=1.0, + use_clipped_value_loss=True, + clip_param=0.2, + entropy_coef=0.01, + num_learning_epochs=8, + num_mini_batches=4, + learning_rate=1.0e-3, + schedule="adaptive", + gamma=0.99, + lam=0.95, + desired_kl=0.01, + max_grad_norm=1.0, + ) diff --git a/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/manipulation/config/HumanoidRLEnv/joint_pos_env_cfg.py b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/manipulation/config/HumanoidRLEnv/joint_pos_env_cfg.py new file mode 100644 index 0000000..61c186e --- /dev/null +++ b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/manipulation/config/HumanoidRLEnv/joint_pos_env_cfg.py @@ -0,0 +1,111 @@ +import math +from isaaclab.utils import configclass +import HumanoidRLPackage.HumanoidRLSetup.tasks.manipulation.mdp as mdp +from HumanoidRLPackage.HumanoidRLSetup.tasks.manipulation.reach_env_cfg import ReachEnvCfg +from HumanoidRLPackage.HumanoidRLSetup.modelCfg.universal_robots import UR10_CFG +from HumanoidRLPackage.HumanoidRLSetup.modelCfg.humanoid import HAND_CFG + + +@configclass +class HumanoidHandReachEnvCfg(ReachEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # self.scene.robot = UR10_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + self.scene.robot = HAND_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + + marker_scale = 0.02 + + self.commands.ee_pose.goal_pose_visualizer_cfg.markers["frame"].scale = ( + marker_scale, marker_scale, marker_scale) + self.commands.ee_pose.current_pose_visualizer_cfg.markers["frame"].scale = ( + marker_scale, marker_scale, marker_scale) + + self.commands.ee_pose_2.goal_pose_visualizer_cfg.markers["frame"].scale = ( + marker_scale, marker_scale, marker_scale) + self.commands.ee_pose_2.current_pose_visualizer_cfg.markers["frame"].scale = ( + marker_scale, marker_scale, marker_scale) + + self.commands.ee_pose_3.goal_pose_visualizer_cfg.markers["frame"].scale = ( + marker_scale, marker_scale, marker_scale) + self.commands.ee_pose_3.current_pose_visualizer_cfg.markers["frame"].scale = ( + marker_scale, marker_scale, marker_scale) + + self.commands.ee_pose_4.goal_pose_visualizer_cfg.markers["frame"].scale = ( + marker_scale, marker_scale, marker_scale) + self.commands.ee_pose_4.current_pose_visualizer_cfg.markers["frame"].scale = ( + marker_scale, marker_scale, marker_scale) + + self.commands.ee_pose_5.goal_pose_visualizer_cfg.markers["frame"].scale = ( + marker_scale, marker_scale, marker_scale) + self.commands.ee_pose_5.current_pose_visualizer_cfg.markers["frame"].scale = ( + marker_scale, marker_scale, marker_scale) + + # override events + self.events.reset_robot_joints.params["position_range"] = (0.75, 1.25) + # override rewards + self.rewards.end_effector_position_tracking.params["asset_cfg"].body_names = ["TIP_B_1"] + self.rewards.end_effector_position_tracking_fine_grained.params["asset_cfg"].body_names = [ + "TIP_B_1"] + # self.rewards.end_effector_orientation_tracking.params["asset_cfg"].body_names = ["TIP_B_1"] + + self.rewards.end_effector_2_position_tracking.params["asset_cfg"].body_names = ["TIP_B_2"] + self.rewards.end_effector_2_position_tracking_fine_grained.params["asset_cfg"].body_names = [ + "TIP_B_2"] + # self.rewards.end_effector_2_orientation_tracking.params["asset_cfg"].body_names = ["TIP_B_2"] + + self.rewards.end_effector_3_position_tracking.params["asset_cfg"].body_names = ["TIP_B_3"] + self.rewards.end_effector_3_position_tracking_fine_grained.params["asset_cfg"].body_names = [ + "TIP_B_3"] + # self.rewards.end_effector_3_orientation_tracking.params["asset_cfg"].body_names = ["TIP_B_3"] + + self.rewards.end_effector_4_position_tracking.params["asset_cfg"].body_names = ["TIP_B_4"] + self.rewards.end_effector_4_position_tracking_fine_grained.params["asset_cfg"].body_names = [ + "TIP_B_4"] + # self.rewards.end_effector_4_orientation_tracking.params["asset_cfg"].body_names = ["TIP_B_4"] + + self.rewards.end_effector_5_position_tracking.params["asset_cfg"].body_names = ["TIP_B_5"] + self.rewards.end_effector_5_position_tracking_fine_grained.params["asset_cfg"].body_names = [ + "TIP_B_5"] + # self.rewards.end_effector_5_orientation_tracking.params["asset_cfg"].body_names = ["TIP_B_5"] + + # override actions + self.actions.arm_action = mdp.JointPositionActionCfg( + asset_name="robot", joint_names=[".*"], scale=0.5, use_default_offset=True + ) + # override command generator body + # end-effector is along x-direction + self.commands.ee_pose.body_name = "TIP_B_1" + # self.commands.ee_pose.ranges.pitch = (math.pi / 2, math.pi / 2) # this rotate the pose + + self.commands.ee_pose_2.body_name = "TIP_B_2" + # self.commands.ee_pose_2.ranges.pitch = (math.pi / 2, math.pi / 2) + + self.commands.ee_pose_3.body_name = "TIP_B_3" + # self.commands.ee_pose_3.ranges.pitch = (math.pi / 2, math.pi / 2) + + self.commands.ee_pose_4.body_name = "TIP_B_4" + # self.commands.ee_pose_4.ranges.pitch = (math.pi / 2, math.pi / 2) + + self.commands.ee_pose_5.body_name = "TIP_B_5" + # self.commands.ee_pose_5.ranges.pitch = (math.pi / 2, math.pi / 2) + + # self.commands.ee_pose.ranges.yaw = (-math.pi / 2, -math.pi / 2) + # self.commands.ee_pose_2.ranges.yaw = (-math.pi / 2, -math.pi / 2) + + +@configclass +class HumanoidHandReachEnvCfg_PLAY(HumanoidHandReachEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + # make a smaller scene for play + self.scene.num_envs = 50 + self.scene.env_spacing = 2.5 + # disable randomization for play + self.observations.policy.enable_corruption = False + +# (env_isaaclab) hy@hy-LOQ-15IRX9:~/Downloads/Humanoid_Wato/HumanoidRL$ PYTHONPATH=$(pwd) /home/hy/IsaacLab/isaaclab.sh -p HumanoidRLPackage/rsl_rl_scripts/train.py --task=Isaac-Reach-Humanoid-Hand-v0 --headless + +# (env_isaaclab) hy@hy-LOQ-15IRX9:~/Downloads/Humanoid_Wato/HumanoidRL$ PYTHONPATH=$(pwd) /home/hy/IsaacLab/isaaclab.sh -p HumanoidRLPackage/rsl_rl_scripts/play.py --task=Isaac-Reach-Humanoid-Hand-v0 --num_envs=1 diff --git a/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/manipulation/config/__init__.py b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/manipulation/config/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/manipulation/mdp/__init__.py b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/manipulation/mdp/__init__.py new file mode 100644 index 0000000..cb8b07b --- /dev/null +++ b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/manipulation/mdp/__init__.py @@ -0,0 +1,12 @@ +# Copyright (c) 2022-2024, The Berkeley Humanoid Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""This sub-module contains the functions that are specific to the locomotion environments.""" + +from isaaclab.envs.mdp import * # noqa: F401, F403 + +from .rewards import * # noqa: F401, F403 + +# From Isaac Lab source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/mdp/__init__.py diff --git a/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/manipulation/mdp/rewards.py b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/manipulation/mdp/rewards.py new file mode 100644 index 0000000..433d69a --- /dev/null +++ b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/manipulation/mdp/rewards.py @@ -0,0 +1,71 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import torch +from typing import TYPE_CHECKING + +from isaaclab.assets import RigidObject +from isaaclab.managers import SceneEntityCfg +from isaaclab.utils.math import combine_frame_transforms, quat_error_magnitude, quat_mul + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + + +def position_command_error(env: ManagerBasedRLEnv, command_name: str, asset_cfg: SceneEntityCfg) -> torch.Tensor: + """Penalize tracking of the position error using L2-norm. + + The function computes the position error between the desired position (from the command) and the + current position of the asset's body (in world frame). The position error is computed as the L2-norm + of the difference between the desired and current positions. + """ + # extract the asset (to enable type hinting) + asset: RigidObject = env.scene[asset_cfg.name] + command = env.command_manager.get_command(command_name) + # obtain the desired and current positions + des_pos_b = command[:, :3] + des_pos_w, _ = combine_frame_transforms( + asset.data.root_state_w[:, :3], asset.data.root_state_w[:, 3:7], des_pos_b) + curr_pos_w = asset.data.body_state_w[:, asset_cfg.body_ids[0], :3] # type: ignore + return torch.norm(curr_pos_w - des_pos_w, dim=1) + + +def position_command_error_tanh( + env: ManagerBasedRLEnv, std: float, command_name: str, asset_cfg: SceneEntityCfg +) -> torch.Tensor: + """Reward tracking of the position using the tanh kernel. + + The function computes the position error between the desired position (from the command) and the + current position of the asset's body (in world frame) and maps it with a tanh kernel. + """ + # extract the asset (to enable type hinting) + asset: RigidObject = env.scene[asset_cfg.name] + command = env.command_manager.get_command(command_name) + # obtain the desired and current positions + des_pos_b = command[:, :3] + des_pos_w, _ = combine_frame_transforms( + asset.data.root_state_w[:, :3], asset.data.root_state_w[:, 3:7], des_pos_b) + curr_pos_w = asset.data.body_state_w[:, asset_cfg.body_ids[0], :3] # type: ignore + distance = torch.norm(curr_pos_w - des_pos_w, dim=1) + return 1 - torch.tanh(distance / std) + + +def orientation_command_error(env: ManagerBasedRLEnv, command_name: str, asset_cfg: SceneEntityCfg) -> torch.Tensor: + """Penalize tracking orientation error using shortest path. + + The function computes the orientation error between the desired orientation (from the command) and the + current orientation of the asset's body (in world frame). The orientation error is computed as the shortest + path between the desired and current orientations. + """ + # extract the asset (to enable type hinting) + asset: RigidObject = env.scene[asset_cfg.name] + command = env.command_manager.get_command(command_name) + # obtain the desired and current orientations + des_quat_b = command[:, 3:7] + des_quat_w = quat_mul(asset.data.root_state_w[:, 3:7], des_quat_b) + curr_quat_w = asset.data.body_state_w[:, asset_cfg.body_ids[0], 3:7] # type: ignore + return quat_error_magnitude(curr_quat_w, des_quat_w) diff --git a/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/manipulation/reach_env_cfg.py b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/manipulation/reach_env_cfg.py new file mode 100644 index 0000000..45646ef --- /dev/null +++ b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/manipulation/reach_env_cfg.py @@ -0,0 +1,343 @@ +from dataclasses import MISSING + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.envs import ManagerBasedRLEnvCfg +from isaaclab.managers import ActionTermCfg as ActionTerm +from isaaclab.managers import CurriculumTermCfg as CurrTerm +from isaaclab.managers import EventTermCfg as EventTerm +from isaaclab.managers import ObservationGroupCfg as ObsGroup +from isaaclab.managers import ObservationTermCfg as ObsTerm +from isaaclab.managers import RewardTermCfg as RewTerm +from isaaclab.managers import SceneEntityCfg +from isaaclab.managers import TerminationTermCfg as DoneTerm +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR +from isaaclab.utils.noise import AdditiveUniformNoiseCfg as Unoise + +import HumanoidRLPackage.HumanoidRLSetup.tasks.manipulation.mdp as mdp + + +@configclass +class ReachSceneCfg(InteractiveSceneCfg): + """Configuration for the scene with a robotic arm.""" + + # world + ground = AssetBaseCfg( + prim_path="/World/ground", + spawn=sim_utils.GroundPlaneCfg(), + init_state=AssetBaseCfg.InitialStateCfg(pos=(0.0, 0.0, -1.05)), + ) + + table = AssetBaseCfg( + prim_path="{ENV_REGEX_NS}/Table", + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/SeattleLabTable/table_instanceable.usd", + ), + init_state=AssetBaseCfg.InitialStateCfg( + pos=(0.55, 0.0, 0.0), rot=(0.70711, 0.0, 0.0, 0.70711)), + ) + + # robots + robot: ArticulationCfg = MISSING + + # lights + light = AssetBaseCfg( + prim_path="/World/light", + spawn=sim_utils.DomeLightCfg(color=(0.75, 0.75, 0.75), intensity=2500.0), + ) + + +## +# MDP settings +## + + +@configclass +class CommandsCfg: + """Command terms for the MDP.""" + + ee_pose = mdp.UniformPoseCommandCfg( + asset_name="robot", + body_name="TIP_B_1", # end_effector name + resampling_time_range=(4.0, 4.0), + debug_vis=True, + ranges=mdp.UniformPoseCommandCfg.Ranges( # within task space of finger + pos_x=(0.07346, 0.07346), + pos_y=(0.13, 0.1455), + pos_z=(0.09, 0.1005), + # no rotational variation + roll=(0.0, 0.0), + pitch=(0.0, 0.0), + yaw=(0.0, 0.0), + ), + ) + + ee_pose_2 = mdp.UniformPoseCommandCfg( + asset_name="robot", + body_name="TIP_B_2", + resampling_time_range=(4.0, 4.0), + debug_vis=True, + ranges=mdp.UniformPoseCommandCfg.Ranges( + pos_x=(0.04746, 0.04746), + # pos_y=(0.135, 0.14), + pos_y=(0.15406, 0.15558), + # pos_z=(0.09, 0.136), + pos_z=(0.098, 0.10058), + roll=(0.0, 0.0), + pitch=(0.0, 0.0), + yaw=(0.0, 0.0), + ), + ) + + ee_pose_3 = mdp.UniformPoseCommandCfg( + asset_name="robot", + body_name="TIP_B_3", + resampling_time_range=(4.0, 4.0), + debug_vis=True, + ranges=mdp.UniformPoseCommandCfg.Ranges( + pos_x=(0.01996, 0.01996), + pos_y=(0.134, 0.145), + pos_z=(0.09, 0.11), + roll=(0.0, 0.0), + pitch=(0.0, 0.0), + yaw=(0.0, 0.0), + ), + ) + + ee_pose_4 = mdp.UniformPoseCommandCfg( + asset_name="robot", + body_name="TIP_B_4", + resampling_time_range=(4.0, 4.0), + debug_vis=True, + ranges=mdp.UniformPoseCommandCfg.Ranges( + pos_x=(-0.00754, -0.00754), + pos_y=(0.119, 0.13), + pos_z=(0.09, 0.1), + roll=(0.0, 0.0), + pitch=(0.0, 0.0), + yaw=(0.0, 0.0), + ), + ) + + ee_pose_5 = mdp.UniformPoseCommandCfg( + asset_name="robot", + body_name="TIP_B_5", + resampling_time_range=(4.0, 4.0), + debug_vis=True, + ranges=mdp.UniformPoseCommandCfg.Ranges( + pos_x=(0.0869, 0.0869), + pos_y=(0.04, 0.052), + pos_z=(0.17, 0.17248), + roll=(0.0, 0.0), + pitch=(0.0, 0.0), + yaw=(0.0, 0.0), + ), + ) + + +@configclass +class ActionsCfg: + """Action specifications for the MDP.""" + + arm_action: ActionTerm = MISSING + gripper_action: ActionTerm | None = None + + +@configclass +class ObservationsCfg: + """Observation specifications for the MDP.""" + + @configclass + class PolicyCfg(ObsGroup): + """Observations for policy group.""" + + # observation terms (order preserved) + joint_pos = ObsTerm(func=mdp.joint_pos_rel, noise=Unoise(n_min=-0.01, n_max=0.01)) + joint_vel = ObsTerm(func=mdp.joint_vel_rel, noise=Unoise(n_min=-0.01, n_max=0.01)) + + pose_command = ObsTerm(func=mdp.generated_commands, params={"command_name": "ee_pose"}) + pose_command_2 = ObsTerm(func=mdp.generated_commands, params={"command_name": "ee_pose_2"}) + pose_command_3 = ObsTerm(func=mdp.generated_commands, params={"command_name": "ee_pose_3"}) + pose_command_4 = ObsTerm(func=mdp.generated_commands, params={"command_name": "ee_pose_4"}) + pose_command_5 = ObsTerm(func=mdp.generated_commands, params={"command_name": "ee_pose_5"}) + + actions = ObsTerm(func=mdp.last_action) + + def __post_init__(self): + self.enable_corruption = True + self.concatenate_terms = True + + # observation groups + policy: PolicyCfg = PolicyCfg() + + +@configclass +class EventCfg: + """Configuration for events.""" + + reset_robot_joints = EventTerm( + func=mdp.reset_joints_by_scale, + mode="reset", + params={ + "position_range": (0.5, 1.5), + "velocity_range": (0.0, 0.0), + }, + ) + + +@configclass +class RewardsCfg: + """Reward terms for the MDP.""" + + # task terms + end_effector_position_tracking = RewTerm( + func=mdp.position_command_error, + weight=-0.2, + params={"asset_cfg": SceneEntityCfg( + "robot", body_names="TIP_B_1"), "command_name": "ee_pose"}, + ) + end_effector_2_position_tracking = RewTerm( + func=mdp.position_command_error, + weight=-0.2, + params={"asset_cfg": SceneEntityCfg( + "robot", body_names="TIP_B_2"), "command_name": "ee_pose_2"}, + ) + end_effector_3_position_tracking = RewTerm( + func=mdp.position_command_error, + weight=-0.2, + params={"asset_cfg": SceneEntityCfg( + "robot", body_names="TIP_B_3"), "command_name": "ee_pose_3"}, + ) + end_effector_4_position_tracking = RewTerm( + func=mdp.position_command_error, + weight=-0.2, + params={"asset_cfg": SceneEntityCfg( + "robot", body_names="TIP_B_4"), "command_name": "ee_pose_4"}, + ) + end_effector_5_position_tracking = RewTerm( + func=mdp.position_command_error, + weight=-0.2, + params={"asset_cfg": SceneEntityCfg( + "robot", body_names="TIP_B_5"), "command_name": "ee_pose_5"}, + ) + + end_effector_position_tracking_fine_grained = RewTerm( + func=mdp.position_command_error_tanh, + weight=0.1, + params={"asset_cfg": SceneEntityCfg( + "robot", body_names="TIP_B_1"), "std": 0.1, "command_name": "ee_pose"}, + ) + end_effector_2_position_tracking_fine_grained = RewTerm( + func=mdp.position_command_error_tanh, + weight=0.1, + params={"asset_cfg": SceneEntityCfg( + "robot", body_names="TIP_B_2"), "std": 0.1, "command_name": "ee_pose_2"}, + ) + end_effector_3_position_tracking_fine_grained = RewTerm( + func=mdp.position_command_error_tanh, + weight=0.1, + params={"asset_cfg": SceneEntityCfg( + "robot", body_names="TIP_B_3"), "std": 0.1, "command_name": "ee_pose_3"}, + ) + end_effector_4_position_tracking_fine_grained = RewTerm( + func=mdp.position_command_error_tanh, + weight=0.1, + params={"asset_cfg": SceneEntityCfg( + "robot", body_names="TIP_B_4"), "std": 0.1, "command_name": "ee_pose_4"}, + ) + end_effector_5_position_tracking_fine_grained = RewTerm( + func=mdp.position_command_error_tanh, + weight=0.1, + params={"asset_cfg": SceneEntityCfg( + "robot", body_names="TIP_B_5"), "std": 0.1, "command_name": "ee_pose_5"}, + ) + + # end_effector_orientation_tracking = RewTerm( + # func=mdp.orientation_command_error, + # weight=-0.1, + # params={"asset_cfg": SceneEntityCfg("robot", body_names="TIP_B_1"), "command_name": "ee_pose"}, + # ) + # end_effector_2_orientation_tracking = RewTerm( + # func=mdp.orientation_command_error, + # weight=-0.1, + # params={"asset_cfg": SceneEntityCfg("robot", body_names="TIP_B_2"), "command_name": "ee_pose_2"}, + # ) + # end_effector_3_orientation_tracking = RewTerm( + # func=mdp.orientation_command_error, + # weight=-0.1, + # params={"asset_cfg": SceneEntityCfg("robot", body_names="TIP_B_3"), "command_name": "ee_pose_3"}, + # ) + # end_effector_4_orientation_tracking = RewTerm( + # func=mdp.orientation_command_error, + # weight=-0.1, + # params={"asset_cfg": SceneEntityCfg("robot", body_names="TIP_B_4"), "command_name": "ee_pose_4"}, + # ) + # end_effector_5_orientation_tracking = RewTerm( + # func=mdp.orientation_command_error, + # weight=-0.1, + # params={"asset_cfg": SceneEntityCfg("robot", body_names="TIP_B_5"), "command_name": "ee_pose_5"}, + # ) + + # action penalty + action_rate = RewTerm(func=mdp.action_rate_l2, weight=-0.0001) + joint_vel = RewTerm( + func=mdp.joint_vel_l2, + weight=-0.0001, + params={"asset_cfg": SceneEntityCfg("robot")}, + ) + + +@configclass +class TerminationsCfg: + """Termination terms for the MDP.""" + + time_out = DoneTerm(func=mdp.time_out, time_out=True) + + +@configclass +class CurriculumCfg: + """Curriculum terms for the MDP.""" + + action_rate = CurrTerm( + func=mdp.modify_reward_weight, params={ + "term_name": "action_rate", "weight": -0.005, "num_steps": 4500} + ) + + joint_vel = CurrTerm( + func=mdp.modify_reward_weight, params={ + "term_name": "joint_vel", "weight": -0.001, "num_steps": 4500} + ) + + +## +# Environment configuration +## + + +@configclass +class ReachEnvCfg(ManagerBasedRLEnvCfg): + """Configuration for the reach end-effector pose tracking environment.""" + + # Scene settings + scene: ReachSceneCfg = ReachSceneCfg(num_envs=4096, env_spacing=2.5) + # Basic settings + observations: ObservationsCfg = ObservationsCfg() + actions: ActionsCfg = ActionsCfg() + commands: CommandsCfg = CommandsCfg() + # MDP settings + rewards: RewardsCfg = RewardsCfg() + terminations: TerminationsCfg = TerminationsCfg() + events: EventCfg = EventCfg() + curriculum: CurriculumCfg = CurriculumCfg() + + def __post_init__(self): + """Post initialization.""" + # general settings + self.decimation = 2 + self.sim.render_interval = self.decimation + self.episode_length_s = 12.0 + self.viewer.eye = (3.5, 3.5, 3.5) + # simulation settings + self.sim.dt = 1.0 / 60.0 diff --git a/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/rsl_rl_scripts/cli_args.py b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/rsl_rl_scripts/cli_args.py new file mode 100644 index 0000000..75b0705 --- /dev/null +++ b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/rsl_rl_scripts/cli_args.py @@ -0,0 +1,97 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import argparse +import random +from typing import TYPE_CHECKING + +if TYPE_CHECKING: + from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg + + +def add_rsl_rl_args(parser: argparse.ArgumentParser): + """Add RSL-RL arguments to the parser. + + Args: + parser: The parser to add the arguments to. + """ + # create a new argument group + arg_group = parser.add_argument_group("rsl_rl", description="Arguments for RSL-RL agent.") + # -- experiment arguments + arg_group.add_argument( + "--experiment_name", type=str, default=None, help="Name of the experiment folder where logs will be stored." + ) + arg_group.add_argument("--run_name", type=str, default=None, + help="Run name suffix to the log directory.") + # -- load arguments + arg_group.add_argument("--resume", type=bool, default=None, + help="Whether to resume from a checkpoint.") + arg_group.add_argument("--load_run", type=str, default=None, + help="Name of the run folder to resume from.") + arg_group.add_argument("--checkpoint", type=str, default=None, + help="Checkpoint file to resume from.") + # -- logger arguments + arg_group.add_argument( + "--logger", type=str, default=None, choices={"wandb", "tensorboard", "neptune"}, help="Logger module to use." + ) + arg_group.add_argument( + "--log_project_name", type=str, default=None, help="Name of the logging project when using wandb or neptune." + ) + + +def parse_rsl_rl_cfg(task_name: str, args_cli: argparse.Namespace) -> RslRlOnPolicyRunnerCfg: + """Parse configuration for RSL-RL agent based on inputs. + + Args: + task_name: The name of the environment. + args_cli: The command line arguments. + + Returns: + The parsed configuration for RSL-RL agent based on inputs. + """ + from isaaclab_tasks.utils.parse_cfg import load_cfg_from_registry + + # load the default configuration + rslrl_cfg: RslRlOnPolicyRunnerCfg = load_cfg_from_registry(task_name, "rsl_rl_cfg_entry_point") + rslrl_cfg = update_rsl_rl_cfg(rslrl_cfg, args_cli) + return rslrl_cfg + + +def update_rsl_rl_cfg(agent_cfg: RslRlOnPolicyRunnerCfg, args_cli: argparse.Namespace): + """Update configuration for RSL-RL agent based on inputs. + + Args: + agent_cfg: The configuration for RSL-RL agent. + args_cli: The command line arguments. + + Returns: + The updated configuration for RSL-RL agent based on inputs. + """ + # override the default configuration with CLI arguments + if hasattr(args_cli, "seed") and args_cli.seed is not None: + # randomly sample a seed if seed = -1 + if args_cli.seed == -1: + args_cli.seed = random.randint(0, 10000) + agent_cfg.seed = args_cli.seed + if args_cli.resume is not None: + agent_cfg.resume = args_cli.resume + if args_cli.load_run is not None: + agent_cfg.load_run = args_cli.load_run + if args_cli.checkpoint is not None: + agent_cfg.load_checkpoint = args_cli.checkpoint + if args_cli.run_name is not None: + agent_cfg.run_name = args_cli.run_name + if args_cli.logger is not None: + agent_cfg.logger = args_cli.logger + # set the project name for wandb and neptune + if agent_cfg.logger in {"wandb", "neptune"} and args_cli.log_project_name: + agent_cfg.wandb_project = args_cli.log_project_name + agent_cfg.neptune_project = args_cli.log_project_name + + return agent_cfg + +# From Isaac Lab scripts/reinforcement_learning/rsl_rl/cli_args.py diff --git a/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/rsl_rl_scripts/play.py b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/rsl_rl_scripts/play.py new file mode 100644 index 0000000..d45a014 --- /dev/null +++ b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/rsl_rl_scripts/play.py @@ -0,0 +1,166 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Script to play a checkpoint if an RL agent from RSL-RL.""" + +"""Launch Isaac Sim Simulator first.""" + + +from isaaclab.app import AppLauncher +import argparse +import cli_args # isort: skip +import gymnasium as gym +import os +import time +import torch +from rsl_rl.runners import OnPolicyRunner +from isaaclab.envs import DirectMARLEnv, multi_agent_to_single_agent +from isaaclab.utils.assets import retrieve_file_path +from isaaclab.utils.dict import print_dict +from isaaclab.utils.pretrained_checkpoint import get_published_pretrained_checkpoint +from isaaclab_tasks.utils import get_checkpoint_path, parse_env_cfg +from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlVecEnvWrapper, export_policy_as_jit, export_policy_as_onnx + +# local imports + +# add argparse arguments +parser = argparse.ArgumentParser(description="Train an RL agent with RSL-RL.") +parser.add_argument("--video", action="store_true", default=False, + help="Record videos during training.") +parser.add_argument("--video_length", type=int, default=200, + help="Length of the recorded video (in steps).") +parser.add_argument( + "--disable_fabric", action="store_true", default=False, help="Disable fabric and use USD I/O operations." +) +parser.add_argument("--num_envs", type=int, default=None, + help="Number of environments to simulate.") +parser.add_argument("--task", type=str, default=None, help="Name of the task.") +parser.add_argument( + "--use_pretrained_checkpoint", + action="store_true", + help="Use the pre-trained checkpoint from Nucleus.", +) +parser.add_argument("--real-time", action="store_true", default=False, + help="Run in real-time, if possible.") +# append RSL-RL cli arguments +cli_args.add_rsl_rl_args(parser) +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +args_cli = parser.parse_args() +# always enable cameras to record video +if args_cli.video: + args_cli.enable_cameras = True + +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + + +import HumanoidRLPackage.HumanoidRLSetup.tasks # noqa: F401 + + +def main(): + """Play with RSL-RL agent.""" + # parse configuration + env_cfg = parse_env_cfg( + args_cli.task, device=args_cli.device, num_envs=args_cli.num_envs, use_fabric=not args_cli.disable_fabric + ) + agent_cfg: RslRlOnPolicyRunnerCfg = cli_args.parse_rsl_rl_cfg(args_cli.task, args_cli) + + # specify directory for logging experiments + log_root_path = os.path.join("logs", "rsl_rl", agent_cfg.experiment_name) + log_root_path = os.path.abspath(log_root_path) + print(f"[INFO] Loading experiment from directory: {log_root_path}") + if args_cli.use_pretrained_checkpoint: + resume_path = get_published_pretrained_checkpoint("rsl_rl", args_cli.task) + if not resume_path: + print("[INFO] Unfortunately a pre-trained checkpoint is currently unavailable for this task.") + return + elif args_cli.checkpoint: + resume_path = retrieve_file_path(args_cli.checkpoint) + else: + resume_path = get_checkpoint_path( + log_root_path, agent_cfg.load_run, agent_cfg.load_checkpoint) + + log_dir = os.path.dirname(resume_path) + + # create isaac environment + env = gym.make(args_cli.task, cfg=env_cfg, render_mode="rgb_array" if args_cli.video else None) + + # convert to single-agent instance if required by the RL algorithm + if isinstance(env.unwrapped, DirectMARLEnv): + env = multi_agent_to_single_agent(env) + + # wrap for video recording + if args_cli.video: + video_kwargs = { + "video_folder": os.path.join(log_dir, "videos", "play"), + "step_trigger": lambda step: step == 0, + "video_length": args_cli.video_length, + "disable_logger": True, + } + print("[INFO] Recording videos during training.") + print_dict(video_kwargs, nesting=4) + env = gym.wrappers.RecordVideo(env, **video_kwargs) + + # wrap around environment for rsl-rl + env = RslRlVecEnvWrapper(env) + + print(f"[INFO]: Loading model checkpoint from: {resume_path}") + # load previously trained model + ppo_runner = OnPolicyRunner(env, agent_cfg.to_dict(), log_dir=None, device=agent_cfg.device) + ppo_runner.load(resume_path) + + # obtain the trained policy for inference + policy = ppo_runner.get_inference_policy(device=env.unwrapped.device) + + policy_nn = ppo_runner.alg.policy + + # export policy to onnx/jit + export_model_dir = os.path.join(os.path.dirname(resume_path), "exported") + export_policy_as_jit(policy_nn, ppo_runner.obs_normalizer, + path=export_model_dir, filename="policy.pt") + export_policy_as_onnx( + policy_nn, normalizer=ppo_runner.obs_normalizer, path=export_model_dir, filename="policy.onnx" + ) + + dt = env.unwrapped.physics_dt + + # reset environment + obs, _ = env.get_observations() + timestep = 0 + # simulate environment + while simulation_app.is_running(): + start_time = time.time() + # run everything in inference mode + with torch.inference_mode(): + # agent stepping + actions = policy(obs) + # env stepping + obs, _, _, _ = env.step(actions) + if args_cli.video: + timestep += 1 + # Exit the play loop after recording one video + if timestep == args_cli.video_length: + break + + # time delay for real-time evaluation + sleep_time = dt - (time.time() - start_time) + if args_cli.real_time and sleep_time > 0: + time.sleep(sleep_time) + + # close the simulator + env.close() + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() + +# From Isaac Lab scripts/reinforcement_learning/rsl_rl/play.py diff --git a/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/rsl_rl_scripts/train.py b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/rsl_rl_scripts/train.py new file mode 100644 index 0000000..c51f544 --- /dev/null +++ b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/rsl_rl_scripts/train.py @@ -0,0 +1,161 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Script to train RL agent with RSL-RL.""" + +"""Launch Isaac Sim Simulator first.""" + +import sys +import argparse +from isaaclab.app import AppLauncher +import cli_args # isort: skip +import gymnasium as gym +import os +import torch +from datetime import datetime +from rsl_rl.runners import OnPolicyRunner +from isaaclab.envs import ( + DirectMARLEnv, + DirectMARLEnvCfg, + DirectRLEnvCfg, + ManagerBasedRLEnvCfg, + multi_agent_to_single_agent, +) +from isaaclab.utils.dict import print_dict +from isaaclab.utils.io import dump_pickle, dump_yaml +from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlVecEnvWrapper +from isaaclab_tasks.utils.hydra import hydra_task_config +from isaaclab_tasks.utils import get_checkpoint_path + + +# local imports + + +# add argparse arguments +parser = argparse.ArgumentParser(description="Train an RL agent with RSL-RL.") +parser.add_argument("--video", action="store_true", default=False, + help="Record videos during training.") +parser.add_argument("--video_length", type=int, default=200, + help="Length of the recorded video (in steps).") +parser.add_argument("--video_interval", type=int, default=2000, + help="Interval between video recordings (in steps).") +parser.add_argument("--num_envs", type=int, default=None, + help="Number of environments to simulate.") +parser.add_argument("--task", type=str, default=None, help="Name of the task.") +parser.add_argument("--seed", type=int, default=None, help="Seed used for the environment") +parser.add_argument("--max_iterations", type=int, default=None, + help="RL Policy training iterations.") +# append RSL-RL cli arguments +cli_args.add_rsl_rl_args(parser) +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +args_cli, hydra_args = parser.parse_known_args() + +# always enable cameras to record video +if args_cli.video: + args_cli.enable_cameras = True + +# clear out sys.argv for Hydra +sys.argv = [sys.argv[0]] + hydra_args + +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + + +import HumanoidRLPackage.HumanoidRLSetup.tasks # noqa: F401 + +torch.backends.cuda.matmul.allow_tf32 = True +torch.backends.cudnn.allow_tf32 = True +torch.backends.cudnn.deterministic = False +torch.backends.cudnn.benchmark = False + + +@hydra_task_config(args_cli.task, "rsl_rl_cfg_entry_point") +def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agent_cfg: RslRlOnPolicyRunnerCfg): + """Train with RSL-RL agent.""" + # override configurations with non-hydra CLI arguments + agent_cfg = cli_args.update_rsl_rl_cfg(agent_cfg, args_cli) + env_cfg.scene.num_envs = args_cli.num_envs if args_cli.num_envs is not None else env_cfg.scene.num_envs + agent_cfg.max_iterations = ( + args_cli.max_iterations if args_cli.max_iterations is not None else agent_cfg.max_iterations + ) + + # set the environment seed + # note: certain randomizations occur in the environment initialization so we set the seed here + env_cfg.seed = agent_cfg.seed + env_cfg.sim.device = args_cli.device if args_cli.device is not None else env_cfg.sim.device + + # specify directory for logging experiments + log_root_path = os.path.join("logs", "rsl_rl", agent_cfg.experiment_name) + log_root_path = os.path.abspath(log_root_path) + print(f"[INFO] Logging experiment in directory: {log_root_path}") + # specify directory for logging runs: {time-stamp}_{run_name} + log_dir = datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + # This way, the Ray Tune workflow can extract experiment name. + print(f"Exact experiment name requested from command line: {log_dir}") + if agent_cfg.run_name: + log_dir += f"_{agent_cfg.run_name}" + log_dir = os.path.join(log_root_path, log_dir) + + # create isaac environment + env = gym.make(args_cli.task, cfg=env_cfg, render_mode="rgb_array" if args_cli.video else None) + + # convert to single-agent instance if required by the RL algorithm + if isinstance(env.unwrapped, DirectMARLEnv): + env = multi_agent_to_single_agent(env) + + # save resume path before creating a new log_dir + if agent_cfg.resume: + resume_path = get_checkpoint_path( + log_root_path, agent_cfg.load_run, agent_cfg.load_checkpoint) + + # wrap for video recording + if args_cli.video: + video_kwargs = { + "video_folder": os.path.join(log_dir, "videos", "train"), + "step_trigger": lambda step: step % args_cli.video_interval == 0, + "video_length": args_cli.video_length, + "disable_logger": True, + } + print("[INFO] Recording videos during training.") + print_dict(video_kwargs, nesting=4) + env = gym.wrappers.RecordVideo(env, **video_kwargs) + + # wrap around environment for rsl-rl + env = RslRlVecEnvWrapper(env) + + # create runner from rsl-rl + runner = OnPolicyRunner(env, agent_cfg.to_dict(), log_dir=log_dir, device=agent_cfg.device) + # write git state to logs + runner.add_git_repo_to_log(__file__) + # load the checkpoint + if agent_cfg.resume: + print(f"[INFO]: Loading model checkpoint from: {resume_path}") + # load previously trained model + runner.load(resume_path) + + # dump the configuration into log-directory + dump_yaml(os.path.join(log_dir, "params", "env.yaml"), env_cfg) + dump_yaml(os.path.join(log_dir, "params", "agent.yaml"), agent_cfg) + dump_pickle(os.path.join(log_dir, "params", "env.pkl"), env_cfg) + dump_pickle(os.path.join(log_dir, "params", "agent.pkl"), agent_cfg) + + # run training + runner.learn(num_learning_iterations=agent_cfg.max_iterations, init_at_random_ep_len=True) + + # close the simulator + env.close() + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() + +# From Isaac Lab scripts/reinforcement_learning/rsl_rl/train.py diff --git a/autonomy/simulation/Humanoid_RL/HumanoidRL/outputs/2025-07-02/23-27-01/.hydra/config.yaml b/autonomy/simulation/Humanoid_RL/HumanoidRL/outputs/2025-07-02/23-27-01/.hydra/config.yaml new file mode 100644 index 0000000..e6be28a --- /dev/null +++ b/autonomy/simulation/Humanoid_RL/HumanoidRL/outputs/2025-07-02/23-27-01/.hydra/config.yaml @@ -0,0 +1,1046 @@ +env: + viewer: + eye: + - 3.5 + - 3.5 + - 3.5 + lookat: + - 0.0 + - 0.0 + - 0.0 + cam_prim_path: /OmniverseKit_Persp + resolution: + - 1280 + - 720 + origin_type: world + env_index: 0 + asset_name: null + body_name: null + sim: + physics_prim_path: /physicsScene + device: cuda:0 + dt: 0.016666666666666666 + render_interval: 2 + gravity: + - 0.0 + - 0.0 + - -9.81 + enable_scene_query_support: false + use_fabric: true + physx: + solver_type: 1 + min_position_iteration_count: 1 + max_position_iteration_count: 255 + min_velocity_iteration_count: 0 + max_velocity_iteration_count: 255 + enable_ccd: false + enable_stabilization: true + enable_enhanced_determinism: false + bounce_threshold_velocity: 0.5 + friction_offset_threshold: 0.04 + friction_correlation_distance: 0.025 + gpu_max_rigid_contact_count: 8388608 + gpu_max_rigid_patch_count: 163840 + gpu_found_lost_pairs_capacity: 2097152 + gpu_found_lost_aggregate_pairs_capacity: 33554432 + gpu_total_aggregate_pairs_capacity: 2097152 + gpu_collision_stack_size: 67108864 + gpu_heap_capacity: 67108864 + gpu_temp_buffer_capacity: 16777216 + gpu_max_num_partitions: 8 + gpu_max_soft_body_contacts: 1048576 + gpu_max_particle_contacts: 1048576 + physics_material: + func: isaaclab.sim.spawners.materials.physics_materials:spawn_rigid_body_material + static_friction: 0.5 + dynamic_friction: 0.5 + restitution: 0.0 + improve_patch_friction: true + friction_combine_mode: average + restitution_combine_mode: average + compliant_contact_stiffness: 0.0 + compliant_contact_damping: 0.0 + render: + enable_translucency: null + enable_reflections: null + enable_global_illumination: null + antialiasing_mode: null + enable_dlssg: null + enable_dl_denoiser: null + dlss_mode: null + enable_direct_lighting: null + samples_per_pixel: null + enable_shadows: null + enable_ambient_occlusion: null + carb_settings: null + rendering_mode: null + ui_window_class_type: isaaclab.envs.ui.manager_based_rl_env_window:ManagerBasedRLEnvWindow + seed: null + decimation: 2 + scene: + num_envs: 4096 + env_spacing: 2.5 + lazy_sensor_update: true + replicate_physics: true + filter_collisions: true + robot: + class_type: isaaclab.assets.articulation.articulation:Articulation + prim_path: '{ENV_REGEX_NS}/Robot' + spawn: + func: isaaclab.sim.spawners.from_files.from_files:spawn_from_usd + visible: true + semantic_tags: null + copy_from_source: true + mass_props: null + deformable_props: null + rigid_props: + rigid_body_enabled: null + kinematic_enabled: null + disable_gravity: false + linear_damping: null + angular_damping: null + max_linear_velocity: null + max_angular_velocity: null + max_depenetration_velocity: 5.0 + max_contact_impulse: null + enable_gyroscopic_forces: null + retain_accelerations: null + solver_position_iteration_count: null + solver_velocity_iteration_count: null + sleep_threshold: null + stabilization_threshold: null + collision_props: null + activate_contact_sensors: false + scale: null + articulation_props: null + fixed_tendons_props: null + joint_drive_props: null + visual_material_path: material + visual_material: null + usd_path: /home/hy/Downloads/Humanoid_Wato/ModelAssets/hand.usd + variants: null + init_state: + pos: + - 0.0 + - 0.0 + - 0.0 + rot: + - 1.0 + - 0.0 + - 0.0 + - 0.0 + lin_vel: + - 0.0 + - 0.0 + - 0.0 + ang_vel: + - 0.0 + - 0.0 + - 0.0 + joint_pos: + Revolute_1: 0.0 + Revolute_2: 0.0 + Revolute_3: 0.0 + Revolute_4: 0.0 + Revolute_5: 0.0 + Revolute_6: 0.0 + Revolute_7: 0.0 + Revolute_8: 0.0 + Revolute_9: 0.0 + Revolute_10: 0.0 + Revolute_11: 0.0 + Revolute_12: 0.0 + Revolute_13: 0.0 + Revolute_14: 0.0 + Revolute_15: 0.0 + joint_vel: + .*: 0.0 + collision_group: 0 + debug_vis: false + soft_joint_pos_limit_factor: 1.0 + actuators: + arm: + class_type: isaaclab.actuators.actuator_pd:ImplicitActuator + joint_names_expr: + - .* + effort_limit: null + velocity_limit: null + effort_limit_sim: null + velocity_limit_sim: null + stiffness: 0.5 + damping: 0.5 + armature: null + friction: null + ground: + class_type: null + prim_path: /World/ground + spawn: + func: isaaclab.sim.spawners.from_files.from_files:spawn_ground_plane + visible: true + semantic_tags: null + copy_from_source: true + usd_path: http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.5/Isaac/Environments/Grid/default_environment.usd + color: + - 0.0 + - 0.0 + - 0.0 + size: + - 100.0 + - 100.0 + physics_material: + func: isaaclab.sim.spawners.materials.physics_materials:spawn_rigid_body_material + static_friction: 0.5 + dynamic_friction: 0.5 + restitution: 0.0 + improve_patch_friction: true + friction_combine_mode: average + restitution_combine_mode: average + compliant_contact_stiffness: 0.0 + compliant_contact_damping: 0.0 + init_state: + pos: + - 0.0 + - 0.0 + - -1.05 + rot: + - 1.0 + - 0.0 + - 0.0 + - 0.0 + collision_group: 0 + debug_vis: false + table: + class_type: null + prim_path: '{ENV_REGEX_NS}/Table' + spawn: + func: isaaclab.sim.spawners.from_files.from_files:spawn_from_usd + visible: true + semantic_tags: null + copy_from_source: true + mass_props: null + deformable_props: null + rigid_props: null + collision_props: null + activate_contact_sensors: false + scale: null + articulation_props: null + fixed_tendons_props: null + joint_drive_props: null + visual_material_path: material + visual_material: null + usd_path: http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.5/Isaac/Props/Mounts/SeattleLabTable/table_instanceable.usd + variants: null + init_state: + pos: + - 0.55 + - 0.0 + - 0.0 + rot: + - 0.70711 + - 0.0 + - 0.0 + - 0.70711 + collision_group: 0 + debug_vis: false + light: + class_type: null + prim_path: /World/light + spawn: + func: isaaclab.sim.spawners.lights.lights:spawn_light + visible: true + semantic_tags: null + copy_from_source: true + prim_type: DomeLight + color: + - 0.75 + - 0.75 + - 0.75 + enable_color_temperature: false + color_temperature: 6500.0 + normalize: false + exposure: 0.0 + intensity: 2500.0 + texture_file: null + texture_format: automatic + visible_in_primary_ray: true + init_state: + pos: + - 0.0 + - 0.0 + - 0.0 + rot: + - 1.0 + - 0.0 + - 0.0 + - 0.0 + collision_group: 0 + debug_vis: false + recorders: + dataset_file_handler_class_type: isaaclab.utils.datasets.hdf5_dataset_file_handler:HDF5DatasetFileHandler + dataset_export_dir_path: /tmp/isaaclab/logs + dataset_filename: dataset + dataset_export_mode: + _value_: 1 + _name_: EXPORT_ALL + export_in_record_pre_reset: true + observations: + policy: + concatenate_terms: true + enable_corruption: true + history_length: null + flatten_history_dim: true + joint_pos: + func: isaaclab.envs.mdp.observations:joint_pos_rel + params: {} + modifiers: null + noise: + func: isaaclab.utils.noise.noise_model:uniform_noise + operation: add + n_min: -0.01 + n_max: 0.01 + clip: null + scale: null + history_length: 0 + flatten_history_dim: true + joint_vel: + func: isaaclab.envs.mdp.observations:joint_vel_rel + params: {} + modifiers: null + noise: + func: isaaclab.utils.noise.noise_model:uniform_noise + operation: add + n_min: -0.01 + n_max: 0.01 + clip: null + scale: null + history_length: 0 + flatten_history_dim: true + pose_command: + func: isaaclab.envs.mdp.observations:generated_commands + params: + command_name: ee_pose + modifiers: null + noise: null + clip: null + scale: null + history_length: 0 + flatten_history_dim: true + pose_command_2: + func: isaaclab.envs.mdp.observations:generated_commands + params: + command_name: ee_pose_2 + modifiers: null + noise: null + clip: null + scale: null + history_length: 0 + flatten_history_dim: true + pose_command_3: + func: isaaclab.envs.mdp.observations:generated_commands + params: + command_name: ee_pose_3 + modifiers: null + noise: null + clip: null + scale: null + history_length: 0 + flatten_history_dim: true + pose_command_4: + func: isaaclab.envs.mdp.observations:generated_commands + params: + command_name: ee_pose_4 + modifiers: null + noise: null + clip: null + scale: null + history_length: 0 + flatten_history_dim: true + pose_command_5: + func: isaaclab.envs.mdp.observations:generated_commands + params: + command_name: ee_pose_5 + modifiers: null + noise: null + clip: null + scale: null + history_length: 0 + flatten_history_dim: true + actions: + func: isaaclab.envs.mdp.observations:last_action + params: {} + modifiers: null + noise: null + clip: null + scale: null + history_length: 0 + flatten_history_dim: true + actions: + arm_action: + class_type: isaaclab.envs.mdp.actions.joint_actions:JointPositionAction + asset_name: robot + debug_vis: false + clip: null + joint_names: + - .* + scale: 0.5 + offset: 0.0 + preserve_order: false + use_default_offset: true + gripper_action: null + events: + reset_robot_joints: + func: isaaclab.envs.mdp.events:reset_joints_by_scale + params: + position_range: + - 0.75 + - 1.25 + velocity_range: + - 0.0 + - 0.0 + mode: reset + interval_range_s: null + is_global_time: false + min_step_count_between_reset: 0 + rerender_on_reset: false + wait_for_textures: true + xr: null + is_finite_horizon: false + episode_length_s: 12.0 + rewards: + end_effector_position_tracking: + func: HumanoidRLPackage.HumanoidRLSetup.tasks.manipulation.mdp.rewards:position_command_error + params: + asset_cfg: + name: robot + joint_names: null + joint_ids: slice(None,None,None) + fixed_tendon_names: null + fixed_tendon_ids: slice(None,None,None) + body_names: + - TIP_B_1 + body_ids: slice(None,None,None) + object_collection_names: null + object_collection_ids: slice(None,None,None) + preserve_order: false + command_name: ee_pose + weight: -0.2 + end_effector_2_position_tracking: + func: HumanoidRLPackage.HumanoidRLSetup.tasks.manipulation.mdp.rewards:position_command_error + params: + asset_cfg: + name: robot + joint_names: null + joint_ids: slice(None,None,None) + fixed_tendon_names: null + fixed_tendon_ids: slice(None,None,None) + body_names: + - TIP_B_2 + body_ids: slice(None,None,None) + object_collection_names: null + object_collection_ids: slice(None,None,None) + preserve_order: false + command_name: ee_pose_2 + weight: -0.2 + end_effector_3_position_tracking: + func: HumanoidRLPackage.HumanoidRLSetup.tasks.manipulation.mdp.rewards:position_command_error + params: + asset_cfg: + name: robot + joint_names: null + joint_ids: slice(None,None,None) + fixed_tendon_names: null + fixed_tendon_ids: slice(None,None,None) + body_names: + - TIP_B_3 + body_ids: slice(None,None,None) + object_collection_names: null + object_collection_ids: slice(None,None,None) + preserve_order: false + command_name: ee_pose_3 + weight: -0.2 + end_effector_4_position_tracking: + func: HumanoidRLPackage.HumanoidRLSetup.tasks.manipulation.mdp.rewards:position_command_error + params: + asset_cfg: + name: robot + joint_names: null + joint_ids: slice(None,None,None) + fixed_tendon_names: null + fixed_tendon_ids: slice(None,None,None) + body_names: + - TIP_B_4 + body_ids: slice(None,None,None) + object_collection_names: null + object_collection_ids: slice(None,None,None) + preserve_order: false + command_name: ee_pose_4 + weight: -0.2 + end_effector_5_position_tracking: + func: HumanoidRLPackage.HumanoidRLSetup.tasks.manipulation.mdp.rewards:position_command_error + params: + asset_cfg: + name: robot + joint_names: null + joint_ids: slice(None,None,None) + fixed_tendon_names: null + fixed_tendon_ids: slice(None,None,None) + body_names: + - TIP_B_5 + body_ids: slice(None,None,None) + object_collection_names: null + object_collection_ids: slice(None,None,None) + preserve_order: false + command_name: ee_pose_5 + weight: -0.2 + end_effector_position_tracking_fine_grained: + func: HumanoidRLPackage.HumanoidRLSetup.tasks.manipulation.mdp.rewards:position_command_error_tanh + params: + asset_cfg: + name: robot + joint_names: null + joint_ids: slice(None,None,None) + fixed_tendon_names: null + fixed_tendon_ids: slice(None,None,None) + body_names: + - TIP_B_1 + body_ids: slice(None,None,None) + object_collection_names: null + object_collection_ids: slice(None,None,None) + preserve_order: false + std: 0.1 + command_name: ee_pose + weight: 0.1 + end_effector_2_position_tracking_fine_grained: + func: HumanoidRLPackage.HumanoidRLSetup.tasks.manipulation.mdp.rewards:position_command_error_tanh + params: + asset_cfg: + name: robot + joint_names: null + joint_ids: slice(None,None,None) + fixed_tendon_names: null + fixed_tendon_ids: slice(None,None,None) + body_names: + - TIP_B_2 + body_ids: slice(None,None,None) + object_collection_names: null + object_collection_ids: slice(None,None,None) + preserve_order: false + std: 0.1 + command_name: ee_pose_2 + weight: 0.1 + end_effector_3_position_tracking_fine_grained: + func: HumanoidRLPackage.HumanoidRLSetup.tasks.manipulation.mdp.rewards:position_command_error_tanh + params: + asset_cfg: + name: robot + joint_names: null + joint_ids: slice(None,None,None) + fixed_tendon_names: null + fixed_tendon_ids: slice(None,None,None) + body_names: + - TIP_B_3 + body_ids: slice(None,None,None) + object_collection_names: null + object_collection_ids: slice(None,None,None) + preserve_order: false + std: 0.1 + command_name: ee_pose_3 + weight: 0.1 + end_effector_4_position_tracking_fine_grained: + func: HumanoidRLPackage.HumanoidRLSetup.tasks.manipulation.mdp.rewards:position_command_error_tanh + params: + asset_cfg: + name: robot + joint_names: null + joint_ids: slice(None,None,None) + fixed_tendon_names: null + fixed_tendon_ids: slice(None,None,None) + body_names: + - TIP_B_4 + body_ids: slice(None,None,None) + object_collection_names: null + object_collection_ids: slice(None,None,None) + preserve_order: false + std: 0.1 + command_name: ee_pose_4 + weight: 0.1 + end_effector_5_position_tracking_fine_grained: + func: HumanoidRLPackage.HumanoidRLSetup.tasks.manipulation.mdp.rewards:position_command_error_tanh + params: + asset_cfg: + name: robot + joint_names: null + joint_ids: slice(None,None,None) + fixed_tendon_names: null + fixed_tendon_ids: slice(None,None,None) + body_names: + - TIP_B_5 + body_ids: slice(None,None,None) + object_collection_names: null + object_collection_ids: slice(None,None,None) + preserve_order: false + std: 0.1 + command_name: ee_pose_5 + weight: 0.1 + action_rate: + func: isaaclab.envs.mdp.rewards:action_rate_l2 + params: {} + weight: -0.0001 + joint_vel: + func: isaaclab.envs.mdp.rewards:joint_vel_l2 + params: + asset_cfg: + name: robot + joint_names: null + joint_ids: slice(None,None,None) + fixed_tendon_names: null + fixed_tendon_ids: slice(None,None,None) + body_names: null + body_ids: slice(None,None,None) + object_collection_names: null + object_collection_ids: slice(None,None,None) + preserve_order: false + weight: -0.0001 + terminations: + time_out: + func: isaaclab.envs.mdp.terminations:time_out + params: {} + time_out: true + curriculum: + action_rate: + func: isaaclab.envs.mdp.curriculums:modify_reward_weight + params: + term_name: action_rate + weight: -0.005 + num_steps: 4500 + joint_vel: + func: isaaclab.envs.mdp.curriculums:modify_reward_weight + params: + term_name: joint_vel + weight: -0.001 + num_steps: 4500 + commands: + ee_pose: + class_type: isaaclab.envs.mdp.commands.pose_command:UniformPoseCommand + resampling_time_range: + - 4.0 + - 4.0 + debug_vis: true + asset_name: robot + body_name: TIP_B_1 + make_quat_unique: false + ranges: + pos_x: + - 0.07346 + - 0.07346 + pos_y: + - 0.13 + - 0.1455 + pos_z: + - 0.09 + - 0.1005 + roll: + - 0.0 + - 0.0 + pitch: + - 0.0 + - 0.0 + yaw: + - 0.0 + - 0.0 + goal_pose_visualizer_cfg: + prim_path: /Visuals/Command/goal_pose + markers: + frame: + func: isaaclab.sim.spawners.from_files.from_files:spawn_from_usd + visible: true + semantic_tags: null + copy_from_source: true + mass_props: null + deformable_props: null + rigid_props: null + collision_props: null + activate_contact_sensors: false + scale: + - 0.02 + - 0.02 + - 0.02 + articulation_props: null + fixed_tendons_props: null + joint_drive_props: null + visual_material_path: material + visual_material: null + usd_path: http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.5/Isaac/Props/UIElements/frame_prim.usd + variants: null + current_pose_visualizer_cfg: + prim_path: /Visuals/Command/body_pose + markers: + frame: + func: isaaclab.sim.spawners.from_files.from_files:spawn_from_usd + visible: true + semantic_tags: null + copy_from_source: true + mass_props: null + deformable_props: null + rigid_props: null + collision_props: null + activate_contact_sensors: false + scale: + - 0.02 + - 0.02 + - 0.02 + articulation_props: null + fixed_tendons_props: null + joint_drive_props: null + visual_material_path: material + visual_material: null + usd_path: http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.5/Isaac/Props/UIElements/frame_prim.usd + variants: null + ee_pose_2: + class_type: isaaclab.envs.mdp.commands.pose_command:UniformPoseCommand + resampling_time_range: + - 4.0 + - 4.0 + debug_vis: true + asset_name: robot + body_name: TIP_B_2 + make_quat_unique: false + ranges: + pos_x: + - 0.04746 + - 0.04746 + pos_y: + - 0.15406 + - 0.15558 + pos_z: + - 0.098 + - 0.10058 + roll: + - 0.0 + - 0.0 + pitch: + - 0.0 + - 0.0 + yaw: + - 0.0 + - 0.0 + goal_pose_visualizer_cfg: + prim_path: /Visuals/Command/goal_pose + markers: + frame: + func: isaaclab.sim.spawners.from_files.from_files:spawn_from_usd + visible: true + semantic_tags: null + copy_from_source: true + mass_props: null + deformable_props: null + rigid_props: null + collision_props: null + activate_contact_sensors: false + scale: + - 0.02 + - 0.02 + - 0.02 + articulation_props: null + fixed_tendons_props: null + joint_drive_props: null + visual_material_path: material + visual_material: null + usd_path: http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.5/Isaac/Props/UIElements/frame_prim.usd + variants: null + current_pose_visualizer_cfg: + prim_path: /Visuals/Command/body_pose + markers: + frame: + func: isaaclab.sim.spawners.from_files.from_files:spawn_from_usd + visible: true + semantic_tags: null + copy_from_source: true + mass_props: null + deformable_props: null + rigid_props: null + collision_props: null + activate_contact_sensors: false + scale: + - 0.02 + - 0.02 + - 0.02 + articulation_props: null + fixed_tendons_props: null + joint_drive_props: null + visual_material_path: material + visual_material: null + usd_path: http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.5/Isaac/Props/UIElements/frame_prim.usd + variants: null + ee_pose_3: + class_type: isaaclab.envs.mdp.commands.pose_command:UniformPoseCommand + resampling_time_range: + - 4.0 + - 4.0 + debug_vis: true + asset_name: robot + body_name: TIP_B_3 + make_quat_unique: false + ranges: + pos_x: + - 0.01996 + - 0.01996 + pos_y: + - 0.134 + - 0.145 + pos_z: + - 0.09 + - 0.11 + roll: + - 0.0 + - 0.0 + pitch: + - 0.0 + - 0.0 + yaw: + - 0.0 + - 0.0 + goal_pose_visualizer_cfg: + prim_path: /Visuals/Command/goal_pose + markers: + frame: + func: isaaclab.sim.spawners.from_files.from_files:spawn_from_usd + visible: true + semantic_tags: null + copy_from_source: true + mass_props: null + deformable_props: null + rigid_props: null + collision_props: null + activate_contact_sensors: false + scale: + - 0.02 + - 0.02 + - 0.02 + articulation_props: null + fixed_tendons_props: null + joint_drive_props: null + visual_material_path: material + visual_material: null + usd_path: http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.5/Isaac/Props/UIElements/frame_prim.usd + variants: null + current_pose_visualizer_cfg: + prim_path: /Visuals/Command/body_pose + markers: + frame: + func: isaaclab.sim.spawners.from_files.from_files:spawn_from_usd + visible: true + semantic_tags: null + copy_from_source: true + mass_props: null + deformable_props: null + rigid_props: null + collision_props: null + activate_contact_sensors: false + scale: + - 0.02 + - 0.02 + - 0.02 + articulation_props: null + fixed_tendons_props: null + joint_drive_props: null + visual_material_path: material + visual_material: null + usd_path: http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.5/Isaac/Props/UIElements/frame_prim.usd + variants: null + ee_pose_4: + class_type: isaaclab.envs.mdp.commands.pose_command:UniformPoseCommand + resampling_time_range: + - 4.0 + - 4.0 + debug_vis: true + asset_name: robot + body_name: TIP_B_4 + make_quat_unique: false + ranges: + pos_x: + - -0.00754 + - -0.00754 + pos_y: + - 0.119 + - 0.13 + pos_z: + - 0.09 + - 0.1 + roll: + - 0.0 + - 0.0 + pitch: + - 0.0 + - 0.0 + yaw: + - 0.0 + - 0.0 + goal_pose_visualizer_cfg: + prim_path: /Visuals/Command/goal_pose + markers: + frame: + func: isaaclab.sim.spawners.from_files.from_files:spawn_from_usd + visible: true + semantic_tags: null + copy_from_source: true + mass_props: null + deformable_props: null + rigid_props: null + collision_props: null + activate_contact_sensors: false + scale: + - 0.02 + - 0.02 + - 0.02 + articulation_props: null + fixed_tendons_props: null + joint_drive_props: null + visual_material_path: material + visual_material: null + usd_path: http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.5/Isaac/Props/UIElements/frame_prim.usd + variants: null + current_pose_visualizer_cfg: + prim_path: /Visuals/Command/body_pose + markers: + frame: + func: isaaclab.sim.spawners.from_files.from_files:spawn_from_usd + visible: true + semantic_tags: null + copy_from_source: true + mass_props: null + deformable_props: null + rigid_props: null + collision_props: null + activate_contact_sensors: false + scale: + - 0.02 + - 0.02 + - 0.02 + articulation_props: null + fixed_tendons_props: null + joint_drive_props: null + visual_material_path: material + visual_material: null + usd_path: http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.5/Isaac/Props/UIElements/frame_prim.usd + variants: null + ee_pose_5: + class_type: isaaclab.envs.mdp.commands.pose_command:UniformPoseCommand + resampling_time_range: + - 4.0 + - 4.0 + debug_vis: true + asset_name: robot + body_name: TIP_B_5 + make_quat_unique: false + ranges: + pos_x: + - 0.0869 + - 0.0869 + pos_y: + - 0.04 + - 0.052 + pos_z: + - 0.17 + - 0.17248 + roll: + - 0.0 + - 0.0 + pitch: + - 0.0 + - 0.0 + yaw: + - 0.0 + - 0.0 + goal_pose_visualizer_cfg: + prim_path: /Visuals/Command/goal_pose + markers: + frame: + func: isaaclab.sim.spawners.from_files.from_files:spawn_from_usd + visible: true + semantic_tags: null + copy_from_source: true + mass_props: null + deformable_props: null + rigid_props: null + collision_props: null + activate_contact_sensors: false + scale: + - 0.02 + - 0.02 + - 0.02 + articulation_props: null + fixed_tendons_props: null + joint_drive_props: null + visual_material_path: material + visual_material: null + usd_path: http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.5/Isaac/Props/UIElements/frame_prim.usd + variants: null + current_pose_visualizer_cfg: + prim_path: /Visuals/Command/body_pose + markers: + frame: + func: isaaclab.sim.spawners.from_files.from_files:spawn_from_usd + visible: true + semantic_tags: null + copy_from_source: true + mass_props: null + deformable_props: null + rigid_props: null + collision_props: null + activate_contact_sensors: false + scale: + - 0.02 + - 0.02 + - 0.02 + articulation_props: null + fixed_tendons_props: null + joint_drive_props: null + visual_material_path: material + visual_material: null + usd_path: http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.5/Isaac/Props/UIElements/frame_prim.usd + variants: null +agent: + seed: 42 + device: cuda:0 + num_steps_per_env: 24 + max_iterations: 1000 + empirical_normalization: false + policy: + class_name: ActorCritic + init_noise_std: 1.0 + noise_std_type: scalar + actor_hidden_dims: + - 64 + - 64 + critic_hidden_dims: + - 64 + - 64 + activation: elu + algorithm: + class_name: PPO + num_learning_epochs: 8 + num_mini_batches: 4 + learning_rate: 0.001 + schedule: adaptive + gamma: 0.99 + lam: 0.95 + entropy_coef: 0.01 + desired_kl: 0.01 + max_grad_norm: 1.0 + value_loss_coef: 1.0 + use_clipped_value_loss: true + clip_param: 0.2 + normalize_advantage_per_mini_batch: false + symmetry_cfg: null + rnd_cfg: null + clip_actions: null + save_interval: 50 + experiment_name: reach_humanoid_hand + run_name: '' + logger: tensorboard + neptune_project: isaaclab + wandb_project: isaaclab + resume: false + load_run: .* + load_checkpoint: model_.*.pt diff --git a/autonomy/simulation/Humanoid_RL/HumanoidRL/outputs/2025-07-02/23-27-01/.hydra/hydra.yaml b/autonomy/simulation/Humanoid_RL/HumanoidRL/outputs/2025-07-02/23-27-01/.hydra/hydra.yaml new file mode 100644 index 0000000..600800b --- /dev/null +++ b/autonomy/simulation/Humanoid_RL/HumanoidRL/outputs/2025-07-02/23-27-01/.hydra/hydra.yaml @@ -0,0 +1,154 @@ +hydra: + run: + dir: outputs/${now:%Y-%m-%d}/${now:%H-%M-%S} + sweep: + dir: multirun/${now:%Y-%m-%d}/${now:%H-%M-%S} + subdir: ${hydra.job.num} + launcher: + _target_: hydra._internal.core_plugins.basic_launcher.BasicLauncher + sweeper: + _target_: hydra._internal.core_plugins.basic_sweeper.BasicSweeper + max_batch_size: null + params: null + help: + app_name: ${hydra.job.name} + header: '${hydra.help.app_name} is powered by Hydra. + + ' + footer: 'Powered by Hydra (https://hydra.cc) + + Use --hydra-help to view Hydra specific help + + ' + template: '${hydra.help.header} + + == Configuration groups == + + Compose your configuration from those groups (group=option) + + + $APP_CONFIG_GROUPS + + + == Config == + + Override anything in the config (foo.bar=value) + + + $CONFIG + + + ${hydra.help.footer} + + ' + hydra_help: + template: 'Hydra (${hydra.runtime.version}) + + See https://hydra.cc for more info. + + + == Flags == + + $FLAGS_HELP + + + == Configuration groups == + + Compose your configuration from those groups (For example, append hydra/job_logging=disabled + to command line) + + + $HYDRA_CONFIG_GROUPS + + + Use ''--cfg hydra'' to Show the Hydra config. + + ' + hydra_help: ??? + hydra_logging: + version: 1 + formatters: + simple: + format: '[%(asctime)s][HYDRA] %(message)s' + handlers: + console: + class: logging.StreamHandler + formatter: simple + stream: ext://sys.stdout + root: + level: INFO + handlers: + - console + loggers: + logging_example: + level: DEBUG + disable_existing_loggers: false + job_logging: + version: 1 + formatters: + simple: + format: '[%(asctime)s][%(name)s][%(levelname)s] - %(message)s' + handlers: + console: + class: logging.StreamHandler + formatter: simple + stream: ext://sys.stdout + file: + class: logging.FileHandler + formatter: simple + filename: ${hydra.runtime.output_dir}/${hydra.job.name}.log + root: + level: INFO + handlers: + - console + - file + disable_existing_loggers: false + env: {} + mode: RUN + searchpath: [] + callbacks: {} + output_subdir: .hydra + overrides: + hydra: + - hydra.mode=RUN + task: [] + job: + name: hydra + chdir: null + override_dirname: '' + id: ??? + num: ??? + config_name: Isaac-Reach-Humanoid-Hand-v0 + env_set: {} + env_copy: [] + config: + override_dirname: + kv_sep: '=' + item_sep: ',' + exclude_keys: [] + runtime: + version: 1.3.2 + version_base: '1.3' + cwd: /home/hy/Downloads/Humanoid_Wato/HumanoidRL + config_sources: + - path: hydra.conf + schema: pkg + provider: hydra + - path: isaaclab_tasks.utils + schema: pkg + provider: main + - path: '' + schema: structured + provider: schema + output_dir: /home/hy/Downloads/Humanoid_Wato/HumanoidRL/outputs/2025-07-02/23-27-01 + choices: + hydra/env: default + hydra/callbacks: null + hydra/job_logging: default + hydra/hydra_logging: default + hydra/hydra_help: default + hydra/help: default + hydra/sweeper: basic + hydra/launcher: basic + hydra/output: default + verbose: false diff --git a/autonomy/simulation/Humanoid_RL/HumanoidRL/outputs/2025-07-02/23-27-01/.hydra/overrides.yaml b/autonomy/simulation/Humanoid_RL/HumanoidRL/outputs/2025-07-02/23-27-01/.hydra/overrides.yaml new file mode 100644 index 0000000..fe51488 --- /dev/null +++ b/autonomy/simulation/Humanoid_RL/HumanoidRL/outputs/2025-07-02/23-27-01/.hydra/overrides.yaml @@ -0,0 +1 @@ +[] diff --git a/autonomy/simulation/Humanoid_RL/HumanoidRL/outputs/2025-07-02/23-27-01/hydra.log b/autonomy/simulation/Humanoid_RL/HumanoidRL/outputs/2025-07-02/23-27-01/hydra.log new file mode 100644 index 0000000..e69de29 diff --git a/autonomy/simulation/Humanoid_RL/ModelAssets/.thumbs/256x256/Hand.usd.png b/autonomy/simulation/Humanoid_RL/ModelAssets/.thumbs/256x256/Hand.usd.png new file mode 100644 index 0000000..6d01b17 Binary files /dev/null and b/autonomy/simulation/Humanoid_RL/ModelAssets/.thumbs/256x256/Hand.usd.png differ diff --git a/autonomy/simulation/Humanoid_RL/ModelAssets/configuration/hand_base.usd b/autonomy/simulation/Humanoid_RL/ModelAssets/configuration/hand_base.usd new file mode 100644 index 0000000..580fe86 Binary files /dev/null and b/autonomy/simulation/Humanoid_RL/ModelAssets/configuration/hand_base.usd differ diff --git a/autonomy/simulation/Humanoid_RL/ModelAssets/configuration/hand_physics.usd b/autonomy/simulation/Humanoid_RL/ModelAssets/configuration/hand_physics.usd new file mode 100644 index 0000000..96c77c4 Binary files /dev/null and b/autonomy/simulation/Humanoid_RL/ModelAssets/configuration/hand_physics.usd differ diff --git a/autonomy/simulation/Humanoid_RL/ModelAssets/configuration/hand_sensor.usd b/autonomy/simulation/Humanoid_RL/ModelAssets/configuration/hand_sensor.usd new file mode 100644 index 0000000..35a9fe2 Binary files /dev/null and b/autonomy/simulation/Humanoid_RL/ModelAssets/configuration/hand_sensor.usd differ diff --git a/autonomy/simulation/Humanoid_RL/ModelAssets/hand.usd b/autonomy/simulation/Humanoid_RL/ModelAssets/hand.usd new file mode 100644 index 0000000..2bc943c Binary files /dev/null and b/autonomy/simulation/Humanoid_RL/ModelAssets/hand.usd differ diff --git a/autonomy/simulation/Humanoid_RL/hand_test.py b/autonomy/simulation/Humanoid_RL/hand_test.py new file mode 100644 index 0000000..ee5ab78 --- /dev/null +++ b/autonomy/simulation/Humanoid_RL/hand_test.py @@ -0,0 +1,99 @@ +from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR +from HumanoidRL.HumanoidRLPackage.HumanoidRLSetup.modelCfg.humanoid import HAND_CFG +from isaaclab.utils import configclass +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from isaaclab.managers import SceneEntityCfg +from isaaclab.assets import AssetBaseCfg +import isaaclab.sim as sim_utils +import torch +import argparse +from isaaclab.app import AppLauncher + +parser = argparse.ArgumentParser(description="Humanoid Hand Testing") +AppLauncher.add_app_launcher_args(parser) +args_cli = parser.parse_args() + +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + + +@configclass +class HandSceneCfg(InteractiveSceneCfg): + ground = AssetBaseCfg( + prim_path="/World/defaultGroundPlane", + spawn=sim_utils.GroundPlaneCfg(), + init_state=AssetBaseCfg.InitialStateCfg(pos=(0.0, 0.0, -1.05)), + ) + + dome_light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + # table = AssetBaseCfg( + # prim_path="{ENV_REGEX_NS}/Table", + # spawn=sim_utils.UsdFileCfg( + # usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/Stand/stand_instanceable.usd", scale=(1.0, 1.0, 1.0) + # ), + # ) + + robot = HAND_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + + +def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene): + + robot = scene["robot"] + + robot_entity_cfg = SceneEntityCfg("robot", joint_names=[".*"], body_names=[".*"]) + + robot_entity_cfg.resolve(scene) + + sim_dt = sim.get_physics_dt() + + joint_position = robot.data.default_joint_pos.clone() + joint_vel = robot.data.default_joint_vel.clone() + robot.write_joint_state_to_sim(joint_position, joint_vel) + + while simulation_app.is_running(): + + # joint_position = robot.data.default_joint_pos.clone() + # print(joint_position) + # joint_vel = robot.data.default_joint_vel.clone() + # robot.write_joint_state_to_sim(joint_position, joint_vel) + + joint_position_list = [[1.0, 1.0, 1.0, 1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]] + joint_position = torch.tensor(joint_position_list[0], device=sim.device) + + robot.reset() + joint_pos_des = joint_position.unsqueeze(0)[:, robot_entity_cfg.joint_ids].clone() + robot.set_joint_position_target(joint_pos_des, joint_ids=robot_entity_cfg.joint_ids) + scene.write_data_to_sim() + + sim.step() + + scene.update(sim_dt) + + +def main(): + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + + sim.set_camera_view([2.5, 2.5, 2.5], [0.0, 0.0, 0.0]) + + scene_cfg = HandSceneCfg(num_envs=1, env_spacing=2.0) + scene = InteractiveScene(scene_cfg) + + sim.reset() + + print("[INFO]: Setup complete...") + + run_simulator(sim, scene) + + +if __name__ == "__main__": + main() + simulation_app.close() + + +# (env_isaaclab) hy@hy-LOQ-15IRX9:~/Downloads/Humanoid$ PYTHONPATH=$(pwd) /home/hy/IsaacLab/isaaclab.sh -p hand_test.py diff --git a/autonomy/simulation/Humanoid_RL/models/reach_model.pt b/autonomy/simulation/Humanoid_RL/models/reach_model.pt new file mode 100644 index 0000000..400c9b9 Binary files /dev/null and b/autonomy/simulation/Humanoid_RL/models/reach_model.pt differ diff --git a/autonomy/simulation/package.xml b/autonomy/simulation/package.xml new file mode 100644 index 0000000..39d2bb3 --- /dev/null +++ b/autonomy/simulation/package.xml @@ -0,0 +1,19 @@ + + + simulation + 0.0.0 + Simulation folder for the WATO Humanoid project. + + Wilson Cheng + Apache2.0 + + ament_cmake + + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + + ament_cmake + + diff --git a/utils/boilerplate-files/foo.launch.py b/utils/boilerplate-files/foo.launch.py index 062a886..4ea3058 100644 --- a/utils/boilerplate-files/foo.launch.py +++ b/utils/boilerplate-files/foo.launch.py @@ -32,8 +32,8 @@ def launch_setup(context, *args, **kwargs): node_parameters.append(param_file_path) print(f"INFO: Using parameter file for {node_name}: {param_file_path}") else: - print(f"WARNING: No params.yaml found for {node_name} at {param_file_path}. Launching without parameters.") - + print( + f"WARNING: No params.yaml found for {node_name} at {param_file_path}. Launching without parameters.") # Define the Node action node = Node( @@ -42,7 +42,7 @@ def launch_setup(context, *args, **kwargs): name=node_name, parameters=node_parameters, output='screen', # Optional: 'screen' or 'log'. 'screen' prints output to the console. - emulate_tty=True, # Optional: Set to True for colored output in the console. + emulate_tty=True, # Optional: Set to True for colored output in the console. ) return [node] @@ -78,10 +78,10 @@ def generate_launch_description(): # e.g., ros2 launch ... node_name:=my_custom_node_name DeclareLaunchArgument( 'node_name', - default_value=LaunchConfiguration('executable_name'), # Defaults to the executable name + default_value=LaunchConfiguration('executable_name'), # Defaults to the executable name description='Name to assign to the ROS 2 node.' ), # OpaqueFunction defers the creation of the Node action until launch arguments are resolved. # This is necessary because we need the actual string values of package_name, executable_name, etc. OpaqueFunction(function=launch_setup) - ]) \ No newline at end of file + ]) diff --git a/utils/boilerplate-files/foo_core.cpp b/utils/boilerplate-files/foo_core.cpp index 7a81eec..00232b8 100644 --- a/utils/boilerplate-files/foo_core.cpp +++ b/utils/boilerplate-files/foo_core.cpp @@ -1,5 +1,3 @@ #include "foo_core.hpp" -FooCore::FooCore() { - -} \ No newline at end of file +FooCore::FooCore() {} \ No newline at end of file diff --git a/utils/boilerplate-files/foo_core.hpp b/utils/boilerplate-files/foo_core.hpp index 46ac331..f65f37e 100644 --- a/utils/boilerplate-files/foo_core.hpp +++ b/utils/boilerplate-files/foo_core.hpp @@ -11,7 +11,6 @@ class FooCore { FooCore(); private: - }; #endif \ No newline at end of file diff --git a/utils/boilerplate-files/foo_node.cpp b/utils/boilerplate-files/foo_node.cpp index c30e113..ea32b93 100644 --- a/utils/boilerplate-files/foo_node.cpp +++ b/utils/boilerplate-files/foo_node.cpp @@ -1,5 +1,3 @@ #include "foo_node.hpp" -FooNode::FooNode() { - -} \ No newline at end of file +FooNode::FooNode() {} \ No newline at end of file diff --git a/utils/boilerplate-files/foo_node.hpp b/utils/boilerplate-files/foo_node.hpp index 806780b..454f170 100644 --- a/utils/boilerplate-files/foo_node.hpp +++ b/utils/boilerplate-files/foo_node.hpp @@ -13,7 +13,6 @@ class FooNode : public rclcpp::Node { FooNode(); private: - }; #endif \ No newline at end of file diff --git a/utils/boilerplate-files/foo_node.py b/utils/boilerplate-files/foo_node.py index dd63326..e4ce4c7 100644 --- a/utils/boilerplate-files/foo_node.py +++ b/utils/boilerplate-files/foo_node.py @@ -3,6 +3,7 @@ from foo.foo_core import FooCore + class Foo(Node): def __init__(self): super().__init__('python_foo') @@ -12,6 +13,7 @@ def __init__(self): self.__foo = FooCore() + def main(args=None): rclpy.init(args=args) @@ -25,5 +27,6 @@ def main(args=None): python_foo.destroy_node() rclpy.shutdown() + if __name__ == '__main__': main() diff --git a/utils/boilerplate-files/foo_test.cpp b/utils/boilerplate-files/foo_test.cpp index 86af3c4..6a9ba66 100644 --- a/utils/boilerplate-files/foo_test.cpp +++ b/utils/boilerplate-files/foo_test.cpp @@ -1,34 +1,35 @@ #include "rclcpp/rclcpp.hpp" #include "gtest/gtest.h" -// Google testing documentation at https://google.github.io/googletest/primer.html +// Google testing documentation at +// https://google.github.io/googletest/primer.html #include "foo_core.hpp" // The fixture for testing class Foo. class FooTest : public testing::Test { - protected: +protected: // You can remove any or all of the following functions if their bodies would // be empty. FooTest() { - // You can do set-up work for each test here. + // You can do set-up work for each test here. } ~FooTest() override { - // You can do clean-up work that doesn't throw exceptions here. + // You can do clean-up work that doesn't throw exceptions here. } // If the constructor and destructor are not enough for setting up // and cleaning up each test, you can define the following methods: void SetUp() override { - // Code here will be called immediately after the constructor (right - // before each test). + // Code here will be called immediately after the constructor (right + // before each test). } void TearDown() override { - // Code here will be called immediately after each test (right - // before the destructor). + // Code here will be called immediately after each test (right + // before the destructor). } // Class members declared here can be used by all tests in the test suite