From f507f77f8ec5771e08b42c8a1e7b0bd50b9db283 Mon Sep 17 00:00:00 2001 From: gursi26 Date: Sun, 12 Jan 2025 22:26:01 -0500 Subject: [PATCH 1/6] added lift squishy env --- docker/.env.base | 1 + docker/docker-compose.yaml | 3 + .../manipulation/liftsquishy/__init__.py | 9 + .../liftsquishy/config/__init__.py | 9 + .../liftsquishy/config/franka/__init__.py | 42 +++ .../config/franka/agents/__init__.py | 4 + .../franka/agents/rl_games_ppo_cfg.yaml | 79 +++++ .../config/franka/agents/robomimic/bc.json | 264 ++++++++++++++++ .../config/franka/agents/robomimic/bcq.json | 299 ++++++++++++++++++ .../config/franka/agents/rsl_rl_ppo_cfg.py | 41 +++ .../config/franka/agents/sb3_ppo_cfg.yaml | 28 ++ .../config/franka/agents/skrl_ppo_cfg.yaml | 80 +++++ .../config/franka/joint_pos_env_cfg.py | 106 +++++++ .../manipulation/liftsquishy/lift_env_cfg.py | 222 +++++++++++++ .../manipulation/liftsquishy/mdp/__init__.py | 12 + .../liftsquishy/mdp/observations.py | 31 ++ .../manipulation/liftsquishy/mdp/rewards.py | 69 ++++ .../liftsquishy/mdp/terminations.py | 55 ++++ 18 files changed, 1354 insertions(+) create mode 100644 source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/__init__.py create mode 100644 source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/__init__.py create mode 100644 source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/__init__.py create mode 100644 source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/agents/__init__.py create mode 100644 source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/agents/rl_games_ppo_cfg.yaml create mode 100644 source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/agents/robomimic/bc.json create mode 100644 source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/agents/robomimic/bcq.json create mode 100644 source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/agents/rsl_rl_ppo_cfg.py create mode 100644 source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/agents/sb3_ppo_cfg.yaml create mode 100644 source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/agents/skrl_ppo_cfg.yaml create mode 100644 source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/joint_pos_env_cfg.py create mode 100644 source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/lift_env_cfg.py create mode 100644 source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/mdp/__init__.py create mode 100644 source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/mdp/observations.py create mode 100644 source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/mdp/rewards.py create mode 100644 source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/mdp/terminations.py diff --git a/docker/.env.base b/docker/.env.base index 0ec3332df3..29d2e72235 100644 --- a/docker/.env.base +++ b/docker/.env.base @@ -14,3 +14,4 @@ DOCKER_ISAACSIM_ROOT_PATH=/isaac-sim DOCKER_ISAACLAB_PATH=/workspace/isaaclab # Docker user directory - by default this is the root user's home directory DOCKER_USER_HOME=/root +DOCKER_USER_CODE_FILES=/workspace/code diff --git a/docker/docker-compose.yaml b/docker/docker-compose.yaml index 5e694040b2..f77612a46c 100644 --- a/docker/docker-compose.yaml +++ b/docker/docker-compose.yaml @@ -37,6 +37,9 @@ x-default-isaac-lab-volumes: &default-isaac-lab-volumes - type: bind source: ../source target: ${DOCKER_ISAACLAB_PATH}/source + - type: bind + source: /home/gursi/Desktop + target: ${DOCKER_USER_CODE_FILES} - type: bind source: ../docs target: ${DOCKER_ISAACLAB_PATH}/docs diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/__init__.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/__init__.py new file mode 100644 index 0000000000..fd33aacd3a --- /dev/null +++ b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configurations for the object lift environments.""" + +# We leave this file empty since we don't want to expose any configs in this package directly. +# We still need this file to import the "config" module in the parent package. diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/__init__.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/__init__.py new file mode 100644 index 0000000000..fd33aacd3a --- /dev/null +++ b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configurations for the object lift environments.""" + +# We leave this file empty since we don't want to expose any configs in this package directly. +# We still need this file to import the "config" module in the parent package. diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/__init__.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/__init__.py new file mode 100644 index 0000000000..d4fae66818 --- /dev/null +++ b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/__init__.py @@ -0,0 +1,42 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause +import gymnasium as gym +import os + +from . import agents + +## +# Register Gym environments. +## + +## +# Joint Position Control +## + +gym.register( + id="Isaac-Lift-Cube-Franka-Squishy-v0", + entry_point="omni.isaac.lab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": f"{__name__}.joint_pos_env_cfg:FrankaCubeLiftEnvCfg", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:LiftCubePPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_ppo_cfg.yaml", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml", + "sb3_cfg_entry_point": f"{agents.__name__}:sb3_ppo_cfg.yaml", + }, + disable_env_checker=True, +) + +gym.register( + id="Isaac-Lift-Cube-Franka-Play-Squishy-v0", + entry_point="omni.isaac.lab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": f"{__name__}.joint_pos_env_cfg:FrankaCubeLiftEnvCfg_PLAY", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:LiftCubePPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_ppo_cfg.yaml", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml", + "sb3_cfg_entry_point": f"{agents.__name__}:sb3_ppo_cfg.yaml", + }, + disable_env_checker=True, +) diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/agents/__init__.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/agents/__init__.py new file mode 100644 index 0000000000..e75ca2bc3f --- /dev/null +++ b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/agents/__init__.py @@ -0,0 +1,4 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/agents/rl_games_ppo_cfg.yaml b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/agents/rl_games_ppo_cfg.yaml new file mode 100644 index 0000000000..6ed68d8891 --- /dev/null +++ b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/agents/rl_games_ppo_cfg.yaml @@ -0,0 +1,79 @@ +params: + seed: 42 + + # environment wrapper clipping + env: + clip_observations: 100.0 + clip_actions: 100.0 + + algo: + name: a2c_continuous + + model: + name: continuous_a2c_logstd + + network: + name: actor_critic + separate: False + space: + continuous: + mu_activation: None + sigma_activation: None + + mu_init: + name: default + sigma_init: + name: const_initializer + val: 0 + fixed_sigma: True + mlp: + units: [256, 128, 64] + activation: elu + d2rl: False + + initializer: + name: default + regularizer: + name: None + + load_checkpoint: False # flag which sets whether to load the checkpoint + load_path: '' # path to the checkpoint to load + + config: + name: franka_lift + env_name: rlgpu + device: 'cuda:0' + device_name: 'cuda:0' + multi_gpu: False + ppo: True + mixed_precision: False + normalize_input: True + normalize_value: True + value_bootstrap: False + num_actors: -1 + reward_shaper: + scale_value: 0.01 + normalize_advantage: True + gamma: 0.99 + tau: 0.95 + learning_rate: 1e-4 + lr_schedule: adaptive + schedule_type: legacy + kl_threshold: 0.01 + score_to_win: 100000000 + max_epochs: 1500 + save_best_after: 100 + save_frequency: 50 + print_stats: True + grad_norm: 1.0 + entropy_coef: 0.001 + truncate_grads: True + e_clip: 0.2 + horizon_length: 24 + minibatch_size: 24576 + mini_epochs: 8 + critic_coef: 4 + clip_value: True + clip_actions: False + seq_len: 4 + bounds_loss_coef: 0.0001 diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/agents/robomimic/bc.json b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/agents/robomimic/bc.json new file mode 100644 index 0000000000..e96f7f7e19 --- /dev/null +++ b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/agents/robomimic/bc.json @@ -0,0 +1,264 @@ +{ + "algo_name": "bc", + "experiment": { + "name": "bc", + "validate": true, + "logging": { + "terminal_output_to_txt": true, + "log_tb": true + }, + "save": { + "enabled": true, + "every_n_seconds": null, + "every_n_epochs": 50, + "epochs": [], + "on_best_validation": false, + "on_best_rollout_return": false, + "on_best_rollout_success_rate": true + }, + "epoch_every_n_steps": 100, + "validation_epoch_every_n_steps": 10, + "env": null, + "additional_envs": null, + "render": false, + "render_video": true, + "keep_all_videos": false, + "video_skip": 5, + "rollout": { + "enabled": false, + "n": 50, + "horizon": 400, + "rate": 50, + "warmstart": 0, + "terminate_on_success": true + } + }, + "train": { + "data": null, + "output_dir": "../bc_trained_models", + "num_data_workers": 0, + "hdf5_cache_mode": "all", + "hdf5_use_swmr": true, + "hdf5_normalize_obs": false, + "hdf5_filter_key": "train", + "hdf5_validation_filter_key": "valid", + "seq_length": 1, + "dataset_keys": [ + "actions", + "rewards", + "dones" + ], + "goal_mode": null, + "cuda": true, + "batch_size": 100, + "num_epochs": 200, + "seed": 1 + }, + "algo": { + "optim_params": { + "policy": { + "learning_rate": { + "initial": 0.0001, + "decay_factor": 0.1, + "epoch_schedule": [] + }, + "regularization": { + "L2": 0.0 + } + } + }, + "loss": { + "l2_weight": 1.0, + "l1_weight": 0.0, + "cos_weight": 0.0 + }, + "actor_layer_dims": [ + 1024, + 1024 + ], + "gaussian": { + "enabled": false, + "fixed_std": false, + "init_std": 0.1, + "min_std": 0.01, + "std_activation": "softplus", + "low_noise_eval": true + }, + "gmm": { + "enabled": false, + "num_modes": 5, + "min_std": 0.0001, + "std_activation": "softplus", + "low_noise_eval": true + }, + "vae": { + "enabled": false, + "latent_dim": 14, + "latent_clip": null, + "kl_weight": 1.0, + "decoder": { + "is_conditioned": true, + "reconstruction_sum_across_elements": false + }, + "prior": { + "learn": false, + "is_conditioned": false, + "use_gmm": false, + "gmm_num_modes": 10, + "gmm_learn_weights": false, + "use_categorical": false, + "categorical_dim": 10, + "categorical_gumbel_softmax_hard": false, + "categorical_init_temp": 1.0, + "categorical_temp_anneal_step": 0.001, + "categorical_min_temp": 0.3 + }, + "encoder_layer_dims": [ + 300, + 400 + ], + "decoder_layer_dims": [ + 300, + 400 + ], + "prior_layer_dims": [ + 300, + 400 + ] + }, + "rnn": { + "enabled": false, + "horizon": 10, + "hidden_dim": 400, + "rnn_type": "LSTM", + "num_layers": 2, + "open_loop": false, + "kwargs": { + "bidirectional": false + } + } + }, + "observation": { + "modalities": { + "obs": { + "low_dim": [ + "joint_pos", + "joint_vel", + "object_position", + "target_object_position" + ], + "rgb": [], + "depth": [], + "scan": [] + }, + "goal": { + "low_dim": [], + "rgb": [], + "depth": [], + "scan": [] + } + }, + "encoder": { + "low_dim": { + "core_class": null, + "core_kwargs": {}, + "obs_randomizer_class": null, + "obs_randomizer_kwargs": {} + }, + "rgb": { + "core_class": "VisualCore", + "core_kwargs": { + "feature_dimension": 64, + "flatten": true, + "backbone_class": "ResNet18Conv", + "backbone_kwargs": { + "pretrained": false, + "input_coord_conv": false + }, + "pool_class": "SpatialSoftmax", + "pool_kwargs": { + "num_kp": 32, + "learnable_temperature": false, + "temperature": 1.0, + "noise_std": 0.0, + "output_variance": false + } + }, + "obs_randomizer_class": null, + "obs_randomizer_kwargs": { + "crop_height": 76, + "crop_width": 76, + "num_crops": 1, + "pos_enc": false + } + }, + "depth": { + "core_class": "VisualCore", + "core_kwargs": { + "feature_dimension": 64, + "flatten": true, + "backbone_class": "ResNet18Conv", + "backbone_kwargs": { + "pretrained": false, + "input_coord_conv": false + }, + "pool_class": "SpatialSoftmax", + "pool_kwargs": { + "num_kp": 32, + "learnable_temperature": false, + "temperature": 1.0, + "noise_std": 0.0, + "output_variance": false + } + }, + "obs_randomizer_class": null, + "obs_randomizer_kwargs": { + "crop_height": 76, + "crop_width": 76, + "num_crops": 1, + "pos_enc": false + } + }, + "scan": { + "core_class": "ScanCore", + "core_kwargs": { + "feature_dimension": 64, + "flatten": true, + "pool_class": "SpatialSoftmax", + "pool_kwargs": { + "num_kp": 32, + "learnable_temperature": false, + "temperature": 1.0, + "noise_std": 0.0, + "output_variance": false + }, + "conv_activation": "relu", + "conv_kwargs": { + "out_channels": [ + 32, + 64, + 64 + ], + "kernel_size": [ + 8, + 4, + 2 + ], + "stride": [ + 4, + 2, + 1 + ] + } + }, + "obs_randomizer_class": null, + "obs_randomizer_kwargs": { + "crop_height": 76, + "crop_width": 76, + "num_crops": 1, + "pos_enc": false + } + } + } + } +} diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/agents/robomimic/bcq.json b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/agents/robomimic/bcq.json new file mode 100644 index 0000000000..1d80b50d28 --- /dev/null +++ b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/agents/robomimic/bcq.json @@ -0,0 +1,299 @@ +{ + "algo_name": "bcq", + "experiment": { + "name": "bcq", + "validate": true, + "logging": { + "terminal_output_to_txt": true, + "log_tb": true + }, + "save": { + "enabled": true, + "every_n_seconds": null, + "every_n_epochs": 50, + "epochs": [], + "on_best_validation": true, + "on_best_rollout_return": false, + "on_best_rollout_success_rate": false + }, + "epoch_every_n_steps": 100, + "validation_epoch_every_n_steps": 10, + "env": null, + "additional_envs": null, + "render": false, + "render_video": true, + "keep_all_videos": false, + "video_skip": 5, + "rollout": { + "enabled": false, + "n": 50, + "horizon": 400, + "rate": 50, + "warmstart": 0, + "terminate_on_success": true + } + }, + "train": { + "data": null, + "output_dir": "../bcq_trained_models", + "num_data_workers": 0, + "hdf5_cache_mode": "all", + "hdf5_use_swmr": true, + "hdf5_normalize_obs": false, + "hdf5_filter_key": null, + "seq_length": 1, + "dataset_keys": [ + "actions", + "rewards", + "dones" + ], + "goal_mode": null, + "cuda": true, + "batch_size": 100, + "num_epochs": 200, + "seed": 1 + }, + "algo": { + "optim_params": { + "critic": { + "learning_rate": { + "initial": 0.001, + "decay_factor": 0.1, + "epoch_schedule": [] + }, + "regularization": { + "L2": 0.0 + }, + "start_epoch": -1, + "end_epoch": -1 + }, + "action_sampler": { + "learning_rate": { + "initial": 0.001, + "decay_factor": 0.1, + "epoch_schedule": [] + }, + "regularization": { + "L2": 0.0 + }, + "start_epoch": -1, + "end_epoch": -1 + }, + "actor": { + "learning_rate": { + "initial": 0.001, + "decay_factor": 0.1, + "epoch_schedule": [] + }, + "regularization": { + "L2": 0.0 + }, + "start_epoch": -1, + "end_epoch": -1 + } + }, + "discount": 0.99, + "n_step": 1, + "target_tau": 0.005, + "infinite_horizon": false, + "critic": { + "use_huber": false, + "max_gradient_norm": null, + "value_bounds": null, + "num_action_samples": 10, + "num_action_samples_rollout": 100, + "ensemble": { + "n": 2, + "weight": 0.75 + }, + "distributional": { + "enabled": false, + "num_atoms": 51 + }, + "layer_dims": [ + 300, + 400 + ] + }, + "action_sampler": { + "actor_layer_dims": [ + 1024, + 1024 + ], + "gmm": { + "enabled": false, + "num_modes": 5, + "min_std": 0.0001, + "std_activation": "softplus", + "low_noise_eval": true + }, + "vae": { + "enabled": true, + "latent_dim": 14, + "latent_clip": null, + "kl_weight": 1.0, + "decoder": { + "is_conditioned": true, + "reconstruction_sum_across_elements": false + }, + "prior": { + "learn": false, + "is_conditioned": false, + "use_gmm": false, + "gmm_num_modes": 10, + "gmm_learn_weights": false, + "use_categorical": false, + "categorical_dim": 10, + "categorical_gumbel_softmax_hard": false, + "categorical_init_temp": 1.0, + "categorical_temp_anneal_step": 0.001, + "categorical_min_temp": 0.3 + }, + "encoder_layer_dims": [ + 300, + 400 + ], + "decoder_layer_dims": [ + 300, + 400 + ], + "prior_layer_dims": [ + 300, + 400 + ] + }, + "freeze_encoder_epoch": -1 + }, + "actor": { + "enabled": false, + "perturbation_scale": 0.05, + "layer_dims": [ + 300, + 400 + ] + } + }, + "observation": { + "modalities": { + "obs": { + "low_dim": [ + "tool_dof_pos_scaled", + "tool_positions", + "object_relative_tool_positions", + "object_desired_positions" + ], + "rgb": [], + "depth": [], + "scan": [] + }, + "goal": { + "low_dim": [], + "rgb": [], + "depth": [], + "scan": [] + } + }, + "encoder": { + "low_dim": { + "core_class": null, + "core_kwargs": {}, + "obs_randomizer_class": null, + "obs_randomizer_kwargs": {} + }, + "rgb": { + "core_class": "VisualCore", + "core_kwargs": { + "feature_dimension": 64, + "flatten": true, + "backbone_class": "ResNet18Conv", + "backbone_kwargs": { + "pretrained": false, + "input_coord_conv": false + }, + "pool_class": "SpatialSoftmax", + "pool_kwargs": { + "num_kp": 32, + "learnable_temperature": false, + "temperature": 1.0, + "noise_std": 0.0, + "output_variance": false + } + }, + "obs_randomizer_class": null, + "obs_randomizer_kwargs": { + "crop_height": 76, + "crop_width": 76, + "num_crops": 1, + "pos_enc": false + } + }, + "depth": { + "core_class": "VisualCore", + "core_kwargs": { + "feature_dimension": 64, + "flatten": true, + "backbone_class": "ResNet18Conv", + "backbone_kwargs": { + "pretrained": false, + "input_coord_conv": false + }, + "pool_class": "SpatialSoftmax", + "pool_kwargs": { + "num_kp": 32, + "learnable_temperature": false, + "temperature": 1.0, + "noise_std": 0.0, + "output_variance": false + } + }, + "obs_randomizer_class": null, + "obs_randomizer_kwargs": { + "crop_height": 76, + "crop_width": 76, + "num_crops": 1, + "pos_enc": false + } + }, + "scan": { + "core_class": "ScanCore", + "core_kwargs": { + "feature_dimension": 64, + "flatten": true, + "pool_class": "SpatialSoftmax", + "pool_kwargs": { + "num_kp": 32, + "learnable_temperature": false, + "temperature": 1.0, + "noise_std": 0.0, + "output_variance": false + }, + "conv_activation": "relu", + "conv_kwargs": { + "out_channels": [ + 32, + 64, + 64 + ], + "kernel_size": [ + 8, + 4, + 2 + ], + "stride": [ + 4, + 2, + 1 + ] + } + }, + "obs_randomizer_class": null, + "obs_randomizer_kwargs": { + "crop_height": 76, + "crop_width": 76, + "num_crops": 1, + "pos_enc": false + } + } + } + } +} diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/agents/rsl_rl_ppo_cfg.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/agents/rsl_rl_ppo_cfg.py new file mode 100644 index 0000000000..c6cfd25840 --- /dev/null +++ b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/agents/rsl_rl_ppo_cfg.py @@ -0,0 +1,41 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from omni.isaac.lab.utils import configclass + +from omni.isaac.lab_tasks.utils.wrappers.rsl_rl import ( + RslRlOnPolicyRunnerCfg, + RslRlPpoActorCriticCfg, + RslRlPpoAlgorithmCfg, +) + + +@configclass +class LiftCubePPORunnerCfg(RslRlOnPolicyRunnerCfg): + num_steps_per_env = 24 + max_iterations = 1500 + save_interval = 50 + experiment_name = "franka_lift" + empirical_normalization = False + policy = RslRlPpoActorCriticCfg( + init_noise_std=1.0, + actor_hidden_dims=[256, 128, 64], + critic_hidden_dims=[256, 128, 64], + activation="elu", + ) + algorithm = RslRlPpoAlgorithmCfg( + value_loss_coef=1.0, + use_clipped_value_loss=True, + clip_param=0.2, + entropy_coef=0.006, + num_learning_epochs=5, + num_mini_batches=4, + learning_rate=1.0e-4, + schedule="adaptive", + gamma=0.98, + lam=0.95, + desired_kl=0.01, + max_grad_norm=1.0, + ) diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/agents/sb3_ppo_cfg.yaml b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/agents/sb3_ppo_cfg.yaml new file mode 100644 index 0000000000..6d6f15781a --- /dev/null +++ b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/agents/sb3_ppo_cfg.yaml @@ -0,0 +1,28 @@ +# Reference: https://github.com/DLR-RM/rl-baselines3-zoo/blob/master/hyperparams/ppo.yml#L32 +seed: 42 + +# epoch * n_steps * nenvs: 500×512*8*8 +n_timesteps: 16384000 +policy: 'MlpPolicy' +n_steps: 64 +# mini batch size: num_envs * nsteps / nminibatches 2048×512÷2048 +batch_size: 192 +gae_lambda: 0.95 +gamma: 0.99 +n_epochs: 8 +ent_coef: 0.00 +vf_coef: 0.0001 +learning_rate: !!float 3e-4 +clip_range: 0.2 +policy_kwargs: "dict( + activation_fn=nn.ELU, + net_arch=dict(pi=[256, 128, 64], vf=[256, 128, 64]) + )" +target_kl: 0.01 +max_grad_norm: 1.0 + +# # Uses VecNormalize class to normalize obs +# normalize_input: True +# # Uses VecNormalize class to normalize rew +# normalize_value: True +# clip_obs: 5 diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/agents/skrl_ppo_cfg.yaml b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/agents/skrl_ppo_cfg.yaml new file mode 100644 index 0000000000..94337d46ba --- /dev/null +++ b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/agents/skrl_ppo_cfg.yaml @@ -0,0 +1,80 @@ +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +models: + separate: False + policy: # see gaussian_model parameters + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: 0.0 + network: + - name: net + input: STATES + layers: [256, 128, 64] + activations: elu + output: ACTIONS + value: # see deterministic_model parameters + class: DeterministicMixin + clip_actions: False + network: + - name: net + input: STATES + layers: [256, 128, 64] + activations: elu + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 24 + learning_epochs: 8 + mini_batches: 4 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 1.0e-04 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.01 + state_preprocessor: RunningStandardScaler + state_preprocessor_kwargs: null + value_preprocessor: RunningStandardScaler + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.001 + value_loss_scale: 2.0 + kl_threshold: 0.0 + rewards_shaper_scale: 0.01 + time_limit_bootstrap: False + # logging and checkpoint + experiment: + directory: "franka_lift" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 36000 + environment_info: log diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/joint_pos_env_cfg.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/joint_pos_env_cfg.py new file mode 100644 index 0000000000..d9f15c58e9 --- /dev/null +++ b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/joint_pos_env_cfg.py @@ -0,0 +1,106 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from omni.isaac.lab.assets import RigidObjectCfg, DeformableObjectCfg +import omni.isaac.lab.sim as sim_utils +from omni.isaac.lab.sensors import FrameTransformerCfg +from omni.isaac.lab.sensors.frame_transformer.frame_transformer_cfg import OffsetCfg +from omni.isaac.lab.sim.schemas.schemas_cfg import RigidBodyPropertiesCfg +from omni.isaac.lab.sim.spawners.from_files.from_files_cfg import UsdFileCfg +from omni.isaac.lab.utils import configclass +from omni.isaac.lab.utils.assets import ISAAC_NUCLEUS_DIR + +from omni.isaac.lab_tasks.manager_based.manipulation.lift import mdp +from omni.isaac.lab_tasks.manager_based.manipulation.lift.lift_env_cfg import LiftEnvCfg + +## +# Pre-defined configs +## +from omni.isaac.lab.markers.config import FRAME_MARKER_CFG # isort: skip +from omni.isaac.lab_assets.franka import FRANKA_PANDA_CFG # isort: skip + + +@configclass +class FrankaCubeLiftEnvCfg(LiftEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # Set Franka as robot + self.scene.robot = FRANKA_PANDA_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + + # Set actions for the specific robot type (franka) + self.actions.arm_action = mdp.JointPositionActionCfg( + asset_name="robot", joint_names=["panda_joint.*"], scale=0.5, use_default_offset=True + ) + self.actions.gripper_action = mdp.BinaryJointPositionActionCfg( + asset_name="robot", + joint_names=["panda_finger.*"], + open_command_expr={"panda_finger_.*": 0.04}, + close_command_expr={"panda_finger_.*": 0.0}, + ) + # Set the body name for the end effector + self.commands.object_pose.body_name = "panda_hand" + + self.scene.object = DeformableObjectCfg( + prim_path="{ENV_REGEX_NS}/Object", + spawn=sim_utils.MeshCuboidCfg( + size=(0.11, 0.11, 0.11), + deformable_props=sim_utils.DeformableBodyPropertiesCfg(rest_offset=0.0, contact_offset=0.001), + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.5, 0.1, 0.0)), + physics_material=sim_utils.DeformableBodyMaterialCfg(poissons_ratio=0.4, youngs_modulus=1e5), + ), + init_state=DeformableObjectCfg.InitialStateCfg(pos=[0.5, 0, 0.055], rot=[1, 0, 0, 0]), + debug_vis=True, + ) + + # Set Cube as object + # self.scene.object = RigidObjectCfg( + # prim_path="{ENV_REGEX_NS}/Object", + # init_state=RigidObjectCfg.InitialStateCfg(pos=[0.5, 0, 0.055], rot=[1, 0, 0, 0]), + # spawn=UsdFileCfg( + # usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/DexCube/dex_cube_instanceable.usd", + # scale=(0.8, 0.8, 0.8), + # rigid_props=RigidBodyPropertiesCfg( + # solver_position_iteration_count=16, + # solver_velocity_iteration_count=1, + # max_angular_velocity=1000.0, + # max_linear_velocity=1000.0, + # max_depenetration_velocity=5.0, + # disable_gravity=False, + # ), + # ), + # ) + + # Listens to the required transforms + marker_cfg = FRAME_MARKER_CFG.copy() + marker_cfg.markers["frame"].scale = (0.1, 0.1, 0.1) + marker_cfg.prim_path = "/Visuals/FrameTransformer" + self.scene.ee_frame = FrameTransformerCfg( + prim_path="{ENV_REGEX_NS}/Robot/panda_link0", + debug_vis=False, + visualizer_cfg=marker_cfg, + target_frames=[ + FrameTransformerCfg.FrameCfg( + prim_path="{ENV_REGEX_NS}/Robot/panda_hand", + name="end_effector", + offset=OffsetCfg( + pos=[0.0, 0.0, 0.1034], + ), + ), + ], + ) + + +@configclass +class FrankaCubeLiftEnvCfg_PLAY(FrankaCubeLiftEnvCfg): + 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 diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/lift_env_cfg.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/lift_env_cfg.py new file mode 100644 index 0000000000..955554108c --- /dev/null +++ b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/lift_env_cfg.py @@ -0,0 +1,222 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from dataclasses import MISSING + +import omni.isaac.lab.sim as sim_utils +from omni.isaac.lab.assets import ArticulationCfg, AssetBaseCfg, DeformableObjectCfg, RigidObjectCfg +from omni.isaac.lab.envs import ManagerBasedRLEnvCfg +from omni.isaac.lab.managers import CurriculumTermCfg as CurrTerm +from omni.isaac.lab.managers import EventTermCfg as EventTerm +from omni.isaac.lab.managers import ObservationGroupCfg as ObsGroup +from omni.isaac.lab.managers import ObservationTermCfg as ObsTerm +from omni.isaac.lab.managers import RewardTermCfg as RewTerm +from omni.isaac.lab.managers import SceneEntityCfg +from omni.isaac.lab.managers import TerminationTermCfg as DoneTerm +from omni.isaac.lab.scene import InteractiveSceneCfg +from omni.isaac.lab.sensors.frame_transformer.frame_transformer_cfg import FrameTransformerCfg +from omni.isaac.lab.sim.spawners.from_files.from_files_cfg import GroundPlaneCfg, UsdFileCfg +from omni.isaac.lab.utils import configclass +from omni.isaac.lab.utils.assets import ISAAC_NUCLEUS_DIR + +from . import mdp + +## +# Scene definition +## + + +@configclass +class ObjectTableSceneCfg(InteractiveSceneCfg): + """Configuration for the lift scene with a robot and a object. + This is the abstract base implementation, the exact scene is defined in the derived classes + which need to set the target object, robot and end-effector frames + """ + + # robots: will be populated by agent env cfg + robot: ArticulationCfg = MISSING + # end-effector sensor: will be populated by agent env cfg + ee_frame: FrameTransformerCfg = MISSING + # target object: will be populated by agent env cfg + object: RigidObjectCfg | DeformableObjectCfg = MISSING + + # Table + table = AssetBaseCfg( + prim_path="{ENV_REGEX_NS}/Table", + init_state=AssetBaseCfg.InitialStateCfg(pos=[0.5, 0, 0], rot=[0.707, 0, 0, 0.707]), + spawn=UsdFileCfg(usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/SeattleLabTable/table_instanceable.usd"), + ) + + # plane + plane = AssetBaseCfg( + prim_path="/World/GroundPlane", + init_state=AssetBaseCfg.InitialStateCfg(pos=[0, 0, -1.05]), + spawn=GroundPlaneCfg(), + ) + + # lights + light = AssetBaseCfg( + prim_path="/World/light", + spawn=sim_utils.DomeLightCfg(color=(0.75, 0.75, 0.75), intensity=3000.0), + ) + + +## +# MDP settings +## + + +@configclass +class CommandsCfg: + """Command terms for the MDP.""" + + object_pose = mdp.UniformPoseCommandCfg( + asset_name="robot", + body_name=MISSING, # will be set by agent env cfg + resampling_time_range=(5.0, 5.0), + debug_vis=True, + ranges=mdp.UniformPoseCommandCfg.Ranges( + pos_x=(0.4, 0.6), pos_y=(-0.25, 0.25), pos_z=(0.25, 0.5), roll=(0.0, 0.0), pitch=(0.0, 0.0), yaw=(0.0, 0.0) + ), + ) + + +@configclass +class ActionsCfg: + """Action specifications for the MDP.""" + + # will be set by agent env cfg + arm_action: mdp.JointPositionActionCfg | mdp.DifferentialInverseKinematicsActionCfg = MISSING + gripper_action: mdp.BinaryJointPositionActionCfg = MISSING + + +@configclass +class ObservationsCfg: + """Observation specifications for the MDP.""" + + @configclass + class PolicyCfg(ObsGroup): + """Observations for policy group.""" + + joint_pos = ObsTerm(func=mdp.joint_pos_rel) + joint_vel = ObsTerm(func=mdp.joint_vel_rel) + object_position = ObsTerm(func=mdp.object_position_in_robot_root_frame) + target_object_position = ObsTerm(func=mdp.generated_commands, params={"command_name": "object_pose"}) + 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_all = EventTerm(func=mdp.reset_scene_to_default, mode="reset") + + reset_object_position = EventTerm( + func=mdp.reset_root_state_uniform, + mode="reset", + params={ + "pose_range": {"x": (-0.1, 0.1), "y": (-0.25, 0.25), "z": (0.0, 0.0)}, + "velocity_range": {}, + "asset_cfg": SceneEntityCfg("object", body_names="Object"), + }, + ) + + +@configclass +class RewardsCfg: + """Reward terms for the MDP.""" + + reaching_object = RewTerm(func=mdp.object_ee_distance, params={"std": 0.1}, weight=1.0) + + lifting_object = RewTerm(func=mdp.object_is_lifted, params={"minimal_height": 0.04}, weight=15.0) + + object_goal_tracking = RewTerm( + func=mdp.object_goal_distance, + params={"std": 0.3, "minimal_height": 0.04, "command_name": "object_pose"}, + weight=16.0, + ) + + object_goal_tracking_fine_grained = RewTerm( + func=mdp.object_goal_distance, + params={"std": 0.05, "minimal_height": 0.04, "command_name": "object_pose"}, + weight=5.0, + ) + + # action penalty + action_rate = RewTerm(func=mdp.action_rate_l2, weight=-1e-4) + + joint_vel = RewTerm( + func=mdp.joint_vel_l2, + weight=-1e-4, + params={"asset_cfg": SceneEntityCfg("robot")}, + ) + + +@configclass +class TerminationsCfg: + """Termination terms for the MDP.""" + + time_out = DoneTerm(func=mdp.time_out, time_out=True) + + object_dropping = DoneTerm( + func=mdp.root_height_below_minimum, params={"minimum_height": -0.05, "asset_cfg": SceneEntityCfg("object")} + ) + + +@configclass +class CurriculumCfg: + """Curriculum terms for the MDP.""" + + action_rate = CurrTerm( + func=mdp.modify_reward_weight, params={"term_name": "action_rate", "weight": -1e-1, "num_steps": 10000} + ) + + joint_vel = CurrTerm( + func=mdp.modify_reward_weight, params={"term_name": "joint_vel", "weight": -1e-1, "num_steps": 10000} + ) + + +## +# Environment configuration +## + + +@configclass +class LiftEnvCfg(ManagerBasedRLEnvCfg): + """Configuration for the lifting environment.""" + + # Scene settings + scene: ObjectTableSceneCfg = ObjectTableSceneCfg(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.episode_length_s = 5.0 + # simulation settings + self.sim.dt = 0.01 # 100Hz + self.sim.render_interval = self.decimation + + self.sim.physx.bounce_threshold_velocity = 0.2 + self.sim.physx.bounce_threshold_velocity = 0.01 + self.sim.physx.gpu_found_lost_aggregate_pairs_capacity = 1024 * 1024 * 4 + self.sim.physx.gpu_total_aggregate_pairs_capacity = 16 * 1024 + self.sim.physx.friction_correlation_distance = 0.00625 diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/mdp/__init__.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/mdp/__init__.py new file mode 100644 index 0000000000..e8b879f89b --- /dev/null +++ b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/mdp/__init__.py @@ -0,0 +1,12 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""This sub-module contains the functions that are specific to the lift environments.""" + +from omni.isaac.lab.envs.mdp import * # noqa: F401, F403 + +from .observations import * # noqa: F401, F403 +from .rewards import * # noqa: F401, F403 +from .terminations import * # noqa: F401, F403 diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/mdp/observations.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/mdp/observations.py new file mode 100644 index 0000000000..605ff9f8c2 --- /dev/null +++ b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/mdp/observations.py @@ -0,0 +1,31 @@ +# 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 omni.isaac.lab.assets import RigidObject +from omni.isaac.lab.managers import SceneEntityCfg +from omni.isaac.lab.utils.math import subtract_frame_transforms + +if TYPE_CHECKING: + from omni.isaac.lab.envs import ManagerBasedRLEnv + + +def object_position_in_robot_root_frame( + env: ManagerBasedRLEnv, + robot_cfg: SceneEntityCfg = SceneEntityCfg("robot"), + object_cfg: SceneEntityCfg = SceneEntityCfg("object"), +) -> torch.Tensor: + """The position of the object in the robot's root frame.""" + robot: RigidObject = env.scene[robot_cfg.name] + object: RigidObject = env.scene[object_cfg.name] + object_pos_w = object.data.root_pos_w[:, :3] + object_pos_b, _ = subtract_frame_transforms( + robot.data.root_link_state_w[:, :3], robot.data.root_link_state_w[:, 3:7], object_pos_w + ) + return object_pos_b diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/mdp/rewards.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/mdp/rewards.py new file mode 100644 index 0000000000..ba9e4115b7 --- /dev/null +++ b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/mdp/rewards.py @@ -0,0 +1,69 @@ +# 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 omni.isaac.lab.assets import RigidObject +from omni.isaac.lab.managers import SceneEntityCfg +from omni.isaac.lab.sensors import FrameTransformer +from omni.isaac.lab.utils.math import combine_frame_transforms + +if TYPE_CHECKING: + from omni.isaac.lab.envs import ManagerBasedRLEnv + + +def object_is_lifted( + env: ManagerBasedRLEnv, minimal_height: float, object_cfg: SceneEntityCfg = SceneEntityCfg("object") +) -> torch.Tensor: + """Reward the agent for lifting the object above the minimal height.""" + object: RigidObject = env.scene[object_cfg.name] + return torch.where(object.data.root_pos_w[:, 2] > minimal_height, 1.0, 0.0) + + +def object_ee_distance( + env: ManagerBasedRLEnv, + std: float, + object_cfg: SceneEntityCfg = SceneEntityCfg("object"), + ee_frame_cfg: SceneEntityCfg = SceneEntityCfg("ee_frame"), +) -> torch.Tensor: + """Reward the agent for reaching the object using tanh-kernel.""" + # extract the used quantities (to enable type-hinting) + object: RigidObject = env.scene[object_cfg.name] + ee_frame: FrameTransformer = env.scene[ee_frame_cfg.name] + # Target object position: (num_envs, 3) + cube_pos_w = object.data.root_pos_w + # End-effector position: (num_envs, 3) + ee_w = ee_frame.data.target_pos_w[..., 0, :] + # Distance of the end-effector to the object: (num_envs,) + object_ee_distance = torch.norm(cube_pos_w - ee_w, dim=1) + + return 1 - torch.tanh(object_ee_distance / std) + + +def object_goal_distance( + env: ManagerBasedRLEnv, + std: float, + minimal_height: float, + command_name: str, + robot_cfg: SceneEntityCfg = SceneEntityCfg("robot"), + object_cfg: SceneEntityCfg = SceneEntityCfg("object"), +) -> torch.Tensor: + """Reward the agent for tracking the goal pose using tanh-kernel.""" + # extract the used quantities (to enable type-hinting) + robot: RigidObject = env.scene[robot_cfg.name] + object: RigidObject = env.scene[object_cfg.name] + command = env.command_manager.get_command(command_name) + # compute the desired position in the world frame + des_pos_b = command[:, :3] + des_pos_w, _ = combine_frame_transforms( + robot.data.root_link_state_w[:, :3], robot.data.root_link_state_w[:, 3:7], des_pos_b + ) + # distance of the end-effector to the object: (num_envs,) + distance = torch.norm(des_pos_w - object.data.root_pos_w[:, :3], dim=1) + # rewarded if the object is lifted above the threshold + return (object.data.root_pos_w[:, 2] > minimal_height) * (1 - torch.tanh(distance / std)) diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/mdp/terminations.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/mdp/terminations.py new file mode 100644 index 0000000000..de2bae84fd --- /dev/null +++ b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/mdp/terminations.py @@ -0,0 +1,55 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Common functions that can be used to activate certain terminations for the lift task. + +The functions can be passed to the :class:`omni.isaac.lab.managers.TerminationTermCfg` object to enable +the termination introduced by the function. +""" + +from __future__ import annotations + +import torch +from typing import TYPE_CHECKING + +from omni.isaac.lab.assets import RigidObject +from omni.isaac.lab.managers import SceneEntityCfg +from omni.isaac.lab.utils.math import combine_frame_transforms + +if TYPE_CHECKING: + from omni.isaac.lab.envs import ManagerBasedRLEnv + + +def object_reached_goal( + env: ManagerBasedRLEnv, + command_name: str = "object_pose", + threshold: float = 0.02, + robot_cfg: SceneEntityCfg = SceneEntityCfg("robot"), + object_cfg: SceneEntityCfg = SceneEntityCfg("object"), +) -> torch.Tensor: + """Termination condition for the object reaching the goal position. + + Args: + env: The environment. + command_name: The name of the command that is used to control the object. + threshold: The threshold for the object to reach the goal position. Defaults to 0.02. + robot_cfg: The robot configuration. Defaults to SceneEntityCfg("robot"). + object_cfg: The object configuration. Defaults to SceneEntityCfg("object"). + + """ + # extract the used quantities (to enable type-hinting) + robot: RigidObject = env.scene[robot_cfg.name] + object: RigidObject = env.scene[object_cfg.name] + command = env.command_manager.get_command(command_name) + # compute the desired position in the world frame + des_pos_b = command[:, :3] + des_pos_w, _ = combine_frame_transforms( + robot.data.root_link_state_w[:, :3], robot.data.root_link_state_w[:, 3:7], des_pos_b + ) + # distance of the end-effector to the object: (num_envs,) + distance = torch.norm(des_pos_w - object.data.root_pos_w[:, :3], dim=1) + + # rewarded if the object is lifted above the threshold + return distance < threshold From 919206d9fc1960f4553f26310973edf9f05d678b Mon Sep 17 00:00:00 2001 From: gursi26 Date: Sun, 12 Jan 2025 23:48:52 -0500 Subject: [PATCH 2/6] added initial direct env code --- .../direct/deformable_cube/__init__.py | 0 .../deformable_cube/deformable_cube_env.py | 57 ++++ .../deformable_cube_env_cfg.py | 68 ++++ .../manipulation/liftsquishy/__init__.py | 9 - .../liftsquishy/config/__init__.py | 9 - .../liftsquishy/config/franka/__init__.py | 42 --- .../config/franka/agents/__init__.py | 4 - .../franka/agents/rl_games_ppo_cfg.yaml | 79 ----- .../config/franka/agents/robomimic/bc.json | 264 ---------------- .../config/franka/agents/robomimic/bcq.json | 299 ------------------ .../config/franka/agents/rsl_rl_ppo_cfg.py | 41 --- .../config/franka/agents/sb3_ppo_cfg.yaml | 28 -- .../config/franka/agents/skrl_ppo_cfg.yaml | 80 ----- .../config/franka/joint_pos_env_cfg.py | 106 ------- .../manipulation/liftsquishy/lift_env_cfg.py | 222 ------------- .../manipulation/liftsquishy/mdp/__init__.py | 12 - .../liftsquishy/mdp/observations.py | 31 -- .../manipulation/liftsquishy/mdp/rewards.py | 69 ---- .../liftsquishy/mdp/terminations.py | 55 ---- 19 files changed, 125 insertions(+), 1350 deletions(-) create mode 100644 source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/deformable_cube/__init__.py create mode 100644 source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/deformable_cube/deformable_cube_env.py create mode 100644 source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/deformable_cube/deformable_cube_env_cfg.py delete mode 100644 source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/__init__.py delete mode 100644 source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/__init__.py delete mode 100644 source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/__init__.py delete mode 100644 source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/agents/__init__.py delete mode 100644 source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/agents/rl_games_ppo_cfg.yaml delete mode 100644 source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/agents/robomimic/bc.json delete mode 100644 source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/agents/robomimic/bcq.json delete mode 100644 source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/agents/rsl_rl_ppo_cfg.py delete mode 100644 source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/agents/sb3_ppo_cfg.yaml delete mode 100644 source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/agents/skrl_ppo_cfg.yaml delete mode 100644 source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/joint_pos_env_cfg.py delete mode 100644 source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/lift_env_cfg.py delete mode 100644 source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/mdp/__init__.py delete mode 100644 source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/mdp/observations.py delete mode 100644 source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/mdp/rewards.py delete mode 100644 source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/mdp/terminations.py diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/deformable_cube/__init__.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/deformable_cube/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/deformable_cube/deformable_cube_env.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/deformable_cube/deformable_cube_env.py new file mode 100644 index 0000000000..f04802c0bf --- /dev/null +++ b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/deformable_cube/deformable_cube_env.py @@ -0,0 +1,57 @@ +from __future__ import annotations + +import math +import torch +import torch.nn.functional as F +from collections.abc import Sequence + + +import omni.isaac.lab.sim as sim_utils +from omni.isaac.lab.assets import RigidObjectCfg, RigidObject, Articulation, DeformableObject +from omni.isaac.lab.envs import DirectRLEnv, DirectRLEnvCfg +from omni.isaac.lab.scene import InteractiveSceneCfg +from omni.isaac.lab.sim import SimulationCfg +from omni.isaac.lab.sim.spawners.from_files import GroundPlaneCfg, spawn_ground_plane +from omni.isaac.lab.utils import configclass +from omni.isaac.lab.utils.math import quat_from_euler_xyz, euler_xyz_from_quat +from .deformabe_cube_env_cfg import DeformableCubeEnvCfg + +class DeformableCubeEnv(DirectRLEnv): + cfg: DeformableCubeEnvCfg + + def __init__(self, cfg: DeformableCubeEnvCfg, render_mode: str | None = None, **kwargs): + super().__init__(cfg, render_mode, **kwargs) + pass + + def _setup_scene(self): + self.robot = Articulation(self.cfg.robot_cfg) + self.object = DeformableObject(self.cfg.object_cfg) + # TODO: spawn container + + spawn_ground_plane(prim_path="/World/ground", cfg=GroundPlaneCfg()) + + # clone, filter, and replicate + self.scene.clone_environments(copy_from_source=False) + self.scene.filter_collisions(global_prim_paths=[]) + + # add lights + light_cfg = sim_utils.DomeLightCfg(intensity=2000.0, color=(0.75, 0.75, 0.75)) + light_cfg.func("/World/Light", light_cfg) + + def _pre_physics_step(self, actions: torch.Tensor) -> None: + pass + + def _apply_action(self) -> None: + pass + + def _get_observations(self) -> dict: + pass + + def _get_rewards(self) -> torch.Tensor: + pass + + def _get_dones(self) -> tuple[torch.Tensor, torch.Tensor]: + pass + + def _reset_idx(self, env_ids: Sequence[int] | None): + pass \ No newline at end of file diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/deformable_cube/deformable_cube_env_cfg.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/deformable_cube/deformable_cube_env_cfg.py new file mode 100644 index 0000000000..aedc0d5d4b --- /dev/null +++ b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/deformable_cube/deformable_cube_env_cfg.py @@ -0,0 +1,68 @@ +from __future__ import annotations + +from dataclasses import MISSING + +import omni.isaac.lab.sim as sim_utils +from omni.isaac.lab.assets import AssetBaseCfg, ArticulationCfg, DeformableObjectCfg +from omni.isaac.lab.envs import DirectRLEnv, DirectRLEnvCfg +from omni.isaac.lab.scene import InteractiveSceneCfg +from omni.isaac.lab.sim import SimulationCfg +from omni.isaac.lab.sim.spawners.from_files import GroundPlaneCfg, spawn_ground_plane, UsdFileCfg +from omni.isaac.lab.utils import configclass +from omni.isaac.lab.utils.math import quat_from_euler_xyz, euler_xyz_from_quat +from omni.isaac.lab.utils.assets import ISAAC_NUCLEUS_DIR + +from omni.isaac.lab_assets.franka import FRANKA_PANDA_CFG + +@configclass +class DeformableCubeEnvCfg(DirectRLEnvCfg): + # required params + num_envs = 256 + env_spacing = 4.0 + dt = 1 / 120 + observation_space = 15 + action_space = 3 + state_space = 0 + + # env params + decimation = 1 + episode_length_s = 15.0 + + # simulation + sim: SimulationCfg = SimulationCfg( + dt=dt, + render_interval=decimation + ) + + # robot + robot_cfg: ArticulationCfg = FRANKA_PANDA_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + + # objects + object_cfg: DeformableObjectCfg = DeformableObjectCfg( + prim_path="{ENV_REGEX_NS}/Cube", + spawn=sim_utils.MeshCuboidCfg( + size=(0.2, 0.2, 0.2), + deformable_props=sim_utils.DeformableBodyPropertiesCfg( + rest_offset=0.0, + contact_offset=0.001 + ), + visual_material=sim_utils.PreviewSurfaceCfg( + diffuse_color=(0.5, 0.1, 0.0) + ), + physics_material=sim_utils.DeformableBodyMaterialCfg( + poissons_ratio=0.4, + youngs_modulus=1e5 + ), + ), + init_state=DeformableObjectCfg.InitialStateCfg(), + debug_vis=True, + ) + + container_cfg: AssetBaseCfg = MISSING #TODO: fill this out + + # scene + scene: InteractiveSceneCfg = InteractiveSceneCfg( + num_envs=num_envs, + env_spacing=env_spacing, + replicate_physics=True, + ) \ No newline at end of file diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/__init__.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/__init__.py deleted file mode 100644 index fd33aacd3a..0000000000 --- a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/__init__.py +++ /dev/null @@ -1,9 +0,0 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. -# All rights reserved. -# -# SPDX-License-Identifier: BSD-3-Clause - -"""Configurations for the object lift environments.""" - -# We leave this file empty since we don't want to expose any configs in this package directly. -# We still need this file to import the "config" module in the parent package. diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/__init__.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/__init__.py deleted file mode 100644 index fd33aacd3a..0000000000 --- a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/__init__.py +++ /dev/null @@ -1,9 +0,0 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. -# All rights reserved. -# -# SPDX-License-Identifier: BSD-3-Clause - -"""Configurations for the object lift environments.""" - -# We leave this file empty since we don't want to expose any configs in this package directly. -# We still need this file to import the "config" module in the parent package. diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/__init__.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/__init__.py deleted file mode 100644 index d4fae66818..0000000000 --- a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/__init__.py +++ /dev/null @@ -1,42 +0,0 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. -# All rights reserved. -# -# SPDX-License-Identifier: BSD-3-Clause -import gymnasium as gym -import os - -from . import agents - -## -# Register Gym environments. -## - -## -# Joint Position Control -## - -gym.register( - id="Isaac-Lift-Cube-Franka-Squishy-v0", - entry_point="omni.isaac.lab.envs:ManagerBasedRLEnv", - kwargs={ - "env_cfg_entry_point": f"{__name__}.joint_pos_env_cfg:FrankaCubeLiftEnvCfg", - "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:LiftCubePPORunnerCfg", - "skrl_cfg_entry_point": f"{agents.__name__}:skrl_ppo_cfg.yaml", - "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml", - "sb3_cfg_entry_point": f"{agents.__name__}:sb3_ppo_cfg.yaml", - }, - disable_env_checker=True, -) - -gym.register( - id="Isaac-Lift-Cube-Franka-Play-Squishy-v0", - entry_point="omni.isaac.lab.envs:ManagerBasedRLEnv", - kwargs={ - "env_cfg_entry_point": f"{__name__}.joint_pos_env_cfg:FrankaCubeLiftEnvCfg_PLAY", - "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:LiftCubePPORunnerCfg", - "skrl_cfg_entry_point": f"{agents.__name__}:skrl_ppo_cfg.yaml", - "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml", - "sb3_cfg_entry_point": f"{agents.__name__}:sb3_ppo_cfg.yaml", - }, - disable_env_checker=True, -) diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/agents/__init__.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/agents/__init__.py deleted file mode 100644 index e75ca2bc3f..0000000000 --- a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/agents/__init__.py +++ /dev/null @@ -1,4 +0,0 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. -# All rights reserved. -# -# SPDX-License-Identifier: BSD-3-Clause diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/agents/rl_games_ppo_cfg.yaml b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/agents/rl_games_ppo_cfg.yaml deleted file mode 100644 index 6ed68d8891..0000000000 --- a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/agents/rl_games_ppo_cfg.yaml +++ /dev/null @@ -1,79 +0,0 @@ -params: - seed: 42 - - # environment wrapper clipping - env: - clip_observations: 100.0 - clip_actions: 100.0 - - algo: - name: a2c_continuous - - model: - name: continuous_a2c_logstd - - network: - name: actor_critic - separate: False - space: - continuous: - mu_activation: None - sigma_activation: None - - mu_init: - name: default - sigma_init: - name: const_initializer - val: 0 - fixed_sigma: True - mlp: - units: [256, 128, 64] - activation: elu - d2rl: False - - initializer: - name: default - regularizer: - name: None - - load_checkpoint: False # flag which sets whether to load the checkpoint - load_path: '' # path to the checkpoint to load - - config: - name: franka_lift - env_name: rlgpu - device: 'cuda:0' - device_name: 'cuda:0' - multi_gpu: False - ppo: True - mixed_precision: False - normalize_input: True - normalize_value: True - value_bootstrap: False - num_actors: -1 - reward_shaper: - scale_value: 0.01 - normalize_advantage: True - gamma: 0.99 - tau: 0.95 - learning_rate: 1e-4 - lr_schedule: adaptive - schedule_type: legacy - kl_threshold: 0.01 - score_to_win: 100000000 - max_epochs: 1500 - save_best_after: 100 - save_frequency: 50 - print_stats: True - grad_norm: 1.0 - entropy_coef: 0.001 - truncate_grads: True - e_clip: 0.2 - horizon_length: 24 - minibatch_size: 24576 - mini_epochs: 8 - critic_coef: 4 - clip_value: True - clip_actions: False - seq_len: 4 - bounds_loss_coef: 0.0001 diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/agents/robomimic/bc.json b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/agents/robomimic/bc.json deleted file mode 100644 index e96f7f7e19..0000000000 --- a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/agents/robomimic/bc.json +++ /dev/null @@ -1,264 +0,0 @@ -{ - "algo_name": "bc", - "experiment": { - "name": "bc", - "validate": true, - "logging": { - "terminal_output_to_txt": true, - "log_tb": true - }, - "save": { - "enabled": true, - "every_n_seconds": null, - "every_n_epochs": 50, - "epochs": [], - "on_best_validation": false, - "on_best_rollout_return": false, - "on_best_rollout_success_rate": true - }, - "epoch_every_n_steps": 100, - "validation_epoch_every_n_steps": 10, - "env": null, - "additional_envs": null, - "render": false, - "render_video": true, - "keep_all_videos": false, - "video_skip": 5, - "rollout": { - "enabled": false, - "n": 50, - "horizon": 400, - "rate": 50, - "warmstart": 0, - "terminate_on_success": true - } - }, - "train": { - "data": null, - "output_dir": "../bc_trained_models", - "num_data_workers": 0, - "hdf5_cache_mode": "all", - "hdf5_use_swmr": true, - "hdf5_normalize_obs": false, - "hdf5_filter_key": "train", - "hdf5_validation_filter_key": "valid", - "seq_length": 1, - "dataset_keys": [ - "actions", - "rewards", - "dones" - ], - "goal_mode": null, - "cuda": true, - "batch_size": 100, - "num_epochs": 200, - "seed": 1 - }, - "algo": { - "optim_params": { - "policy": { - "learning_rate": { - "initial": 0.0001, - "decay_factor": 0.1, - "epoch_schedule": [] - }, - "regularization": { - "L2": 0.0 - } - } - }, - "loss": { - "l2_weight": 1.0, - "l1_weight": 0.0, - "cos_weight": 0.0 - }, - "actor_layer_dims": [ - 1024, - 1024 - ], - "gaussian": { - "enabled": false, - "fixed_std": false, - "init_std": 0.1, - "min_std": 0.01, - "std_activation": "softplus", - "low_noise_eval": true - }, - "gmm": { - "enabled": false, - "num_modes": 5, - "min_std": 0.0001, - "std_activation": "softplus", - "low_noise_eval": true - }, - "vae": { - "enabled": false, - "latent_dim": 14, - "latent_clip": null, - "kl_weight": 1.0, - "decoder": { - "is_conditioned": true, - "reconstruction_sum_across_elements": false - }, - "prior": { - "learn": false, - "is_conditioned": false, - "use_gmm": false, - "gmm_num_modes": 10, - "gmm_learn_weights": false, - "use_categorical": false, - "categorical_dim": 10, - "categorical_gumbel_softmax_hard": false, - "categorical_init_temp": 1.0, - "categorical_temp_anneal_step": 0.001, - "categorical_min_temp": 0.3 - }, - "encoder_layer_dims": [ - 300, - 400 - ], - "decoder_layer_dims": [ - 300, - 400 - ], - "prior_layer_dims": [ - 300, - 400 - ] - }, - "rnn": { - "enabled": false, - "horizon": 10, - "hidden_dim": 400, - "rnn_type": "LSTM", - "num_layers": 2, - "open_loop": false, - "kwargs": { - "bidirectional": false - } - } - }, - "observation": { - "modalities": { - "obs": { - "low_dim": [ - "joint_pos", - "joint_vel", - "object_position", - "target_object_position" - ], - "rgb": [], - "depth": [], - "scan": [] - }, - "goal": { - "low_dim": [], - "rgb": [], - "depth": [], - "scan": [] - } - }, - "encoder": { - "low_dim": { - "core_class": null, - "core_kwargs": {}, - "obs_randomizer_class": null, - "obs_randomizer_kwargs": {} - }, - "rgb": { - "core_class": "VisualCore", - "core_kwargs": { - "feature_dimension": 64, - "flatten": true, - "backbone_class": "ResNet18Conv", - "backbone_kwargs": { - "pretrained": false, - "input_coord_conv": false - }, - "pool_class": "SpatialSoftmax", - "pool_kwargs": { - "num_kp": 32, - "learnable_temperature": false, - "temperature": 1.0, - "noise_std": 0.0, - "output_variance": false - } - }, - "obs_randomizer_class": null, - "obs_randomizer_kwargs": { - "crop_height": 76, - "crop_width": 76, - "num_crops": 1, - "pos_enc": false - } - }, - "depth": { - "core_class": "VisualCore", - "core_kwargs": { - "feature_dimension": 64, - "flatten": true, - "backbone_class": "ResNet18Conv", - "backbone_kwargs": { - "pretrained": false, - "input_coord_conv": false - }, - "pool_class": "SpatialSoftmax", - "pool_kwargs": { - "num_kp": 32, - "learnable_temperature": false, - "temperature": 1.0, - "noise_std": 0.0, - "output_variance": false - } - }, - "obs_randomizer_class": null, - "obs_randomizer_kwargs": { - "crop_height": 76, - "crop_width": 76, - "num_crops": 1, - "pos_enc": false - } - }, - "scan": { - "core_class": "ScanCore", - "core_kwargs": { - "feature_dimension": 64, - "flatten": true, - "pool_class": "SpatialSoftmax", - "pool_kwargs": { - "num_kp": 32, - "learnable_temperature": false, - "temperature": 1.0, - "noise_std": 0.0, - "output_variance": false - }, - "conv_activation": "relu", - "conv_kwargs": { - "out_channels": [ - 32, - 64, - 64 - ], - "kernel_size": [ - 8, - 4, - 2 - ], - "stride": [ - 4, - 2, - 1 - ] - } - }, - "obs_randomizer_class": null, - "obs_randomizer_kwargs": { - "crop_height": 76, - "crop_width": 76, - "num_crops": 1, - "pos_enc": false - } - } - } - } -} diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/agents/robomimic/bcq.json b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/agents/robomimic/bcq.json deleted file mode 100644 index 1d80b50d28..0000000000 --- a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/agents/robomimic/bcq.json +++ /dev/null @@ -1,299 +0,0 @@ -{ - "algo_name": "bcq", - "experiment": { - "name": "bcq", - "validate": true, - "logging": { - "terminal_output_to_txt": true, - "log_tb": true - }, - "save": { - "enabled": true, - "every_n_seconds": null, - "every_n_epochs": 50, - "epochs": [], - "on_best_validation": true, - "on_best_rollout_return": false, - "on_best_rollout_success_rate": false - }, - "epoch_every_n_steps": 100, - "validation_epoch_every_n_steps": 10, - "env": null, - "additional_envs": null, - "render": false, - "render_video": true, - "keep_all_videos": false, - "video_skip": 5, - "rollout": { - "enabled": false, - "n": 50, - "horizon": 400, - "rate": 50, - "warmstart": 0, - "terminate_on_success": true - } - }, - "train": { - "data": null, - "output_dir": "../bcq_trained_models", - "num_data_workers": 0, - "hdf5_cache_mode": "all", - "hdf5_use_swmr": true, - "hdf5_normalize_obs": false, - "hdf5_filter_key": null, - "seq_length": 1, - "dataset_keys": [ - "actions", - "rewards", - "dones" - ], - "goal_mode": null, - "cuda": true, - "batch_size": 100, - "num_epochs": 200, - "seed": 1 - }, - "algo": { - "optim_params": { - "critic": { - "learning_rate": { - "initial": 0.001, - "decay_factor": 0.1, - "epoch_schedule": [] - }, - "regularization": { - "L2": 0.0 - }, - "start_epoch": -1, - "end_epoch": -1 - }, - "action_sampler": { - "learning_rate": { - "initial": 0.001, - "decay_factor": 0.1, - "epoch_schedule": [] - }, - "regularization": { - "L2": 0.0 - }, - "start_epoch": -1, - "end_epoch": -1 - }, - "actor": { - "learning_rate": { - "initial": 0.001, - "decay_factor": 0.1, - "epoch_schedule": [] - }, - "regularization": { - "L2": 0.0 - }, - "start_epoch": -1, - "end_epoch": -1 - } - }, - "discount": 0.99, - "n_step": 1, - "target_tau": 0.005, - "infinite_horizon": false, - "critic": { - "use_huber": false, - "max_gradient_norm": null, - "value_bounds": null, - "num_action_samples": 10, - "num_action_samples_rollout": 100, - "ensemble": { - "n": 2, - "weight": 0.75 - }, - "distributional": { - "enabled": false, - "num_atoms": 51 - }, - "layer_dims": [ - 300, - 400 - ] - }, - "action_sampler": { - "actor_layer_dims": [ - 1024, - 1024 - ], - "gmm": { - "enabled": false, - "num_modes": 5, - "min_std": 0.0001, - "std_activation": "softplus", - "low_noise_eval": true - }, - "vae": { - "enabled": true, - "latent_dim": 14, - "latent_clip": null, - "kl_weight": 1.0, - "decoder": { - "is_conditioned": true, - "reconstruction_sum_across_elements": false - }, - "prior": { - "learn": false, - "is_conditioned": false, - "use_gmm": false, - "gmm_num_modes": 10, - "gmm_learn_weights": false, - "use_categorical": false, - "categorical_dim": 10, - "categorical_gumbel_softmax_hard": false, - "categorical_init_temp": 1.0, - "categorical_temp_anneal_step": 0.001, - "categorical_min_temp": 0.3 - }, - "encoder_layer_dims": [ - 300, - 400 - ], - "decoder_layer_dims": [ - 300, - 400 - ], - "prior_layer_dims": [ - 300, - 400 - ] - }, - "freeze_encoder_epoch": -1 - }, - "actor": { - "enabled": false, - "perturbation_scale": 0.05, - "layer_dims": [ - 300, - 400 - ] - } - }, - "observation": { - "modalities": { - "obs": { - "low_dim": [ - "tool_dof_pos_scaled", - "tool_positions", - "object_relative_tool_positions", - "object_desired_positions" - ], - "rgb": [], - "depth": [], - "scan": [] - }, - "goal": { - "low_dim": [], - "rgb": [], - "depth": [], - "scan": [] - } - }, - "encoder": { - "low_dim": { - "core_class": null, - "core_kwargs": {}, - "obs_randomizer_class": null, - "obs_randomizer_kwargs": {} - }, - "rgb": { - "core_class": "VisualCore", - "core_kwargs": { - "feature_dimension": 64, - "flatten": true, - "backbone_class": "ResNet18Conv", - "backbone_kwargs": { - "pretrained": false, - "input_coord_conv": false - }, - "pool_class": "SpatialSoftmax", - "pool_kwargs": { - "num_kp": 32, - "learnable_temperature": false, - "temperature": 1.0, - "noise_std": 0.0, - "output_variance": false - } - }, - "obs_randomizer_class": null, - "obs_randomizer_kwargs": { - "crop_height": 76, - "crop_width": 76, - "num_crops": 1, - "pos_enc": false - } - }, - "depth": { - "core_class": "VisualCore", - "core_kwargs": { - "feature_dimension": 64, - "flatten": true, - "backbone_class": "ResNet18Conv", - "backbone_kwargs": { - "pretrained": false, - "input_coord_conv": false - }, - "pool_class": "SpatialSoftmax", - "pool_kwargs": { - "num_kp": 32, - "learnable_temperature": false, - "temperature": 1.0, - "noise_std": 0.0, - "output_variance": false - } - }, - "obs_randomizer_class": null, - "obs_randomizer_kwargs": { - "crop_height": 76, - "crop_width": 76, - "num_crops": 1, - "pos_enc": false - } - }, - "scan": { - "core_class": "ScanCore", - "core_kwargs": { - "feature_dimension": 64, - "flatten": true, - "pool_class": "SpatialSoftmax", - "pool_kwargs": { - "num_kp": 32, - "learnable_temperature": false, - "temperature": 1.0, - "noise_std": 0.0, - "output_variance": false - }, - "conv_activation": "relu", - "conv_kwargs": { - "out_channels": [ - 32, - 64, - 64 - ], - "kernel_size": [ - 8, - 4, - 2 - ], - "stride": [ - 4, - 2, - 1 - ] - } - }, - "obs_randomizer_class": null, - "obs_randomizer_kwargs": { - "crop_height": 76, - "crop_width": 76, - "num_crops": 1, - "pos_enc": false - } - } - } - } -} diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/agents/rsl_rl_ppo_cfg.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/agents/rsl_rl_ppo_cfg.py deleted file mode 100644 index c6cfd25840..0000000000 --- a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/agents/rsl_rl_ppo_cfg.py +++ /dev/null @@ -1,41 +0,0 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. -# All rights reserved. -# -# SPDX-License-Identifier: BSD-3-Clause - -from omni.isaac.lab.utils import configclass - -from omni.isaac.lab_tasks.utils.wrappers.rsl_rl import ( - RslRlOnPolicyRunnerCfg, - RslRlPpoActorCriticCfg, - RslRlPpoAlgorithmCfg, -) - - -@configclass -class LiftCubePPORunnerCfg(RslRlOnPolicyRunnerCfg): - num_steps_per_env = 24 - max_iterations = 1500 - save_interval = 50 - experiment_name = "franka_lift" - empirical_normalization = False - policy = RslRlPpoActorCriticCfg( - init_noise_std=1.0, - actor_hidden_dims=[256, 128, 64], - critic_hidden_dims=[256, 128, 64], - activation="elu", - ) - algorithm = RslRlPpoAlgorithmCfg( - value_loss_coef=1.0, - use_clipped_value_loss=True, - clip_param=0.2, - entropy_coef=0.006, - num_learning_epochs=5, - num_mini_batches=4, - learning_rate=1.0e-4, - schedule="adaptive", - gamma=0.98, - lam=0.95, - desired_kl=0.01, - max_grad_norm=1.0, - ) diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/agents/sb3_ppo_cfg.yaml b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/agents/sb3_ppo_cfg.yaml deleted file mode 100644 index 6d6f15781a..0000000000 --- a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/agents/sb3_ppo_cfg.yaml +++ /dev/null @@ -1,28 +0,0 @@ -# Reference: https://github.com/DLR-RM/rl-baselines3-zoo/blob/master/hyperparams/ppo.yml#L32 -seed: 42 - -# epoch * n_steps * nenvs: 500×512*8*8 -n_timesteps: 16384000 -policy: 'MlpPolicy' -n_steps: 64 -# mini batch size: num_envs * nsteps / nminibatches 2048×512÷2048 -batch_size: 192 -gae_lambda: 0.95 -gamma: 0.99 -n_epochs: 8 -ent_coef: 0.00 -vf_coef: 0.0001 -learning_rate: !!float 3e-4 -clip_range: 0.2 -policy_kwargs: "dict( - activation_fn=nn.ELU, - net_arch=dict(pi=[256, 128, 64], vf=[256, 128, 64]) - )" -target_kl: 0.01 -max_grad_norm: 1.0 - -# # Uses VecNormalize class to normalize obs -# normalize_input: True -# # Uses VecNormalize class to normalize rew -# normalize_value: True -# clip_obs: 5 diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/agents/skrl_ppo_cfg.yaml b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/agents/skrl_ppo_cfg.yaml deleted file mode 100644 index 94337d46ba..0000000000 --- a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/agents/skrl_ppo_cfg.yaml +++ /dev/null @@ -1,80 +0,0 @@ -seed: 42 - - -# Models are instantiated using skrl's model instantiator utility -# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html -models: - separate: False - policy: # see gaussian_model parameters - class: GaussianMixin - clip_actions: False - clip_log_std: True - min_log_std: -20.0 - max_log_std: 2.0 - initial_log_std: 0.0 - network: - - name: net - input: STATES - layers: [256, 128, 64] - activations: elu - output: ACTIONS - value: # see deterministic_model parameters - class: DeterministicMixin - clip_actions: False - network: - - name: net - input: STATES - layers: [256, 128, 64] - activations: elu - output: ONE - - -# Rollout memory -# https://skrl.readthedocs.io/en/latest/api/memories/random.html -memory: - class: RandomMemory - memory_size: -1 # automatically determined (same as agent:rollouts) - - -# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) -# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html -agent: - class: PPO - rollouts: 24 - learning_epochs: 8 - mini_batches: 4 - discount_factor: 0.99 - lambda: 0.95 - learning_rate: 1.0e-04 - learning_rate_scheduler: KLAdaptiveLR - learning_rate_scheduler_kwargs: - kl_threshold: 0.01 - state_preprocessor: RunningStandardScaler - state_preprocessor_kwargs: null - value_preprocessor: RunningStandardScaler - value_preprocessor_kwargs: null - random_timesteps: 0 - learning_starts: 0 - grad_norm_clip: 1.0 - ratio_clip: 0.2 - value_clip: 0.2 - clip_predicted_values: True - entropy_loss_scale: 0.001 - value_loss_scale: 2.0 - kl_threshold: 0.0 - rewards_shaper_scale: 0.01 - time_limit_bootstrap: False - # logging and checkpoint - experiment: - directory: "franka_lift" - experiment_name: "" - write_interval: auto - checkpoint_interval: auto - - -# Sequential trainer -# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html -trainer: - class: SequentialTrainer - timesteps: 36000 - environment_info: log diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/joint_pos_env_cfg.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/joint_pos_env_cfg.py deleted file mode 100644 index d9f15c58e9..0000000000 --- a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/config/franka/joint_pos_env_cfg.py +++ /dev/null @@ -1,106 +0,0 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. -# All rights reserved. -# -# SPDX-License-Identifier: BSD-3-Clause - -from omni.isaac.lab.assets import RigidObjectCfg, DeformableObjectCfg -import omni.isaac.lab.sim as sim_utils -from omni.isaac.lab.sensors import FrameTransformerCfg -from omni.isaac.lab.sensors.frame_transformer.frame_transformer_cfg import OffsetCfg -from omni.isaac.lab.sim.schemas.schemas_cfg import RigidBodyPropertiesCfg -from omni.isaac.lab.sim.spawners.from_files.from_files_cfg import UsdFileCfg -from omni.isaac.lab.utils import configclass -from omni.isaac.lab.utils.assets import ISAAC_NUCLEUS_DIR - -from omni.isaac.lab_tasks.manager_based.manipulation.lift import mdp -from omni.isaac.lab_tasks.manager_based.manipulation.lift.lift_env_cfg import LiftEnvCfg - -## -# Pre-defined configs -## -from omni.isaac.lab.markers.config import FRAME_MARKER_CFG # isort: skip -from omni.isaac.lab_assets.franka import FRANKA_PANDA_CFG # isort: skip - - -@configclass -class FrankaCubeLiftEnvCfg(LiftEnvCfg): - def __post_init__(self): - # post init of parent - super().__post_init__() - - # Set Franka as robot - self.scene.robot = FRANKA_PANDA_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") - - # Set actions for the specific robot type (franka) - self.actions.arm_action = mdp.JointPositionActionCfg( - asset_name="robot", joint_names=["panda_joint.*"], scale=0.5, use_default_offset=True - ) - self.actions.gripper_action = mdp.BinaryJointPositionActionCfg( - asset_name="robot", - joint_names=["panda_finger.*"], - open_command_expr={"panda_finger_.*": 0.04}, - close_command_expr={"panda_finger_.*": 0.0}, - ) - # Set the body name for the end effector - self.commands.object_pose.body_name = "panda_hand" - - self.scene.object = DeformableObjectCfg( - prim_path="{ENV_REGEX_NS}/Object", - spawn=sim_utils.MeshCuboidCfg( - size=(0.11, 0.11, 0.11), - deformable_props=sim_utils.DeformableBodyPropertiesCfg(rest_offset=0.0, contact_offset=0.001), - visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.5, 0.1, 0.0)), - physics_material=sim_utils.DeformableBodyMaterialCfg(poissons_ratio=0.4, youngs_modulus=1e5), - ), - init_state=DeformableObjectCfg.InitialStateCfg(pos=[0.5, 0, 0.055], rot=[1, 0, 0, 0]), - debug_vis=True, - ) - - # Set Cube as object - # self.scene.object = RigidObjectCfg( - # prim_path="{ENV_REGEX_NS}/Object", - # init_state=RigidObjectCfg.InitialStateCfg(pos=[0.5, 0, 0.055], rot=[1, 0, 0, 0]), - # spawn=UsdFileCfg( - # usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/DexCube/dex_cube_instanceable.usd", - # scale=(0.8, 0.8, 0.8), - # rigid_props=RigidBodyPropertiesCfg( - # solver_position_iteration_count=16, - # solver_velocity_iteration_count=1, - # max_angular_velocity=1000.0, - # max_linear_velocity=1000.0, - # max_depenetration_velocity=5.0, - # disable_gravity=False, - # ), - # ), - # ) - - # Listens to the required transforms - marker_cfg = FRAME_MARKER_CFG.copy() - marker_cfg.markers["frame"].scale = (0.1, 0.1, 0.1) - marker_cfg.prim_path = "/Visuals/FrameTransformer" - self.scene.ee_frame = FrameTransformerCfg( - prim_path="{ENV_REGEX_NS}/Robot/panda_link0", - debug_vis=False, - visualizer_cfg=marker_cfg, - target_frames=[ - FrameTransformerCfg.FrameCfg( - prim_path="{ENV_REGEX_NS}/Robot/panda_hand", - name="end_effector", - offset=OffsetCfg( - pos=[0.0, 0.0, 0.1034], - ), - ), - ], - ) - - -@configclass -class FrankaCubeLiftEnvCfg_PLAY(FrankaCubeLiftEnvCfg): - 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 diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/lift_env_cfg.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/lift_env_cfg.py deleted file mode 100644 index 955554108c..0000000000 --- a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/lift_env_cfg.py +++ /dev/null @@ -1,222 +0,0 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. -# All rights reserved. -# -# SPDX-License-Identifier: BSD-3-Clause - -from dataclasses import MISSING - -import omni.isaac.lab.sim as sim_utils -from omni.isaac.lab.assets import ArticulationCfg, AssetBaseCfg, DeformableObjectCfg, RigidObjectCfg -from omni.isaac.lab.envs import ManagerBasedRLEnvCfg -from omni.isaac.lab.managers import CurriculumTermCfg as CurrTerm -from omni.isaac.lab.managers import EventTermCfg as EventTerm -from omni.isaac.lab.managers import ObservationGroupCfg as ObsGroup -from omni.isaac.lab.managers import ObservationTermCfg as ObsTerm -from omni.isaac.lab.managers import RewardTermCfg as RewTerm -from omni.isaac.lab.managers import SceneEntityCfg -from omni.isaac.lab.managers import TerminationTermCfg as DoneTerm -from omni.isaac.lab.scene import InteractiveSceneCfg -from omni.isaac.lab.sensors.frame_transformer.frame_transformer_cfg import FrameTransformerCfg -from omni.isaac.lab.sim.spawners.from_files.from_files_cfg import GroundPlaneCfg, UsdFileCfg -from omni.isaac.lab.utils import configclass -from omni.isaac.lab.utils.assets import ISAAC_NUCLEUS_DIR - -from . import mdp - -## -# Scene definition -## - - -@configclass -class ObjectTableSceneCfg(InteractiveSceneCfg): - """Configuration for the lift scene with a robot and a object. - This is the abstract base implementation, the exact scene is defined in the derived classes - which need to set the target object, robot and end-effector frames - """ - - # robots: will be populated by agent env cfg - robot: ArticulationCfg = MISSING - # end-effector sensor: will be populated by agent env cfg - ee_frame: FrameTransformerCfg = MISSING - # target object: will be populated by agent env cfg - object: RigidObjectCfg | DeformableObjectCfg = MISSING - - # Table - table = AssetBaseCfg( - prim_path="{ENV_REGEX_NS}/Table", - init_state=AssetBaseCfg.InitialStateCfg(pos=[0.5, 0, 0], rot=[0.707, 0, 0, 0.707]), - spawn=UsdFileCfg(usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/SeattleLabTable/table_instanceable.usd"), - ) - - # plane - plane = AssetBaseCfg( - prim_path="/World/GroundPlane", - init_state=AssetBaseCfg.InitialStateCfg(pos=[0, 0, -1.05]), - spawn=GroundPlaneCfg(), - ) - - # lights - light = AssetBaseCfg( - prim_path="/World/light", - spawn=sim_utils.DomeLightCfg(color=(0.75, 0.75, 0.75), intensity=3000.0), - ) - - -## -# MDP settings -## - - -@configclass -class CommandsCfg: - """Command terms for the MDP.""" - - object_pose = mdp.UniformPoseCommandCfg( - asset_name="robot", - body_name=MISSING, # will be set by agent env cfg - resampling_time_range=(5.0, 5.0), - debug_vis=True, - ranges=mdp.UniformPoseCommandCfg.Ranges( - pos_x=(0.4, 0.6), pos_y=(-0.25, 0.25), pos_z=(0.25, 0.5), roll=(0.0, 0.0), pitch=(0.0, 0.0), yaw=(0.0, 0.0) - ), - ) - - -@configclass -class ActionsCfg: - """Action specifications for the MDP.""" - - # will be set by agent env cfg - arm_action: mdp.JointPositionActionCfg | mdp.DifferentialInverseKinematicsActionCfg = MISSING - gripper_action: mdp.BinaryJointPositionActionCfg = MISSING - - -@configclass -class ObservationsCfg: - """Observation specifications for the MDP.""" - - @configclass - class PolicyCfg(ObsGroup): - """Observations for policy group.""" - - joint_pos = ObsTerm(func=mdp.joint_pos_rel) - joint_vel = ObsTerm(func=mdp.joint_vel_rel) - object_position = ObsTerm(func=mdp.object_position_in_robot_root_frame) - target_object_position = ObsTerm(func=mdp.generated_commands, params={"command_name": "object_pose"}) - 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_all = EventTerm(func=mdp.reset_scene_to_default, mode="reset") - - reset_object_position = EventTerm( - func=mdp.reset_root_state_uniform, - mode="reset", - params={ - "pose_range": {"x": (-0.1, 0.1), "y": (-0.25, 0.25), "z": (0.0, 0.0)}, - "velocity_range": {}, - "asset_cfg": SceneEntityCfg("object", body_names="Object"), - }, - ) - - -@configclass -class RewardsCfg: - """Reward terms for the MDP.""" - - reaching_object = RewTerm(func=mdp.object_ee_distance, params={"std": 0.1}, weight=1.0) - - lifting_object = RewTerm(func=mdp.object_is_lifted, params={"minimal_height": 0.04}, weight=15.0) - - object_goal_tracking = RewTerm( - func=mdp.object_goal_distance, - params={"std": 0.3, "minimal_height": 0.04, "command_name": "object_pose"}, - weight=16.0, - ) - - object_goal_tracking_fine_grained = RewTerm( - func=mdp.object_goal_distance, - params={"std": 0.05, "minimal_height": 0.04, "command_name": "object_pose"}, - weight=5.0, - ) - - # action penalty - action_rate = RewTerm(func=mdp.action_rate_l2, weight=-1e-4) - - joint_vel = RewTerm( - func=mdp.joint_vel_l2, - weight=-1e-4, - params={"asset_cfg": SceneEntityCfg("robot")}, - ) - - -@configclass -class TerminationsCfg: - """Termination terms for the MDP.""" - - time_out = DoneTerm(func=mdp.time_out, time_out=True) - - object_dropping = DoneTerm( - func=mdp.root_height_below_minimum, params={"minimum_height": -0.05, "asset_cfg": SceneEntityCfg("object")} - ) - - -@configclass -class CurriculumCfg: - """Curriculum terms for the MDP.""" - - action_rate = CurrTerm( - func=mdp.modify_reward_weight, params={"term_name": "action_rate", "weight": -1e-1, "num_steps": 10000} - ) - - joint_vel = CurrTerm( - func=mdp.modify_reward_weight, params={"term_name": "joint_vel", "weight": -1e-1, "num_steps": 10000} - ) - - -## -# Environment configuration -## - - -@configclass -class LiftEnvCfg(ManagerBasedRLEnvCfg): - """Configuration for the lifting environment.""" - - # Scene settings - scene: ObjectTableSceneCfg = ObjectTableSceneCfg(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.episode_length_s = 5.0 - # simulation settings - self.sim.dt = 0.01 # 100Hz - self.sim.render_interval = self.decimation - - self.sim.physx.bounce_threshold_velocity = 0.2 - self.sim.physx.bounce_threshold_velocity = 0.01 - self.sim.physx.gpu_found_lost_aggregate_pairs_capacity = 1024 * 1024 * 4 - self.sim.physx.gpu_total_aggregate_pairs_capacity = 16 * 1024 - self.sim.physx.friction_correlation_distance = 0.00625 diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/mdp/__init__.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/mdp/__init__.py deleted file mode 100644 index e8b879f89b..0000000000 --- a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/mdp/__init__.py +++ /dev/null @@ -1,12 +0,0 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. -# All rights reserved. -# -# SPDX-License-Identifier: BSD-3-Clause - -"""This sub-module contains the functions that are specific to the lift environments.""" - -from omni.isaac.lab.envs.mdp import * # noqa: F401, F403 - -from .observations import * # noqa: F401, F403 -from .rewards import * # noqa: F401, F403 -from .terminations import * # noqa: F401, F403 diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/mdp/observations.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/mdp/observations.py deleted file mode 100644 index 605ff9f8c2..0000000000 --- a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/mdp/observations.py +++ /dev/null @@ -1,31 +0,0 @@ -# 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 omni.isaac.lab.assets import RigidObject -from omni.isaac.lab.managers import SceneEntityCfg -from omni.isaac.lab.utils.math import subtract_frame_transforms - -if TYPE_CHECKING: - from omni.isaac.lab.envs import ManagerBasedRLEnv - - -def object_position_in_robot_root_frame( - env: ManagerBasedRLEnv, - robot_cfg: SceneEntityCfg = SceneEntityCfg("robot"), - object_cfg: SceneEntityCfg = SceneEntityCfg("object"), -) -> torch.Tensor: - """The position of the object in the robot's root frame.""" - robot: RigidObject = env.scene[robot_cfg.name] - object: RigidObject = env.scene[object_cfg.name] - object_pos_w = object.data.root_pos_w[:, :3] - object_pos_b, _ = subtract_frame_transforms( - robot.data.root_link_state_w[:, :3], robot.data.root_link_state_w[:, 3:7], object_pos_w - ) - return object_pos_b diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/mdp/rewards.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/mdp/rewards.py deleted file mode 100644 index ba9e4115b7..0000000000 --- a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/mdp/rewards.py +++ /dev/null @@ -1,69 +0,0 @@ -# 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 omni.isaac.lab.assets import RigidObject -from omni.isaac.lab.managers import SceneEntityCfg -from omni.isaac.lab.sensors import FrameTransformer -from omni.isaac.lab.utils.math import combine_frame_transforms - -if TYPE_CHECKING: - from omni.isaac.lab.envs import ManagerBasedRLEnv - - -def object_is_lifted( - env: ManagerBasedRLEnv, minimal_height: float, object_cfg: SceneEntityCfg = SceneEntityCfg("object") -) -> torch.Tensor: - """Reward the agent for lifting the object above the minimal height.""" - object: RigidObject = env.scene[object_cfg.name] - return torch.where(object.data.root_pos_w[:, 2] > minimal_height, 1.0, 0.0) - - -def object_ee_distance( - env: ManagerBasedRLEnv, - std: float, - object_cfg: SceneEntityCfg = SceneEntityCfg("object"), - ee_frame_cfg: SceneEntityCfg = SceneEntityCfg("ee_frame"), -) -> torch.Tensor: - """Reward the agent for reaching the object using tanh-kernel.""" - # extract the used quantities (to enable type-hinting) - object: RigidObject = env.scene[object_cfg.name] - ee_frame: FrameTransformer = env.scene[ee_frame_cfg.name] - # Target object position: (num_envs, 3) - cube_pos_w = object.data.root_pos_w - # End-effector position: (num_envs, 3) - ee_w = ee_frame.data.target_pos_w[..., 0, :] - # Distance of the end-effector to the object: (num_envs,) - object_ee_distance = torch.norm(cube_pos_w - ee_w, dim=1) - - return 1 - torch.tanh(object_ee_distance / std) - - -def object_goal_distance( - env: ManagerBasedRLEnv, - std: float, - minimal_height: float, - command_name: str, - robot_cfg: SceneEntityCfg = SceneEntityCfg("robot"), - object_cfg: SceneEntityCfg = SceneEntityCfg("object"), -) -> torch.Tensor: - """Reward the agent for tracking the goal pose using tanh-kernel.""" - # extract the used quantities (to enable type-hinting) - robot: RigidObject = env.scene[robot_cfg.name] - object: RigidObject = env.scene[object_cfg.name] - command = env.command_manager.get_command(command_name) - # compute the desired position in the world frame - des_pos_b = command[:, :3] - des_pos_w, _ = combine_frame_transforms( - robot.data.root_link_state_w[:, :3], robot.data.root_link_state_w[:, 3:7], des_pos_b - ) - # distance of the end-effector to the object: (num_envs,) - distance = torch.norm(des_pos_w - object.data.root_pos_w[:, :3], dim=1) - # rewarded if the object is lifted above the threshold - return (object.data.root_pos_w[:, 2] > minimal_height) * (1 - torch.tanh(distance / std)) diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/mdp/terminations.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/mdp/terminations.py deleted file mode 100644 index de2bae84fd..0000000000 --- a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/liftsquishy/mdp/terminations.py +++ /dev/null @@ -1,55 +0,0 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. -# All rights reserved. -# -# SPDX-License-Identifier: BSD-3-Clause - -"""Common functions that can be used to activate certain terminations for the lift task. - -The functions can be passed to the :class:`omni.isaac.lab.managers.TerminationTermCfg` object to enable -the termination introduced by the function. -""" - -from __future__ import annotations - -import torch -from typing import TYPE_CHECKING - -from omni.isaac.lab.assets import RigidObject -from omni.isaac.lab.managers import SceneEntityCfg -from omni.isaac.lab.utils.math import combine_frame_transforms - -if TYPE_CHECKING: - from omni.isaac.lab.envs import ManagerBasedRLEnv - - -def object_reached_goal( - env: ManagerBasedRLEnv, - command_name: str = "object_pose", - threshold: float = 0.02, - robot_cfg: SceneEntityCfg = SceneEntityCfg("robot"), - object_cfg: SceneEntityCfg = SceneEntityCfg("object"), -) -> torch.Tensor: - """Termination condition for the object reaching the goal position. - - Args: - env: The environment. - command_name: The name of the command that is used to control the object. - threshold: The threshold for the object to reach the goal position. Defaults to 0.02. - robot_cfg: The robot configuration. Defaults to SceneEntityCfg("robot"). - object_cfg: The object configuration. Defaults to SceneEntityCfg("object"). - - """ - # extract the used quantities (to enable type-hinting) - robot: RigidObject = env.scene[robot_cfg.name] - object: RigidObject = env.scene[object_cfg.name] - command = env.command_manager.get_command(command_name) - # compute the desired position in the world frame - des_pos_b = command[:, :3] - des_pos_w, _ = combine_frame_transforms( - robot.data.root_link_state_w[:, :3], robot.data.root_link_state_w[:, 3:7], des_pos_b - ) - # distance of the end-effector to the object: (num_envs,) - distance = torch.norm(des_pos_w - object.data.root_pos_w[:, :3], dim=1) - - # rewarded if the object is lifted above the threshold - return distance < threshold From 49e743faf4303cf6a26cadda637f5247a0011cd9 Mon Sep 17 00:00:00 2001 From: gursi26 Date: Wed, 15 Jan 2025 00:02:30 -0500 Subject: [PATCH 3/6] added robot, cube and container --- .../deformable_cube/deformable_cube_env.py | 194 ++++++++++++++++-- .../deformable_cube_env_cfg.py | 68 ------ .../lab_tasks/direct/deformable_cube/main.py | 18 ++ 3 files changed, 199 insertions(+), 81 deletions(-) delete mode 100644 source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/deformable_cube/deformable_cube_env_cfg.py create mode 100644 source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/deformable_cube/main.py diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/deformable_cube/deformable_cube_env.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/deformable_cube/deformable_cube_env.py index f04802c0bf..dd81b18ebc 100644 --- a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/deformable_cube/deformable_cube_env.py +++ b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/deformable_cube/deformable_cube_env.py @@ -1,32 +1,174 @@ from __future__ import annotations - import math import torch -import torch.nn.functional as F +from dataclasses import MISSING from collections.abc import Sequence - import omni.isaac.lab.sim as sim_utils -from omni.isaac.lab.assets import RigidObjectCfg, RigidObject, Articulation, DeformableObject +import omni.isaac.lab.utils.math as math_utils +from omni.isaac.lab.actuators.actuator_cfg import ImplicitActuatorCfg +from omni.isaac.lab.assets import Articulation, DeformableObject, ArticulationCfg, DeformableObjectCfg, RigidObjectCfg, RigidObject from omni.isaac.lab.envs import DirectRLEnv, DirectRLEnvCfg from omni.isaac.lab.scene import InteractiveSceneCfg from omni.isaac.lab.sim import SimulationCfg -from omni.isaac.lab.sim.spawners.from_files import GroundPlaneCfg, spawn_ground_plane +from omni.isaac.lab.sim.spawners.from_files import GroundPlaneCfg, spawn_ground_plane, UsdFileCfg from omni.isaac.lab.utils import configclass -from omni.isaac.lab.utils.math import quat_from_euler_xyz, euler_xyz_from_quat -from .deformabe_cube_env_cfg import DeformableCubeEnvCfg + +from omni.isaac.lab.utils.assets import ISAAC_NUCLEUS_DIR +from omni.isaac.lab_assets.franka import FRANKA_PANDA_CFG + +CUBE_SIZE = 0.1 +CUBE_X_MIN, CUBE_X_MAX = 0.1, 1.0 +CUBE_Y_MIN, CUBE_Y_MAX = -0.5, 0.5 + +# TODO: Edit GPU settings for softbody contact buffer size +# TODO: mess with deformable object settings +# TODO: Add container + +def get_robot() -> ArticulationCfg: + return ArticulationCfg( + prim_path="/World/envs/env_.*/Robot", + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/Franka/franka_instanceable.usd", + activate_contact_sensors=False, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=False, + max_depenetration_velocity=5.0, + ), + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, solver_position_iteration_count=12, solver_velocity_iteration_count=1 + ), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + "panda_joint1": 1.157, + "panda_joint2": -1.066, + "panda_joint3": -0.155, + "panda_joint4": -2.239, + "panda_joint5": -1.841, + "panda_joint6": 1.003, + "panda_joint7": 0.469, + "panda_finger_joint.*": 0.035, + } + ), + actuators={ + "panda_shoulder": ImplicitActuatorCfg( + joint_names_expr=["panda_joint[1-4]"], + effort_limit=87.0, + velocity_limit=2.175, + stiffness=80.0, + damping=4.0, + ), + "panda_forearm": ImplicitActuatorCfg( + joint_names_expr=["panda_joint[5-7]"], + effort_limit=12.0, + velocity_limit=2.61, + stiffness=80.0, + damping=4.0, + ), + "panda_hand": ImplicitActuatorCfg( + joint_names_expr=["panda_finger_joint.*"], + effort_limit=200.0, + velocity_limit=0.2, + stiffness=2e3, + damping=1e2, + ), + }, + ) + +def get_object() -> DeformableObjectCfg: + return DeformableObjectCfg( + prim_path="/World/envs/env_.*/Cube", + spawn=sim_utils.MeshCuboidCfg( + size=(CUBE_SIZE, CUBE_SIZE, CUBE_SIZE), + deformable_props=sim_utils.DeformableBodyPropertiesCfg( + rest_offset=0.0, + contact_offset=0.001 + ), + visual_material=sim_utils.PreviewSurfaceCfg( + diffuse_color=(0.5, 0.1, 0.0) + ), + physics_material=sim_utils.DeformableBodyMaterialCfg( + poissons_ratio=0.4, + youngs_modulus=1e5 + ), + ), + init_state=DeformableObjectCfg.InitialStateCfg(pos=(0.0, 0.0, CUBE_SIZE / 1.9)), + debug_vis=True, + ) + +def get_container() -> RigidObjectCfg: + return RigidObjectCfg( + prim_path="/World/envs/env_.*/Container", + spawn=sim_utils.UsdFileCfg( + usd_path=f"./Container_B04_40x30x12cm_PR_V_NVD_01.usd", + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=False, + solver_position_iteration_count=8, + solver_velocity_iteration_count=1, + max_depenetration_velocity=0.01 + ), + mass_props=sim_utils.MassPropertiesCfg(mass=1.0), + collision_props=sim_utils.CollisionPropertiesCfg( + collision_enabled=True, + contact_offset=0.02, + rest_offset=0.001 + ), + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 0.0, 1.0), metallic=0.2), + scale=(0.02, 0.02, 0.02), + ), + init_state=RigidObjectCfg.InitialStateCfg(pos=(0.5, 0.5, 0.5)), + ) + +@configclass +class DeformableCubeEnvCfg(DirectRLEnvCfg): + num_envs = 5 + env_spacing = 3.0 + dt = 1 / 120 + observation_space = 15 + action_space = 9 + state_space = 0 + action_scale = 7.5 + + # env + decimation = 1 + episode_length_s = 5.0 + + # simulation + sim: SimulationCfg = SimulationCfg(dt=dt, render_interval=decimation) + scene: InteractiveSceneCfg = InteractiveSceneCfg( + num_envs=num_envs, + env_spacing=env_spacing, + replicate_physics=False, + ) + + # entities + robot_cfg: ArticulationCfg = get_robot() + object_cfg: DeformableObjectCfg = get_object() + container_cfg: RigidObjectCfg = get_container() + class DeformableCubeEnv(DirectRLEnv): cfg: DeformableCubeEnvCfg def __init__(self, cfg: DeformableCubeEnvCfg, render_mode: str | None = None, **kwargs): super().__init__(cfg, render_mode, **kwargs) - pass + + self.dt = self.cfg.sim.dt * self.cfg.decimation + + self.robot_dof_lower_limits = self.robot.data.soft_joint_pos_limits[0, :, 0].to(device=self.device) + self.robot_dof_upper_limits = self.robot.data.soft_joint_pos_limits[0, :, 1].to(device=self.device) + + self.robot_dof_speed_scales = torch.ones_like(self.robot_dof_lower_limits) + self.robot_dof_speed_scales[self.robot.find_joints("panda_finger_joint1")[0]] = 0.1 + self.robot_dof_speed_scales[self.robot.find_joints("panda_finger_joint2")[0]] = 0.1 + self.robot_dof_targets = torch.zeros((self.num_envs, self.robot.num_joints), device=self.device) def _setup_scene(self): self.robot = Articulation(self.cfg.robot_cfg) self.object = DeformableObject(self.cfg.object_cfg) - # TODO: spawn container + self.container = RigidObject(self.cfg.container_cfg) + self.scene.articulations["robot"] = self.robot spawn_ground_plane(prim_path="/World/ground", cfg=GroundPlaneCfg()) @@ -39,10 +181,12 @@ def _setup_scene(self): light_cfg.func("/World/Light", light_cfg) def _pre_physics_step(self, actions: torch.Tensor) -> None: - pass + self.actions = actions.clone().clamp(-1.0, 1.0) + targets = self.robot_dof_targets + self.robot_dof_speed_scales * self.dt * self.actions * self.cfg.action_scale + self.robot_dof_targets[:] = torch.clamp(targets, self.robot_dof_lower_limits, self.robot_dof_upper_limits) def _apply_action(self) -> None: - pass + self.robot.set_joint_position_target(self.robot_dof_targets) def _get_observations(self) -> dict: pass @@ -51,7 +195,31 @@ def _get_rewards(self) -> torch.Tensor: pass def _get_dones(self) -> tuple[torch.Tensor, torch.Tensor]: - pass + out_of_bounds = torch.full((self.cfg.num_envs,), fill_value=False, dtype=bool) + time_out = self.episode_length_buf >= self.max_episode_length - 1 + return out_of_bounds, time_out def _reset_idx(self, env_ids: Sequence[int] | None): - pass \ No newline at end of file + print(f"[INFO]: Resetting envs {env_ids}") + self.episode_length_buf[env_ids] = 0.0 + + # reset objects + x_offset, y_offset = torch.rand(2, len(env_ids), 1, device=self.device) + x_offset = x_offset * (CUBE_X_MAX - CUBE_X_MIN) + CUBE_X_MIN + y_offset = y_offset * (CUBE_Y_MAX - CUBE_Y_MIN) + CUBE_Y_MIN + pos_t = torch.cat([x_offset, y_offset, torch.zeros_like(x_offset)], dim=-1) + + quat_t = torch.zeros(len(env_ids), 4, device=self.device) + quat_t[:, 0], quat_t[:, -1] = torch.rand(2, len(env_ids), device=self.device) * 2 - 1.0 + quat_t = quat_t / quat_t.norm(dim=1, keepdim=True) + + nodal_state = self.object.data.default_nodal_state_w[env_ids, :, :] + nodal_state[..., :3] = self.object.transform_nodal_pos(nodal_state[..., :3], pos=pos_t, quat=quat_t) + nodal_state[..., 3:] = 0.0 + self.object.write_nodal_state_to_sim(nodal_state, env_ids) + + # reset robots + joint_pos = self.robot.data.default_joint_pos[env_ids] + joint_vel = torch.zeros_like(joint_pos) + # self.robot.set_joint_position_target(joint_pos, env_ids=env_ids) + self.robot.write_joint_state_to_sim(position=joint_pos, velocity=joint_vel, env_ids=env_ids) \ No newline at end of file diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/deformable_cube/deformable_cube_env_cfg.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/deformable_cube/deformable_cube_env_cfg.py deleted file mode 100644 index aedc0d5d4b..0000000000 --- a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/deformable_cube/deformable_cube_env_cfg.py +++ /dev/null @@ -1,68 +0,0 @@ -from __future__ import annotations - -from dataclasses import MISSING - -import omni.isaac.lab.sim as sim_utils -from omni.isaac.lab.assets import AssetBaseCfg, ArticulationCfg, DeformableObjectCfg -from omni.isaac.lab.envs import DirectRLEnv, DirectRLEnvCfg -from omni.isaac.lab.scene import InteractiveSceneCfg -from omni.isaac.lab.sim import SimulationCfg -from omni.isaac.lab.sim.spawners.from_files import GroundPlaneCfg, spawn_ground_plane, UsdFileCfg -from omni.isaac.lab.utils import configclass -from omni.isaac.lab.utils.math import quat_from_euler_xyz, euler_xyz_from_quat -from omni.isaac.lab.utils.assets import ISAAC_NUCLEUS_DIR - -from omni.isaac.lab_assets.franka import FRANKA_PANDA_CFG - -@configclass -class DeformableCubeEnvCfg(DirectRLEnvCfg): - # required params - num_envs = 256 - env_spacing = 4.0 - dt = 1 / 120 - observation_space = 15 - action_space = 3 - state_space = 0 - - # env params - decimation = 1 - episode_length_s = 15.0 - - # simulation - sim: SimulationCfg = SimulationCfg( - dt=dt, - render_interval=decimation - ) - - # robot - robot_cfg: ArticulationCfg = FRANKA_PANDA_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") - - # objects - object_cfg: DeformableObjectCfg = DeformableObjectCfg( - prim_path="{ENV_REGEX_NS}/Cube", - spawn=sim_utils.MeshCuboidCfg( - size=(0.2, 0.2, 0.2), - deformable_props=sim_utils.DeformableBodyPropertiesCfg( - rest_offset=0.0, - contact_offset=0.001 - ), - visual_material=sim_utils.PreviewSurfaceCfg( - diffuse_color=(0.5, 0.1, 0.0) - ), - physics_material=sim_utils.DeformableBodyMaterialCfg( - poissons_ratio=0.4, - youngs_modulus=1e5 - ), - ), - init_state=DeformableObjectCfg.InitialStateCfg(), - debug_vis=True, - ) - - container_cfg: AssetBaseCfg = MISSING #TODO: fill this out - - # scene - scene: InteractiveSceneCfg = InteractiveSceneCfg( - num_envs=num_envs, - env_spacing=env_spacing, - replicate_physics=True, - ) \ No newline at end of file diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/deformable_cube/main.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/deformable_cube/main.py new file mode 100644 index 0000000000..2eea18194e --- /dev/null +++ b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/deformable_cube/main.py @@ -0,0 +1,18 @@ +import argparse +import torch +from omni.isaac.lab.app import AppLauncher + +parser = argparse.ArgumentParser() +AppLauncher.add_app_launcher_args(parser) +args_cli = parser.parse_args() +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +from deformable_cube_env import DeformableCubeEnv, DeformableCubeEnvCfg + +if __name__ == "__main__": + env_cfg = DeformableCubeEnvCfg() + env = DeformableCubeEnv(env_cfg) + env.reset() + while simulation_app.is_running(): + env.step(torch.randn(env_cfg.num_envs, env_cfg.action_space, device="cuda")) From 3d7d097b547ab4505cfeebd3fdbdf9e548340a02 Mon Sep 17 00:00:00 2001 From: gursi26 Date: Wed, 15 Jan 2025 00:21:26 -0500 Subject: [PATCH 4/6] fixed container collision and gravity, added resets --- .../deformable_cube/deformable_cube_env.py | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/deformable_cube/deformable_cube_env.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/deformable_cube/deformable_cube_env.py index dd81b18ebc..e13269d6d7 100644 --- a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/deformable_cube/deformable_cube_env.py +++ b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/deformable_cube/deformable_cube_env.py @@ -101,28 +101,23 @@ def get_container() -> RigidObjectCfg: return RigidObjectCfg( prim_path="/World/envs/env_.*/Container", spawn=sim_utils.UsdFileCfg( - usd_path=f"./Container_B04_40x30x12cm_PR_V_NVD_01.usd", + usd_path=f"./Container_B04_01.usd", rigid_props=sim_utils.RigidBodyPropertiesCfg( disable_gravity=False, - solver_position_iteration_count=8, - solver_velocity_iteration_count=1, - max_depenetration_velocity=0.01 ), - mass_props=sim_utils.MassPropertiesCfg(mass=1.0), + mass_props=sim_utils.MassPropertiesCfg(density=5000.0), collision_props=sim_utils.CollisionPropertiesCfg( collision_enabled=True, - contact_offset=0.02, - rest_offset=0.001 ), - visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 0.0, 1.0), metallic=0.2), + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 1.0, 1.0), metallic=0.2), scale=(0.02, 0.02, 0.02), ), - init_state=RigidObjectCfg.InitialStateCfg(pos=(0.5, 0.5, 0.5)), + init_state=RigidObjectCfg.InitialStateCfg(pos=(0.5, 1.0, 0.1)), ) @configclass class DeformableCubeEnvCfg(DirectRLEnvCfg): - num_envs = 5 + num_envs = 32 env_spacing = 3.0 dt = 1 / 120 observation_space = 15 @@ -222,4 +217,9 @@ def _reset_idx(self, env_ids: Sequence[int] | None): joint_pos = self.robot.data.default_joint_pos[env_ids] joint_vel = torch.zeros_like(joint_pos) # self.robot.set_joint_position_target(joint_pos, env_ids=env_ids) - self.robot.write_joint_state_to_sim(position=joint_pos, velocity=joint_vel, env_ids=env_ids) \ No newline at end of file + self.robot.write_joint_state_to_sim(position=joint_pos, velocity=joint_vel, env_ids=env_ids) + + # reset containers + container_pose = self.container.data.default_root_state[env_ids, :] + container_pose[:, :3] += self.scene.env_origins + self.container.write_root_state_to_sim(container_pose, env_ids) \ No newline at end of file From 44715708c6a54527e5d4ded80e027cea52e5d840 Mon Sep 17 00:00:00 2001 From: gursi26 Date: Wed, 15 Jan 2025 14:30:20 -0500 Subject: [PATCH 5/6] fixed container, added table --- .../deformable_cube/deformable_cube_env.py | 44 ++++++++++++++----- 1 file changed, 34 insertions(+), 10 deletions(-) diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/deformable_cube/deformable_cube_env.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/deformable_cube/deformable_cube_env.py index e13269d6d7..aad3e73a30 100644 --- a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/deformable_cube/deformable_cube_env.py +++ b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/deformable_cube/deformable_cube_env.py @@ -23,7 +23,6 @@ # TODO: Edit GPU settings for softbody contact buffer size # TODO: mess with deformable object settings -# TODO: Add container def get_robot() -> ArticulationCfg: return ArticulationCfg( @@ -49,7 +48,9 @@ def get_robot() -> ArticulationCfg: "panda_joint6": 1.003, "panda_joint7": 0.469, "panda_finger_joint.*": 0.035, - } + }, + pos=(-0.05, 0.0, 1.0), + rot=(1.0, 0.0, 0.0, 0.0) ), actuators={ "panda_shoulder": ImplicitActuatorCfg( @@ -93,7 +94,7 @@ def get_object() -> DeformableObjectCfg: youngs_modulus=1e5 ), ), - init_state=DeformableObjectCfg.InitialStateCfg(pos=(0.0, 0.0, CUBE_SIZE / 1.9)), + init_state=DeformableObjectCfg.InitialStateCfg(pos=(0.0, 0.0, 1.0 + CUBE_SIZE / 1.9)), debug_vis=True, ) @@ -101,7 +102,7 @@ def get_container() -> RigidObjectCfg: return RigidObjectCfg( prim_path="/World/envs/env_.*/Container", spawn=sim_utils.UsdFileCfg( - usd_path=f"./Container_B04_01.usd", + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/KLT_Bin/small_KLT_visual_collision.usd", rigid_props=sim_utils.RigidBodyPropertiesCfg( disable_gravity=False, ), @@ -109,12 +110,33 @@ def get_container() -> RigidObjectCfg: collision_props=sim_utils.CollisionPropertiesCfg( collision_enabled=True, ), - visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 1.0, 1.0), metallic=0.2), - scale=(0.02, 0.02, 0.02), + scale=(2.4, 2.5, 1.2), + ), + init_state=RigidObjectCfg.InitialStateCfg( + pos=(0.5, 0.75, 1.02), + rot=(math.sqrt(2)/2, 0.0, 0.0, math.sqrt(2)/2) + ), + ) + +def get_table() -> RigidObjectCfg: + return RigidObjectCfg( + prim_path="/World/envs/env_.*/Table", + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/SeattleLabTable/table_instanceable.usd", + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=False, + ), + collision_props=sim_utils.CollisionPropertiesCfg( + collision_enabled=True, + ), + ), + init_state=RigidObjectCfg.InitialStateCfg( + pos=(0.5, 0.0, 1.0), + rot=(0.707, 0.0, 0.0, 0.707) ), - init_state=RigidObjectCfg.InitialStateCfg(pos=(0.5, 1.0, 0.1)), ) + @configclass class DeformableCubeEnvCfg(DirectRLEnvCfg): num_envs = 32 @@ -138,6 +160,7 @@ class DeformableCubeEnvCfg(DirectRLEnvCfg): ) # entities + table_cfg: RigidObjectCfg = get_table() robot_cfg: ArticulationCfg = get_robot() object_cfg: DeformableObjectCfg = get_object() container_cfg: RigidObjectCfg = get_container() @@ -163,6 +186,7 @@ def _setup_scene(self): self.robot = Articulation(self.cfg.robot_cfg) self.object = DeformableObject(self.cfg.object_cfg) self.container = RigidObject(self.cfg.container_cfg) + self.table = RigidObject(self.cfg.table_cfg) self.scene.articulations["robot"] = self.robot spawn_ground_plane(prim_path="/World/ground", cfg=GroundPlaneCfg()) @@ -220,6 +244,6 @@ def _reset_idx(self, env_ids: Sequence[int] | None): self.robot.write_joint_state_to_sim(position=joint_pos, velocity=joint_vel, env_ids=env_ids) # reset containers - container_pose = self.container.data.default_root_state[env_ids, :] - container_pose[:, :3] += self.scene.env_origins - self.container.write_root_state_to_sim(container_pose, env_ids) \ No newline at end of file + # container_pose = self.container.data.default_root_state[env_ids, :] + # container_pose[:, :3] += self.scene.env_origins + # self.container.write_root_state_to_sim(container_pose, env_ids) \ No newline at end of file From 1411bc42f41d9367f6a08e507d05e01f4d430ace Mon Sep 17 00:00:00 2001 From: gursi26 Date: Wed, 15 Jan 2025 18:42:03 -0500 Subject: [PATCH 6/6] added markers for object spawn region --- .../deformable_cube/deformable_cube_env.py | 58 ++++++++++++++++++- 1 file changed, 56 insertions(+), 2 deletions(-) diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/deformable_cube/deformable_cube_env.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/deformable_cube/deformable_cube_env.py index aad3e73a30..4cc4da07e4 100644 --- a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/deformable_cube/deformable_cube_env.py +++ b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/direct/deformable_cube/deformable_cube_env.py @@ -7,6 +7,7 @@ import omni.isaac.lab.sim as sim_utils import omni.isaac.lab.utils.math as math_utils from omni.isaac.lab.actuators.actuator_cfg import ImplicitActuatorCfg +from omni.isaac.lab.markers import VisualizationMarkersCfg, VisualizationMarkers from omni.isaac.lab.assets import Articulation, DeformableObject, ArticulationCfg, DeformableObjectCfg, RigidObjectCfg, RigidObject from omni.isaac.lab.envs import DirectRLEnv, DirectRLEnvCfg from omni.isaac.lab.scene import InteractiveSceneCfg @@ -18,11 +19,13 @@ from omni.isaac.lab_assets.franka import FRANKA_PANDA_CFG CUBE_SIZE = 0.1 -CUBE_X_MIN, CUBE_X_MAX = 0.1, 1.0 -CUBE_Y_MIN, CUBE_Y_MAX = -0.5, 0.5 +CUBE_X_MIN, CUBE_X_MAX = 0.0, 1.0 +CUBE_Y_MIN, CUBE_Y_MAX = -0.45, 0.45 # TODO: Edit GPU settings for softbody contact buffer size # TODO: mess with deformable object settings +# TODO: Make sure cube spawns on table +# TODO: Add some way to detect if cube is in container def get_robot() -> ArticulationCfg: return ArticulationCfg( @@ -165,6 +168,55 @@ class DeformableCubeEnvCfg(DirectRLEnvCfg): object_cfg: DeformableObjectCfg = get_object() container_cfg: RigidObjectCfg = get_container() + # markers + table_markers_cfg: VisualizationMarkersCfg = VisualizationMarkersCfg( + prim_path="/Visuals/TableMarkers", + markers={ + "table_bottom_left": sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/UIElements/frame_prim.usd", + scale=(0.15, 0.15, 0.15), + ), + "table_bottom_right": sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/UIElements/frame_prim.usd", + scale=(0.15, 0.15, 0.15), + ), + "table_top_left": sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/UIElements/frame_prim.usd", + scale=(0.15, 0.15, 0.15), + ), + "table_top_right": sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/UIElements/frame_prim.usd", + scale=(0.15, 0.15, 0.15), + ), + } + ) + +def draw_markers(markers: VisualizationMarkers, origins: torch.Tensor): + N_envs = origins.shape[0] + + # table corner markers + bottom_left_T = torch.tensor([0.0, 0.45, 1.0], device=origins.device).reshape(1, -1) + bottom_right_T = torch.tensor([0.0, -0.45, 1.0], device=origins.device).reshape(1, -1) + top_left_T = torch.tensor([1.0, 0.45, 1.0], device=origins.device).reshape(1, -1) + top_right_T = torch.tensor([1.0, -0.45, 1.0], device=origins.device).reshape(1, -1) + + bottom_left, bottom_right, top_left, top_right = origins.clone().repeat(4, 1).reshape(4, -1, 3).unbind(0) + bottom_left += bottom_left_T + bottom_right += bottom_right_T + top_left += top_left_T + top_right += top_right_T + + r2b2 = math.sqrt(2) / 2 + bottom_left_quat = torch.tensor([-r2b2, 0.0, 0.0, r2b2], device=origins.device).reshape(1, -1).repeat(N_envs, 1) + bottom_right_quat = torch.tensor([1.0, 0.0, 0.0, 0.0], device=origins.device).reshape(1, -1).repeat(N_envs, 1) + top_left_quat = torch.tensor([0.0, 0.0, 0.0, 1.0], device=origins.device).reshape(1, -1).repeat(N_envs, 1) + top_right_quat = torch.tensor([r2b2, 0.0, 0.0, r2b2], device=origins.device).reshape(1, -1).repeat(N_envs, 1) + + marker_quats = torch.cat([bottom_left_quat, bottom_right_quat, top_left_quat, top_right_quat], dim=0) + marker_locs = torch.cat([bottom_left, bottom_right, top_left, top_right], dim=0) + marker_idxs = torch.tensor([0, 1, 2, 3]).repeat_interleave(N_envs) + markers.visualize(marker_locs, marker_quats, marker_indices=marker_idxs) + class DeformableCubeEnv(DirectRLEnv): cfg: DeformableCubeEnvCfg @@ -187,9 +239,11 @@ def _setup_scene(self): self.object = DeformableObject(self.cfg.object_cfg) self.container = RigidObject(self.cfg.container_cfg) self.table = RigidObject(self.cfg.table_cfg) + self.table_markers = VisualizationMarkers(self.cfg.table_markers_cfg) self.scene.articulations["robot"] = self.robot spawn_ground_plane(prim_path="/World/ground", cfg=GroundPlaneCfg()) + draw_markers(self.table_markers, self.scene.env_origins) # clone, filter, and replicate self.scene.clone_environments(copy_from_source=False)