Skip to content

Commit

Permalink
Merge branch 'main' into feature/tiled_camera_examples
Browse files Browse the repository at this point in the history
  • Loading branch information
glvov-bdai authored Sep 24, 2024
2 parents 4f68566 + 361578f commit 3e749e6
Show file tree
Hide file tree
Showing 10 changed files with 108 additions and 14 deletions.
3 changes: 2 additions & 1 deletion CONTRIBUTORS.md
Original file line number Diff line number Diff line change
Expand Up @@ -31,9 +31,10 @@ Guidelines for modifications:

## Contributors

* Anton Bjørndahl Mortensen
* Alice Zhou
* Amr Mousa
* Andrej Orsula
* Anton Bjørndahl Mortensen
* Antonio Serrano-Muñoz
* Arjun Bhardwaj
* Brayden Zhang
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -79,6 +79,12 @@ def __init__(self, cfg: DirectMARLEnvCfg, render_mode: str | None = None, **kwar
# initialize internal variables
self._is_closed = False

# set the seed for the environment
if self.cfg.seed is not None:
self.seed(self.cfg.seed)
else:
carb.log_warn("Seed not set for the environment. The environment creation may not be deterministic.")

# create a simulation context to control the simulator
if SimulationContext.instance() is None:
self.sim: SimulationContext = SimulationContext(self.cfg.sim)
Expand All @@ -88,6 +94,7 @@ def __init__(self, cfg: DirectMARLEnvCfg, render_mode: str | None = None, **kwar
# print useful information
print("[INFO]: Base environment:")
print(f"\tEnvironment device : {self.device}")
print(f"\tEnvironment seed : {self.cfg.seed}")
print(f"\tPhysics step-size : {self.physics_dt}")
print(f"\tRendering step-size : {self.physics_dt * self.cfg.sim.render_interval}")
print(f"\tEnvironment step-size : {self.step_dt}")
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,14 @@ class DirectMARLEnvCfg:
"""

# general settings
seed: int | None = None
"""The seed for the random number generator. Defaults to None, in which case the seed is not set.
Note:
The seed is set at the beginning of the environment initialization. This ensures that the environment
creation is deterministic and behaves similarly across different runs.
"""

decimation: int = MISSING
"""Number of control action updates @ sim dt per policy dt.
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,7 @@
import omni.isaac.lab.sim as sim_utils
import omni.isaac.lab.utils.math as math_utils
from omni.isaac.lab.actuators import ImplicitActuator
from omni.isaac.lab.assets import Articulation, RigidObject
from omni.isaac.lab.assets import Articulation, DeformableObject, RigidObject
from omni.isaac.lab.managers import SceneEntityCfg
from omni.isaac.lab.terrains import TerrainImporter

Expand Down Expand Up @@ -824,6 +824,48 @@ def reset_joints_by_offset(
asset.write_joint_state_to_sim(joint_pos, joint_vel, env_ids=env_ids)


def reset_nodal_state_uniform(
env: ManagerBasedEnv,
env_ids: torch.Tensor,
position_range: dict[str, tuple[float, float]],
velocity_range: dict[str, tuple[float, float]],
asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"),
):
"""Reset the asset nodal state to a random position and velocity uniformly within the given ranges.
This function randomizes the nodal position and velocity of the asset.
* It samples the root position from the given ranges and adds them to the default nodal position, before setting
them into the physics simulation.
* It samples the root velocity from the given ranges and sets them into the physics simulation.
The function takes a dictionary of position and velocity ranges for each axis. The keys of the
dictionary are ``x``, ``y``, ``z``. The values are tuples of the form ``(min, max)``.
If the dictionary does not contain a key, the position or velocity is set to zero for that axis.
"""
# extract the used quantities (to enable type-hinting)
asset: DeformableObject = env.scene[asset_cfg.name]
# get default root state
nodal_state = asset.data.default_nodal_state_w[env_ids].clone()

# position
range_list = [position_range.get(key, (0.0, 0.0)) for key in ["x", "y", "z"]]
ranges = torch.tensor(range_list, device=asset.device)
rand_samples = math_utils.sample_uniform(ranges[:, 0], ranges[:, 1], (len(env_ids), 1, 3), device=asset.device)

nodal_state[..., :3] += rand_samples

# velocities
range_list = [velocity_range.get(key, (0.0, 0.0)) for key in ["x", "y", "z"]]
ranges = torch.tensor(range_list, device=asset.device)
rand_samples = math_utils.sample_uniform(ranges[:, 0], ranges[:, 1], (len(env_ids), 1, 3), device=asset.device)

nodal_state[..., 3:] += rand_samples

# set into the physics simulation
asset.write_nodal_state_to_sim(nodal_state, env_ids=env_ids)


def reset_scene_to_default(env: ManagerBasedEnv, env_ids: torch.Tensor):
"""Reset the scene to the default state specified in the scene configuration."""
# rigid bodies
Expand All @@ -845,6 +887,11 @@ def reset_scene_to_default(env: ManagerBasedEnv, env_ids: torch.Tensor):
default_joint_vel = articulation_asset.data.default_joint_vel[env_ids].clone()
# set into the physics simulation
articulation_asset.write_joint_state_to_sim(default_joint_pos, default_joint_vel, env_ids=env_ids)
# deformable objects
for deformable_object in env.scene.deformable_objects.values():
# obtain default and set into the physics simulation
nodal_state = deformable_object.data.default_nodal_state_w[env_ids].clone()
deformable_object.write_nodal_state_to_sim(nodal_state, env_ids=env_ids)


"""
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -6,10 +6,14 @@
from omni.isaac.lab.assets import DeformableObjectCfg
from omni.isaac.lab.controllers.differential_ik_cfg import DifferentialIKControllerCfg
from omni.isaac.lab.envs.mdp.actions.actions_cfg import DifferentialInverseKinematicsActionCfg
from omni.isaac.lab.managers import EventTermCfg as EventTerm
from omni.isaac.lab.managers import SceneEntityCfg
from omni.isaac.lab.sim.spawners import UsdFileCfg
from omni.isaac.lab.utils import configclass
from omni.isaac.lab.utils.assets import ISAACLAB_NUCLEUS_DIR

import omni.isaac.lab_tasks.manager_based.manipulation.lift.mdp as mdp

from . import joint_pos_env_cfg

##
Expand All @@ -18,6 +22,11 @@
from omni.isaac.lab_assets.franka import FRANKA_PANDA_HIGH_PD_CFG # isort: skip


##
# Rigid object lift environment.
##


@configclass
class FrankaCubeLiftEnvCfg(joint_pos_env_cfg.FrankaCubeLiftEnvCfg):
def __post_init__(self):
Expand Down Expand Up @@ -50,6 +59,11 @@ def __post_init__(self):
self.observations.policy.enable_corruption = False


##
# Deformable object lift environment.
##


@configclass
class FrankaTeddyBearLiftEnvCfg(FrankaCubeLiftEnvCfg):
def __post_init__(self):
Expand All @@ -58,7 +72,7 @@ def __post_init__(self):

self.scene.object = DeformableObjectCfg(
prim_path="{ENV_REGEX_NS}/Object",
init_state=DeformableObjectCfg.InitialStateCfg(pos=[0.5, 0, 0.05], rot=[0.707, 0, 0, 0.707]),
init_state=DeformableObjectCfg.InitialStateCfg(pos=(0.5, 0, 0.05), rot=(0.707, 0, 0, 0.707)),
spawn=UsdFileCfg(
usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Objects/Teddy_Bear/teddy_bear.usd",
scale=(0.01, 0.01, 0.01),
Expand All @@ -70,11 +84,27 @@ def __post_init__(self):
self.scene.robot.actuators["panda_hand"].stiffness = 40.0
self.scene.robot.actuators["panda_hand"].damping = 10.0

# Remove all the terms for the lift_teddy_bear_sm demo
# Disable replicate physics as it doesn't work for deformable objects
# FIXME: This should be fixed by the PhysX replication system.
self.scene.replicate_physics = False

# Set events for the specific object type (deformable cube)
self.events.reset_object_position = EventTerm(
func=mdp.reset_nodal_state_uniform,
mode="reset",
params={
"position_range": {"x": (-0.1, 0.1), "y": (-0.25, 0.25), "z": (0.0, 0.0)},
"velocity_range": {},
"asset_cfg": SceneEntityCfg("object"),
},
)

# Remove all the terms for the state machine demo
# TODO: Computing the root pose of deformable object from nodal positions is expensive.
# We need to fix that part before enabling these terms for the training.
self.terminations.object_dropping = None
self.rewards.reaching_object = None
self.rewards.lifting_object = None
self.rewards.object_goal_tracking = None
self.rewards.object_goal_tracking_fine_grained = None
self.events.reset_object_position = None
self.observations.policy.object_position = None
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@
from dataclasses import MISSING

import omni.isaac.lab.sim as sim_utils
from omni.isaac.lab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg
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
Expand Down Expand Up @@ -40,7 +40,7 @@ class ObjectTableSceneCfg(InteractiveSceneCfg):
# 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 = MISSING
object: RigidObjectCfg | DeformableObjectCfg = MISSING

# Table
table = AssetBaseCfg(
Expand Down Expand Up @@ -88,7 +88,7 @@ class ActionsCfg:
"""Action specifications for the MDP."""

# will be set by agent env cfg
arm_action: mdp.JointPositionActionCfg = MISSING
arm_action: mdp.JointPositionActionCfg | mdp.DifferentialInverseKinematicsActionCfg = MISSING
gripper_action: mdp.BinaryJointPositionActionCfg = MISSING


Expand Down
2 changes: 2 additions & 0 deletions source/standalone/workflows/rl_games/train.py
Original file line number Diff line number Diff line change
Expand Up @@ -74,6 +74,8 @@ def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agen
"""Train with RL-Games agent."""
# override configurations with non-hydra CLI arguments
env_cfg.scene.num_envs = args_cli.num_envs if args_cli.num_envs is not None else env_cfg.scene.num_envs
env_cfg.sim.device = args_cli.device if args_cli.device is not None else env_cfg.sim.device

agent_cfg["params"]["seed"] = args_cli.seed if args_cli.seed is not None else agent_cfg["params"]["seed"]
agent_cfg["params"]["config"]["max_epochs"] = (
args_cli.max_iterations if args_cli.max_iterations is not None else agent_cfg["params"]["config"]["max_epochs"]
Expand Down
1 change: 1 addition & 0 deletions source/standalone/workflows/rsl_rl/train.py
Original file line number Diff line number Diff line change
Expand Up @@ -85,6 +85,7 @@ def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agen
# set the environment seed
# note: certain randomizations occur in the environment initialization so we set the seed here
env_cfg.seed = agent_cfg.seed
env_cfg.sim.device = args_cli.device if args_cli.device is not None else env_cfg.sim.device

# specify directory for logging experiments
log_root_path = os.path.join("logs", "rsl_rl", agent_cfg.experiment_name)
Expand Down
1 change: 1 addition & 0 deletions source/standalone/workflows/sb3/train.py
Original file line number Diff line number Diff line change
Expand Up @@ -81,6 +81,7 @@ def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agen
# set the environment seed
# note: certain randomizations occur in the environment initialization so we set the seed here
env_cfg.seed = agent_cfg["seed"]
env_cfg.sim.device = args_cli.device if args_cli.device is not None else env_cfg.sim.device

# directory for logging into
log_dir = os.path.join("logs", "sb3", args_cli.task, datetime.now().strftime("%Y-%m-%d_%H-%M-%S"))
Expand Down
9 changes: 3 additions & 6 deletions source/standalone/workflows/skrl/train.py
Original file line number Diff line number Diff line change
Expand Up @@ -106,6 +106,8 @@ def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agen
"""Train with skrl agent."""
# override configurations with non-hydra CLI arguments
env_cfg.scene.num_envs = args_cli.num_envs if args_cli.num_envs is not None else env_cfg.scene.num_envs
env_cfg.sim.device = args_cli.device if args_cli.device is not None else env_cfg.sim.device

# multi-gpu training config
if args_cli.distributed:
env_cfg.sim.device = f"cuda:{app_launcher.local_rank}"
Expand All @@ -118,7 +120,7 @@ def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agen
skrl.config.jax.backend = "jax" if args_cli.ml_framework == "jax" else "numpy"

# set the environment seed
# note: certain randomizations occur in the environment initialization so we set the seed here
# note: certain randomization occur in the environment initialization so we set the seed here
env_cfg.seed = args_cli.seed if args_cli.seed is not None else agent_cfg["seed"]

# specify directory for logging experiments
Expand All @@ -135,11 +137,6 @@ def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agen
# update log_dir
log_dir = os.path.join(log_root_path, log_dir)

# multi-gpu training config
if args_cli.distributed:
# update env config device
env_cfg.sim.device = f"cuda:{app_launcher.local_rank}"

# dump the configuration into log-directory
dump_yaml(os.path.join(log_dir, "params", "env.yaml"), env_cfg)
dump_yaml(os.path.join(log_dir, "params", "agent.yaml"), agent_cfg)
Expand Down

0 comments on commit 3e749e6

Please sign in to comment.