Commit 6d855682 authored by Mayank Mittal's avatar Mayank Mittal

fixes lints for additional flake8 plugins

parent a2554881
...@@ -2,7 +2,16 @@ ...@@ -2,7 +2,16 @@
show-source=True show-source=True
statistics=True statistics=True
per-file-ignores=*/__init__.py:F401 per-file-ignores=*/__init__.py:F401
ignore=E402,E501,W503,E203 # E402: Module level import not at top of file
# E501: Line too long
# W503: Line break before binary operator
# E203: Whitespace before ':' -> conflicts with black
# D401: First line should be in imperative mood
# R504: Unnecessary variable assignment before return statement.
# R505: Unnecessary elif after return statement
# SIM102: Use a single if-statement instead of nested if-statements
# SIM117: Merge with statements for context managers that have same scope.
ignore=E402,E501,W503,E203,D401,R504,R505,SIM102,SIM117
max-line-length = 120 max-line-length = 120
max-complexity = 30 max-complexity = 30
exclude=_*,.vscode,.git exclude=_*,.vscode,.git,docs/**,**/test/**
repos: repos:
- repo: https://github.com/python/black - repo: https://github.com/python/black
rev: 22.10.0 rev: 22.12.0
hooks: hooks:
- id: black - id: black
args: ["--line-length", "120"] args: ["--line-length", "120"]
...@@ -8,30 +8,36 @@ repos: ...@@ -8,30 +8,36 @@ repos:
rev: 6.0.0 rev: 6.0.0
hooks: hooks:
- id: flake8 - id: flake8
additional_dependencies: [flake8-simplify, flake8-return]
- repo: https://github.com/pre-commit/pre-commit-hooks - repo: https://github.com/pre-commit/pre-commit-hooks
rev: v4.4.0 rev: v4.4.0
hooks: hooks:
- id: trailing-whitespace - id: trailing-whitespace
- id: check-symlinks
- id: destroyed-symlinks
- id: check-yaml - id: check-yaml
- id: check-merge-conflict - id: check-merge-conflict
- id: check-case-conflict - id: check-case-conflict
- id: check-executables-have-shebangs - id: check-executables-have-shebangs
- id: check-toml - id: check-toml
- id: end-of-file-fixer - id: end-of-file-fixer
- id: check-shebang-scripts-are-executable
- id: detect-private-key
- id: debug-statements
- repo: https://github.com/pycqa/isort - repo: https://github.com/pycqa/isort
rev: 5.10.1 rev: 5.11.4
hooks: hooks:
- id: isort - id: isort
name: isort (python) name: isort (python)
args: ["--profile", "black", "--filter-files"] args: ["--profile", "black", "--filter-files"]
- repo: https://github.com/asottile/pyupgrade - repo: https://github.com/asottile/pyupgrade
rev: v3.3.0 rev: v3.3.1
hooks: hooks:
- id: pyupgrade - id: pyupgrade
args: ["--py37-plus"] args: ["--py37-plus"]
# FIXME: Figure out why this is getting stuck under VPN. # FIXME: Figure out why this is getting stuck under VPN.
# - repo: https://github.com/RobertCraigie/pyright-python # - repo: https://github.com/RobertCraigie/pyright-python
# rev: v1.1.282 # rev: v1.1.291
# hooks: # hooks:
# - id: pyright # - id: pyright
# Note: We disable this by default since not all code is compatible with it. # Note: We disable this by default since not all code is compatible with it.
......
...@@ -3,8 +3,16 @@ ...@@ -3,8 +3,16 @@
# #
# SPDX-License-Identifier: BSD-3-Clause # SPDX-License-Identifier: BSD-3-Clause
""" """Configuration instances of actuation models for ANYmal robot.
Configuration instances of actuation models for ANYmal robot.
The following actuator models are available:
* ANYdrive 3.x with DC actuator model.
* ANYdrive 3.0 (used on ANYmal-C) with LSTM actuator model.
The following actuator groups are available:
* ANYmal-C default actuator group that uses ANYdrive 3.0 with LSTM actuator model.
""" """
from omni.isaac.orbit.actuators.group import ActuatorControlCfg, ActuatorGroupCfg from omni.isaac.orbit.actuators.group import ActuatorControlCfg, ActuatorGroupCfg
......
...@@ -16,16 +16,17 @@ from omni.isaac.orbit.actuators.model import DCMotor, IdealActuator, ImplicitAct ...@@ -16,16 +16,17 @@ from omni.isaac.orbit.actuators.model import DCMotor, IdealActuator, ImplicitAct
class ActuatorGroup: class ActuatorGroup:
""" """A class for applying actuator models over a collection of actuated joints in an articulation.
The default actuator group for applying the same actuator model over a collection of actuated joints in The default actuator group for applying the same actuator model over a collection of actuated joints in
an articulation. an articulation. It is possible to specify multiple joint-level command types (position, velocity or
torque control).
The joint names are specified in the configuration through a list of regular expressions. The regular The joint names are specified in the configuration through a list of regular expressions. The regular
expressions are matched against the joint names in the articulation. The first match is used to determine expressions are matched against the joint names in the articulation. The first match is used to determine
the joint indices in the articulation. the joint indices in the articulation.
It is possible to specify multiple joint-level command types (position, velocity or torque control). The The command types are applied in the order they are specified in the configuration. For each command, the
command types are applied in the order they are specified in the configuration. For each command, the
scaling and offset can be configured through the :class:`ActuatorControlCfg` class. scaling and offset can be configured through the :class:`ActuatorControlCfg` class.
In the default actuator group, no constraints or formatting is performed over the input actions. Thus, the In the default actuator group, no constraints or formatting is performed over the input actions. Thus, the
...@@ -135,7 +136,7 @@ class ActuatorGroup: ...@@ -135,7 +136,7 @@ class ActuatorGroup:
def __str__(self) -> str: def __str__(self) -> str:
"""A string representation of the actuator group.""" """A string representation of the actuator group."""
msg = ( return (
"<class ActuatorGroup> object:\n" "<class ActuatorGroup> object:\n"
f"\tNumber of DOFs: {self.num_actuators}\n" f"\tNumber of DOFs: {self.num_actuators}\n"
f"\tDOF names : {self.dof_names}\n" f"\tDOF names : {self.dof_names}\n"
...@@ -145,7 +146,6 @@ class ActuatorGroup: ...@@ -145,7 +146,6 @@ class ActuatorGroup:
f"\tControl mode : {self.control_mode}\n" f"\tControl mode : {self.control_mode}\n"
f"\tControl dim : {self.control_dim}" f"\tControl dim : {self.control_dim}"
) )
return msg
""" """
Properties- Group. Properties- Group.
......
...@@ -17,7 +17,7 @@ class GripperActuatorGroup(ActuatorGroup): ...@@ -17,7 +17,7 @@ class GripperActuatorGroup(ActuatorGroup):
""" """
A mimicking actuator group to format a binary open/close command into the joint commands. A mimicking actuator group to format a binary open/close command into the joint commands.
The input actions are processed as a scalar which sign determines whethter to open or close the gripper. The input actions are processed as a scalar which sign determines whether to open or close the gripper.
We consider the following convention: We consider the following convention:
1. Positive value (> 0): open command 1. Positive value (> 0): open command
...@@ -25,7 +25,7 @@ class GripperActuatorGroup(ActuatorGroup): ...@@ -25,7 +25,7 @@ class GripperActuatorGroup(ActuatorGroup):
The mimicking actuator group has only two valid command types: absolute positions (``"p_abs"``) or absolute The mimicking actuator group has only two valid command types: absolute positions (``"p_abs"``) or absolute
velocity (``"v_abs"``). Based on the chosen command type, the joint commands are computed by multiplying the velocity (``"v_abs"``). Based on the chosen command type, the joint commands are computed by multiplying the
reference command with the mimicking multipler. reference command with the mimicking multiplier.
* **position mode:** The reference command is resolved as the joint position target for opening or closing the * **position mode:** The reference command is resolved as the joint position target for opening or closing the
gripper. These targets are read from the :class:`GripperActuatorGroupCfg` class. gripper. These targets are read from the :class:`GripperActuatorGroupCfg` class.
...@@ -74,6 +74,7 @@ class GripperActuatorGroup(ActuatorGroup): ...@@ -74,6 +74,7 @@ class GripperActuatorGroup(ActuatorGroup):
self._previous_dof_targets = torch.zeros(self.num_articulation, self.num_actuators, device=self.device) self._previous_dof_targets = torch.zeros(self.num_articulation, self.num_actuators, device=self.device)
def __str__(self) -> str: def __str__(self) -> str:
"""String representation of the actuator group."""
msg = super().__str__() + "\n" msg = super().__str__() + "\n"
msg = msg.replace("ActuatorGroup", "GripperActuatorGroup") msg = msg.replace("ActuatorGroup", "GripperActuatorGroup")
msg += ( msg += (
......
...@@ -14,7 +14,7 @@ from .actuator_group_cfg import NonHolonomicKinematicsGroupCfg ...@@ -14,7 +14,7 @@ from .actuator_group_cfg import NonHolonomicKinematicsGroupCfg
class NonHolonomicKinematicsGroup(ActuatorGroup): class NonHolonomicKinematicsGroup(ActuatorGroup):
""" r"""
An actuator group that formulates the 2D-base constraint for vehicle kinematics control. An actuator group that formulates the 2D-base constraint for vehicle kinematics control.
In simulation, it is easier to consider the mobile base as a floating link controlled by three dummy joints In simulation, it is easier to consider the mobile base as a floating link controlled by three dummy joints
......
...@@ -20,7 +20,7 @@ from .actuator_cfg import DCMotorCfg, IdealActuatorCfg, VariableGearRatioDCMotor ...@@ -20,7 +20,7 @@ from .actuator_cfg import DCMotorCfg, IdealActuatorCfg, VariableGearRatioDCMotor
class IdealActuator: class IdealActuator:
"""Ideal torque-controlled actuator model with a simple saturation model. r"""Ideal torque-controlled actuator model with a simple saturation model.
It employs the following model for computing torques for the actuated joint :math:`j`: It employs the following model for computing torques for the actuated joint :math:`j`:
...@@ -172,7 +172,7 @@ class IdealActuator: ...@@ -172,7 +172,7 @@ class IdealActuator:
class DCMotor(IdealActuator): class DCMotor(IdealActuator):
""" r"""
Direct control (DC) motor actuator model with velocity-based saturation model. Direct control (DC) motor actuator model with velocity-based saturation model.
It uses the same model as the :class:`IdealActuator` for computing the torques from input commands. It uses the same model as the :class:`IdealActuator` for computing the torques from input commands.
...@@ -253,7 +253,7 @@ class DCMotor(IdealActuator): ...@@ -253,7 +253,7 @@ class DCMotor(IdealActuator):
class VariableGearRatioDCMotor(DCMotor): class VariableGearRatioDCMotor(DCMotor):
"""Torque-controlled actuator with variable gear-ratio based saturation model. r"""Torque-controlled actuator with variable gear-ratio based saturation model.
Instead of using a fixed gear box, some motors are equiped with variators that allow the gear-ratio Instead of using a fixed gear box, some motors are equiped with variators that allow the gear-ratio
to be changed continuously (instead of steps). This model implements a DC motor with a variable to be changed continuously (instead of steps). This model implements a DC motor with a variable
......
...@@ -81,6 +81,17 @@ class DifferentialInverseKinematics: ...@@ -81,6 +81,17 @@ class DifferentialInverseKinematics:
"""Default parameters for different inverse kinematics approaches.""" """Default parameters for different inverse kinematics approaches."""
def __init__(self, cfg: DifferentialInverseKinematicsCfg, num_robots: int, device: str): def __init__(self, cfg: DifferentialInverseKinematicsCfg, num_robots: int, device: str):
"""Initialize the controller.
Args:
cfg (DifferentialInverseKinematicsCfg): The configuration for the controller.
num_robots (int): The number of robots to control.
device (str): The device to use for computations.
Raises:
ValueError: When configured IK-method is not supported.
ValueError: When configured command type is not supported.
"""
# store inputs # store inputs
self.cfg = cfg self.cfg = cfg
self.num_robots = num_robots self.num_robots = num_robots
......
...@@ -64,6 +64,18 @@ class JointImpedanceController: ...@@ -64,6 +64,18 @@ class JointImpedanceController:
""" """
def __init__(self, cfg: JointImpedanceControllerCfg, num_robots: int, dof_pos_limits: torch.Tensor, device: str): def __init__(self, cfg: JointImpedanceControllerCfg, num_robots: int, dof_pos_limits: torch.Tensor, device: str):
"""Initialize joint impedance controller.
Args:
cfg (JointImpedanceControllerCfg): The configuration for the controller.
num_robots (int): The number of robots to control.
dof_pos_limits (torch.Tensor): The joint position limits for each robot. This is a tensor of shape
(num_robots, num_dof, 2) where the last dimension contains the lower and upper limits.
device (str): The device to use for computations.
Raises:
ValueError: When the shape of :obj:`dof_pos_limits` is not (num_robots, num_dof, 2).
"""
# check valid inputs # check valid inputs
if len(dof_pos_limits.shape) != 3: if len(dof_pos_limits.shape) != 3:
raise ValueError(f"Joint position limits has shape '{dof_pos_limits.shape}'. Expected length of shape = 3.") raise ValueError(f"Joint position limits has shape '{dof_pos_limits.shape}'. Expected length of shape = 3.")
...@@ -127,7 +139,12 @@ class JointImpedanceController: ...@@ -127,7 +139,12 @@ class JointImpedanceController:
pass pass
def set_command(self, command: torch.Tensor): def set_command(self, command: torch.Tensor):
"""Set target end-effector pose command.""" """Set target end-effector pose command.
Args:
command (torch.Tensor): The command to set. This is a tensor of shape (num_robots, num_actions) where
:obj:`num_actions` is the dimension of the action space of the controller.
"""
# check input size # check input size
if command.shape != (self.num_robots, self.num_actions): if command.shape != (self.num_robots, self.num_actions):
raise ValueError( raise ValueError(
...@@ -168,6 +185,15 @@ class JointImpedanceController: ...@@ -168,6 +185,15 @@ class JointImpedanceController:
) -> torch.Tensor: ) -> torch.Tensor:
"""Performs inference with the controller. """Performs inference with the controller.
Args:
dof_pos (torch.Tensor): The current joint positions.
dof_vel (torch.Tensor): The current joint velocities.
mass_matrix (Optional[torch.Tensor], optional): The joint-space inertial matrix. Defaults to None.
gravity (Optional[torch.Tensor], optional): The joint-space gravity vector. Defaults to None.
Raises:
ValueError: When the command type is invalid.
Returns: Returns:
torch.Tensor: The target joint torques commands. torch.Tensor: The target joint torques commands.
""" """
......
...@@ -83,6 +83,17 @@ class OperationSpaceController: ...@@ -83,6 +83,17 @@ class OperationSpaceController:
""" """
def __init__(self, cfg: OperationSpaceControllerCfg, num_robots: int, num_dof: int, device: str): def __init__(self, cfg: OperationSpaceControllerCfg, num_robots: int, num_dof: int, device: str):
"""Initialize operation-space controller.
Args:
cfg (OperationSpaceControllerCfg): The configuration for operation-space controller.
num_robots (int): The number of robots to control.
num_dof (int): The number of degrees of freedom of the robot.
device (str): The device to use for computations.
Raises:
ValueError: When invalid control command is provided.
"""
# store inputs # store inputs
self.cfg = cfg self.cfg = cfg
self.num_robots = num_robots self.num_robots = num_robots
...@@ -171,7 +182,11 @@ class OperationSpaceController: ...@@ -171,7 +182,11 @@ class OperationSpaceController:
pass pass
def set_command(self, command: torch.Tensor): def set_command(self, command: torch.Tensor):
"""Set target end-effector pose command.""" """Set target end-effector pose or force command.
Args:
command (torch.Tensor): The target end-effector pose or force command.
"""
# check input size # check input size
if command.shape != (self.num_robots, self.num_actions): if command.shape != (self.num_robots, self.num_actions):
raise ValueError( raise ValueError(
...@@ -214,6 +229,27 @@ class OperationSpaceController: ...@@ -214,6 +229,27 @@ class OperationSpaceController:
) -> torch.Tensor: ) -> torch.Tensor:
"""Performs inference with the controller. """Performs inference with the controller.
Args:
jacobian (torch.Tensor): The Jacobian matrix of the end-effector.
ee_pose (Optional[torch.Tensor], optional): The current end-effector pose. It is a tensor of shape
(num_robots, 7), which contains the position and quaternion ``(w, x, y, z)``. Defaults to None.
ee_vel (Optional[torch.Tensor], optional): The current end-effector velocity. It is a tensor of shape
(num_robots, 6), which contains the linear and angular velocities. Defaults to None.
ee_force (Optional[torch.Tensor], optional): The current external force on the end-effector.
It is a tensor of shape (num_robots, 3), which contains the linear force. Defaults to None.
mass_matrix (Optional[torch.Tensor], optional): The joint-space inertial matrix. Defaults to None.
gravity (Optional[torch.Tensor], optional): The joint-space gravity vector. Defaults to None.
Raises:
ValueError: When the end-effector pose is not provided for the 'position_rel' command.
ValueError: When the end-effector pose is not provided for the 'position_abs' command.
ValueError: When the end-effector pose is not provided for the 'pose_rel' command.
ValueError: When an invalid command type is provided.
ValueError: When motion-control is enabled but the end-effector pose or velocity is not provided.
ValueError: When force-control is enabled but the end-effector force is not provided.
ValueError: When inertial compensation is enabled but the mass matrix is not provided.
ValueError: When gravity compensation is enabled but the gravity vector is not provided.
Returns: Returns:
torch.Tensor: The target joint torques commands. torch.Tensor: The target joint torques commands.
""" """
......
...@@ -38,6 +38,16 @@ class RmpFlowController: ...@@ -38,6 +38,16 @@ class RmpFlowController:
"""Wraps around RMP-Flow from IsaacSim for batched environments.""" """Wraps around RMP-Flow from IsaacSim for batched environments."""
def __init__(self, cfg: RmpFlowControllerCfg, prim_paths_expr: str, device: str): def __init__(self, cfg: RmpFlowControllerCfg, prim_paths_expr: str, device: str):
"""Initialize the controller.
Args:
cfg (RmpFlowControllerCfg): The configuration for the controller.
prim_paths_expr (str): The expression to find the articulation prim paths.
device (str): The device to use for computation.
Raises:
NotImplementedError: When the robot name is not supported.
"""
# store input # store input
self.cfg = cfg self.cfg = cfg
self._device = device self._device = device
......
...@@ -18,6 +18,17 @@ class RmpFlowController: ...@@ -18,6 +18,17 @@ class RmpFlowController:
"""Wraps around RMP-Flow from IsaacSim for batched environments.""" """Wraps around RMP-Flow from IsaacSim for batched environments."""
def __init__(self, prim_paths_expr: str, robot_name: str, dt: float, device: str): def __init__(self, prim_paths_expr: str, robot_name: str, dt: float, device: str):
"""Initialize the controller.
Args:
prim_paths_expr (str): The expression to find the articulation prim paths.
robot_name (str): The name of the robot. Supported robots: franka, ur10.
dt (float): The time step of the simulation.
device (str): The device to use for computation.
Raises:
NotImplementedError: When the robot name is not supported.
"""
# store input # store input
self._device = device self._device = device
# Load configuration for the controller # Load configuration for the controller
......
...@@ -3,9 +3,7 @@ ...@@ -3,9 +3,7 @@
# #
# SPDX-License-Identifier: BSD-3-Clause # SPDX-License-Identifier: BSD-3-Clause
""" """Base class for teleoperation interface."""
Base class for tele-operation interface.
"""
from abc import ABC, abstractmethod from abc import ABC, abstractmethod
...@@ -47,6 +45,6 @@ class DeviceBase(ABC): ...@@ -47,6 +45,6 @@ class DeviceBase(ABC):
"""Provides the joystick event state. """Provides the joystick event state.
Returns: Returns:
Any -- The processed output form the joystick. Any: The processed output form the joystick.
""" """
raise NotImplementedError raise NotImplementedError
...@@ -3,9 +3,7 @@ ...@@ -3,9 +3,7 @@
# #
# SPDX-License-Identifier: BSD-3-Clause # SPDX-License-Identifier: BSD-3-Clause
""" """Keyboard controller for SE(2) control."""
Keyboard controller for SE(2) control.
"""
import numpy as np import numpy as np
...@@ -18,7 +16,7 @@ from ..device_base import DeviceBase ...@@ -18,7 +16,7 @@ from ..device_base import DeviceBase
class Se2Keyboard(DeviceBase): class Se2Keyboard(DeviceBase):
"""A keyboard controller for sending SE(2) commands as velocity commands. r"""A keyboard controller for sending SE(2) commands as velocity commands.
This class is designed to provide a keyboard controller for mobile base (such as quadrupeds). This class is designed to provide a keyboard controller for mobile base (such as quadrupeds).
It uses the Omniverse keyboard interface to listen to keyboard events and map them to robot's It uses the Omniverse keyboard interface to listen to keyboard events and map them to robot's
......
...@@ -3,9 +3,7 @@ ...@@ -3,9 +3,7 @@
# #
# SPDX-License-Identifier: BSD-3-Clause # SPDX-License-Identifier: BSD-3-Clause
""" """Keyboard controller for SE(3) control."""
Keyboard controller for SE(3) control.
"""
import numpy as np import numpy as np
......
...@@ -16,7 +16,7 @@ from .utils import convert_buffer ...@@ -16,7 +16,7 @@ from .utils import convert_buffer
class Se2SpaceMouse(DeviceBase): class Se2SpaceMouse(DeviceBase):
"""A space-mouse controller for sending SE(2) commands as delta poses. r"""A space-mouse controller for sending SE(2) commands as delta poses.
This class implements a space-mouse controller to provide commands to mobile base. This class implements a space-mouse controller to provide commands to mobile base.
It uses the `HID-API`_ which interfaces with USD and Bluetooth HID-class devices across multiple platforms. It uses the `HID-API`_ which interfaces with USD and Bluetooth HID-class devices across multiple platforms.
......
...@@ -75,5 +75,4 @@ def _scale_to_control(x, axis_scale=350.0, min_v=-1.0, max_v=1.0): ...@@ -75,5 +75,4 @@ def _scale_to_control(x, axis_scale=350.0, min_v=-1.0, max_v=1.0):
float: Clipped, scaled input from HID float: Clipped, scaled input from HID
""" """
x = x / axis_scale x = x / axis_scale
x = min(max(x, min_v), max_v) return min(max(x, min_v), max_v)
return x
...@@ -57,10 +57,7 @@ class ArticulatedObjectData: ...@@ -57,10 +57,7 @@ class ArticulatedObjectData:
@property @property
def root_quat_w(self) -> torch.Tensor: def root_quat_w(self) -> torch.Tensor:
""" """Root orientation in quaternion (w, x, y, z) in simulation world frame. Shape is ``(count, 4)``."""
Root orientation in quaternion (w, x, y, z) in simulation world frame.
Shape is ``(count, 4)``.
"""
return self.root_state_w[:, 3:7] return self.root_state_w[:, 3:7]
@property @property
......
...@@ -24,7 +24,7 @@ class RigidObjectData: ...@@ -24,7 +24,7 @@ class RigidObjectData:
@property @property
def root_pos_w(self) -> torch.Tensor: def root_pos_w(self) -> torch.Tensor:
"""Root position in simulation world frame. Shape is ``(count, 3).``""" """Root position in simulation world frame. Shape is ``(count, 3)``."""
return self.root_state_w[:, :3] return self.root_state_w[:, :3]
@property @property
......
...@@ -32,6 +32,11 @@ class LeggedRobot(RobotBase): ...@@ -32,6 +32,11 @@ class LeggedRobot(RobotBase):
""" """
def __init__(self, cfg: LeggedRobotCfg): def __init__(self, cfg: LeggedRobotCfg):
"""Initialize the robot class.
Args:
cfg (LeggedRobotCfg): The configuration instance.
"""
# initialize parent # initialize parent
super().__init__(cfg) super().__init__(cfg)
# container for data access # container for data access
......
...@@ -23,6 +23,11 @@ class MobileManipulator(SingleArmManipulator): ...@@ -23,6 +23,11 @@ class MobileManipulator(SingleArmManipulator):
"""Configuration for the mobile manipulator.""" """Configuration for the mobile manipulator."""
def __init__(self, cfg: MobileManipulatorCfg): def __init__(self, cfg: MobileManipulatorCfg):
"""Initialize the robot class.
Args:
cfg (MobileManipulatorCfg): The configuration instance.
"""
# initialize parent # initialize parent
super().__init__(cfg) super().__init__(cfg)
# container for data access # container for data access
...@@ -116,6 +121,11 @@ class LeggedMobileManipulator(MobileManipulator, LeggedRobot): ...@@ -116,6 +121,11 @@ class LeggedMobileManipulator(MobileManipulator, LeggedRobot):
"""Configuration for the legged mobile manipulator.""" """Configuration for the legged mobile manipulator."""
def __init__(self, cfg: LeggedMobileManipulatorCfg): def __init__(self, cfg: LeggedMobileManipulatorCfg):
"""Initialize the robot class.
Args:
cfg (LeggedMobileManipulatorCfg): The configuration instance.
"""
# initialize parent # initialize parent
super().__init__(cfg) super().__init__(cfg)
# container for data access # container for data access
......
...@@ -31,6 +31,11 @@ class SingleArmManipulator(RobotBase): ...@@ -31,6 +31,11 @@ class SingleArmManipulator(RobotBase):
""" """
def __init__(self, cfg: SingleArmManipulatorCfg): def __init__(self, cfg: SingleArmManipulatorCfg):
"""Initialize the robot class.
Args:
cfg (SingleArmManipulatorCfg): The configuration instance.
"""
# initialize parent # initialize parent
super().__init__(cfg) super().__init__(cfg)
# container for data access # container for data access
......
...@@ -117,7 +117,7 @@ class Camera(SensorBase): ...@@ -117,7 +117,7 @@ class Camera(SensorBase):
def __str__(self) -> str: def __str__(self) -> str:
"""Returns: A string containing information about the instance.""" """Returns: A string containing information about the instance."""
# message for class # message for class
msg = ( return (
f"Camera @ '{self.prim_path}': \n" f"Camera @ '{self.prim_path}': \n"
f"\tdata types : {list(self._data.output.keys())} \n" f"\tdata types : {list(self._data.output.keys())} \n"
f"\ttick rate (s): {self.sensor_tick}\n" f"\ttick rate (s): {self.sensor_tick}\n"
...@@ -127,7 +127,6 @@ class Camera(SensorBase): ...@@ -127,7 +127,6 @@ class Camera(SensorBase):
f"\tposition : {self._data.position} \n" f"\tposition : {self._data.position} \n"
f"\torientation : {self._data.orientation} \n" f"\torientation : {self._data.orientation} \n"
) )
return msg
""" """
Properties Properties
...@@ -523,9 +522,8 @@ class Camera(SensorBase): ...@@ -523,9 +522,8 @@ class Camera(SensorBase):
b = width * 0.5 b = width * 0.5
c = focal_px c = focal_px
d = height * 0.5 d = height * 0.5
cam_intrinsic_matrix = np.array([[a, 0, b], [0, c, d], [0, 0, 1]], dtype=float) # return the matrix
return np.array([[a, 0, b], [0, c, d], [0, 0, 1]], dtype=float)
return cam_intrinsic_matrix
def _compute_ros_pose(self) -> Tuple[np.ndarray, np.ndarray]: def _compute_ros_pose(self) -> Tuple[np.ndarray, np.ndarray]:
"""Computes the pose of the camera in the world frame with ROS convention. """Computes the pose of the camera in the world frame with ROS convention.
......
...@@ -28,7 +28,7 @@ def transform_points( ...@@ -28,7 +28,7 @@ def transform_points(
orientation: Optional[Sequence[float]] = None, orientation: Optional[Sequence[float]] = None,
device: Union[torch.device, str, None] = None, device: Union[torch.device, str, None] = None,
) -> Union[np.ndarray, torch.Tensor]: ) -> Union[np.ndarray, torch.Tensor]:
"""Transform input points in a given frame to a target frame. r"""Transform input points in a given frame to a target frame.
This function uses torch operations to transform points from a source frame to a target frame. The This function uses torch operations to transform points from a source frame to a target frame. The
transformation is defined by the position ``t`` and orientation ``R`` of the target frame in the source frame. transformation is defined by the position ``t`` and orientation ``R`` of the target frame in the source frame.
...@@ -85,7 +85,7 @@ def create_pointcloud_from_depth( ...@@ -85,7 +85,7 @@ def create_pointcloud_from_depth(
orientation: Optional[Sequence[float]] = None, orientation: Optional[Sequence[float]] = None,
device: Optional[Union[torch.device, str]] = None, device: Optional[Union[torch.device, str]] = None,
) -> Union[np.ndarray, torch.Tensor]: ) -> Union[np.ndarray, torch.Tensor]:
"""Creates pointcloud from input depth image and camera intrinsic matrix. r"""Creates pointcloud from input depth image and camera intrinsic matrix.
This function creates a pointcloud from a depth image and camera intrinsic matrix. The pointcloud is This function creates a pointcloud from a depth image and camera intrinsic matrix. The pointcloud is
computed using the following equation: computed using the following equation:
...@@ -356,4 +356,4 @@ def _create_pointcloud_from_depth_jit( ...@@ -356,4 +356,4 @@ def _create_pointcloud_from_depth_jit(
pts_idx_to_keep = torch.all(torch.logical_and(~torch.isnan(points_xyz), ~torch.isinf(points_xyz)), dim=1) pts_idx_to_keep = torch.all(torch.logical_and(~torch.isnan(points_xyz), ~torch.isinf(points_xyz)), dim=1)
points_xyz = points_xyz[pts_idx_to_keep, ...] points_xyz = points_xyz[pts_idx_to_keep, ...]
return points_xyz return points_xyz # noqa: D504
...@@ -100,7 +100,7 @@ class HeightScanner(SensorBase): ...@@ -100,7 +100,7 @@ class HeightScanner(SensorBase):
def __str__(self) -> str: def __str__(self) -> str:
"""Returns: A string containing information about the instance.""" """Returns: A string containing information about the instance."""
# message for class # message for class
msg = ( return (
f"Height Scanner @ '{self.prim_path}': \n" f"Height Scanner @ '{self.prim_path}': \n"
f"\ttick rate (s) : {self.sensor_tick}\n" f"\ttick rate (s) : {self.sensor_tick}\n"
f"\ttimestamp (s) : {self.timestamp}\n" f"\ttimestamp (s) : {self.timestamp}\n"
...@@ -109,7 +109,6 @@ class HeightScanner(SensorBase): ...@@ -109,7 +109,6 @@ class HeightScanner(SensorBase):
f"\torientation : {self.data.orientation}\n" f"\torientation : {self.data.orientation}\n"
f"\t# of hits : {np.sum(self.data.hit_status)} / {self._scan_points[0]}\n" f"\t# of hits : {np.sum(self.data.hit_status)} / {self._scan_points[0]}\n"
) )
return msg
""" """
Properties Properties
...@@ -159,7 +158,7 @@ class HeightScanner(SensorBase): ...@@ -159,7 +158,7 @@ class HeightScanner(SensorBase):
Operations Operations
""" """
def spawn(self, parent_prim_path: str): def spawn(self, parent_prim_path: str): # noqa: D102
# Check if sensor is already spawned # Check if sensor is already spawned
if self._is_spawned: if self._is_spawned:
raise RuntimeError(f"The height scanner sensor instance has already been spawned at: {self.prim_path}.") raise RuntimeError(f"The height scanner sensor instance has already been spawned at: {self.prim_path}.")
...@@ -175,7 +174,7 @@ class HeightScanner(SensorBase): ...@@ -175,7 +174,7 @@ class HeightScanner(SensorBase):
# Set spawning to true # Set spawning to true
self._is_spawned = True self._is_spawned = True
def initialize(self): def initialize(self): # noqa: D102
# Check that sensor has been spawned # Check that sensor has been spawned
if not self._is_spawned: if not self._is_spawned:
raise RuntimeError("Height scanner sensor must be spawned first. Please call `spawn(...)`.") raise RuntimeError("Height scanner sensor must be spawned first. Please call `spawn(...)`.")
...@@ -187,7 +186,7 @@ class HeightScanner(SensorBase): ...@@ -187,7 +186,7 @@ class HeightScanner(SensorBase):
# Initialize buffers # Initialize buffers
self.reset() self.reset()
def reset(self): def reset(self): # noqa: D102
# reset the timestamp # reset the timestamp
super().reset() super().reset()
# reset the buffer # reset the buffer
......
...@@ -3,6 +3,9 @@ ...@@ -3,6 +3,9 @@
# #
# SPDX-License-Identifier: BSD-3-Clause # SPDX-License-Identifier: BSD-3-Clause
"""Helper class to handle visual sphere markers to show ray-casting of height scanner."""
import numpy as np import numpy as np
import torch import torch
from typing import List, Optional, Sequence, Union from typing import List, Optional, Sequence, Union
...@@ -28,6 +31,16 @@ class HeightScannerMarker: ...@@ -28,6 +31,16 @@ class HeightScannerMarker:
""" """
def __init__(self, prim_path: str, count: int, radius: float = 1.0) -> None: def __init__(self, prim_path: str, count: int, radius: float = 1.0) -> None:
"""Initialize the class.
Args:
prim_path (str): The prim path of the point instancer.
count (int): The number of markers to create.
radius (float, optional): The radius of the spherical markers. Defaults to 1.0.
Raises:
ValueError: When a prim at the given path exists but is not a valid point instancer.
"""
# check inputs # check inputs
stage = stage_utils.get_current_stage() stage = stage_utils.get_current_stage()
# -- prim path # -- prim path
......
...@@ -48,9 +48,7 @@ def create_points_from_grid(size: Tuple[float, float], resolution: float) -> np. ...@@ -48,9 +48,7 @@ def create_points_from_grid(size: Tuple[float, float], resolution: float) -> np.
y = np.arange(-size[1] / 2, size[1] / 2 + resolution, resolution) y = np.arange(-size[1] / 2, size[1] / 2 + resolution, resolution)
grid = np.meshgrid(x, y, sparse=False, indexing="ij") grid = np.meshgrid(x, y, sparse=False, indexing="ij")
# Concatenate the scan grid into points array (N, 2): first x is fixed while y changes # Concatenate the scan grid into points array (N, 2): first x is fixed while y changes
points = np.vstack(list(map(np.ravel, grid))).T return np.vstack(list(map(np.ravel, grid))).T
return points
def plot_height_grid( def plot_height_grid(
......
...@@ -61,7 +61,7 @@ class SensorBase: ...@@ -61,7 +61,7 @@ class SensorBase:
@property @property
def data(self) -> Any: def data(self) -> Any:
"""The data from the simulated sensor.""" """The data from the simulated sensor."""
return None return None # noqa: R501
""" """
Helpers Helpers
......
...@@ -69,7 +69,8 @@ def read_file(path: str) -> io.BytesIO: ...@@ -69,7 +69,8 @@ def read_file(path: str) -> io.BytesIO:
# check file status # check file status
file_status = check_file_path(path) file_status = check_file_path(path)
if file_status == 1: if file_status == 1:
return io.BytesIO(open(path, "rb").read()) with open(path, "rb") as f:
return io.BytesIO(f.read())
elif file_status == 2: elif file_status == 2:
file_content = omni.client.read_file(path)[2] file_content = omni.client.read_file(path)[2]
return io.BytesIO(memoryview(file_content).tobytes()) return io.BytesIO(memoryview(file_content).tobytes())
......
...@@ -97,8 +97,8 @@ def update_class_from_dict(obj, data: Dict[str, Any], _ns: str = "") -> None: ...@@ -97,8 +97,8 @@ def update_class_from_dict(obj, data: Dict[str, Any], _ns: str = "") -> None:
raise ValueError( raise ValueError(
f"[Config]: Incorrect length under namespace: {key_ns}. Expected: {len(obj_mem)}, Received: {len(value)}." f"[Config]: Incorrect length under namespace: {key_ns}. Expected: {len(obj_mem)}, Received: {len(value)}."
) )
else: # set value
setattr(obj, key, value) setattr(obj, key, value)
elif callable(obj_mem): elif callable(obj_mem):
# update function name # update function name
value = _string_to_callable(value) value = _string_to_callable(value)
...@@ -148,11 +148,11 @@ def convert_dict_to_backend( ...@@ -148,11 +148,11 @@ def convert_dict_to_backend(
dict: The updated dict with the data converted to the desired backend. dict: The updated dict with the data converted to the desired backend.
""" """
# THINK: Should we also support converting to a specific device, e.g. "cuda:0"? # THINK: Should we also support converting to a specific device, e.g. "cuda:0"?
# Define the conversion functions for each backend. # Check the backend is valid.
if backend not in TENSOR_TYPE_CONVERSIONS: if backend not in TENSOR_TYPE_CONVERSIONS:
raise ValueError(f"Unknown backend '{backend}'. Supported backends are 'numpy', 'torch', and 'warp'.") raise ValueError(f"Unknown backend '{backend}'. Supported backends are 'numpy', 'torch', and 'warp'.")
else: # Define the conversion functions for each backend.
tensor_type_conversions = TENSOR_TYPE_CONVERSIONS[backend] tensor_type_conversions = TENSOR_TYPE_CONVERSIONS[backend]
# Parse the array types and convert them to the corresponding types: "numpy" -> np.ndarray, etc. # Parse the array types and convert them to the corresponding types: "numpy" -> np.ndarray, etc.
parsed_types = list() parsed_types = list()
...@@ -176,8 +176,8 @@ def convert_dict_to_backend( ...@@ -176,8 +176,8 @@ def convert_dict_to_backend(
# check if we have a known conversion. # check if we have a known conversion.
if data_type not in tensor_type_conversions: if data_type not in tensor_type_conversions:
raise ValueError(f"No registered conversion for data type: {data_type} to {backend}!") raise ValueError(f"No registered conversion for data type: {data_type} to {backend}!")
else: # convert the data to the desired backend.
output_dict[key] = tensor_type_conversions[data_type](value) output_dict[key] = tensor_type_conversions[data_type](value)
# -- nested dictionaries # -- nested dictionaries
elif isinstance(data[key], dict): elif isinstance(data[key], dict):
output_dict[key] = convert_dict_to_backend(value) output_dict[key] = convert_dict_to_backend(value)
......
...@@ -3,6 +3,9 @@ ...@@ -3,6 +3,9 @@
# #
# SPDX-License-Identifier: BSD-3-Clause # SPDX-License-Identifier: BSD-3-Clause
"""Utilities for file I/O with pickle."""
import os import os
import pickle import pickle
from typing import Any from typing import Any
......
...@@ -3,6 +3,8 @@ ...@@ -3,6 +3,8 @@
# #
# SPDX-License-Identifier: BSD-3-Clause # SPDX-License-Identifier: BSD-3-Clause
"""Utilities for file I/O with yaml."""
import os import os
import yaml import yaml
from typing import Dict, Union from typing import Dict, Union
......
...@@ -3,6 +3,7 @@ ...@@ -3,6 +3,7 @@
# #
# SPDX-License-Identifier: BSD-3-Clause # SPDX-License-Identifier: BSD-3-Clause
import contextlib
import math import math
from typing import Optional, Sequence from typing import Optional, Sequence
...@@ -343,9 +344,9 @@ def set_rigid_body_properties( ...@@ -343,9 +344,9 @@ def set_rigid_body_properties(
# check if prim has rigid-body applied on it # check if prim has rigid-body applied on it
if not UsdPhysics.RigidBodyAPI(rigid_body_prim): if not UsdPhysics.RigidBodyAPI(rigid_body_prim):
raise ValueError(f"No rigid body schema present for prim '{prim_path}'.") raise ValueError(f"No rigid body schema present for prim '{prim_path}'.")
else: # retrieve the USD rigid-body api
usd_rigid_body_api = UsdPhysics.RigidBodyAPI(rigid_body_prim) usd_rigid_body_api = UsdPhysics.RigidBodyAPI(rigid_body_prim)
# retrieve the rigid-body api # retrieve the physx rigid-body api
physx_rigid_body_api = PhysxSchema.PhysxRigidBodyAPI(rigid_body_prim) physx_rigid_body_api = PhysxSchema.PhysxRigidBodyAPI(rigid_body_prim)
if not physx_rigid_body_api: if not physx_rigid_body_api:
physx_rigid_body_api = PhysxSchema.PhysxRigidBodyAPI.Apply(rigid_body_prim) physx_rigid_body_api = PhysxSchema.PhysxRigidBodyAPI.Apply(rigid_body_prim)
...@@ -471,10 +472,8 @@ def set_nested_articulation_properties(prim_path: str, **kwargs) -> None: ...@@ -471,10 +472,8 @@ def set_nested_articulation_properties(prim_path: str, **kwargs) -> None:
# get current prim # get current prim
child_prim = all_prims.pop(0) child_prim = all_prims.pop(0)
# set articulation properties # set articulation properties
try: with contextlib.suppress(ValueError):
set_articulation_properties(prim_utils.get_prim_path(child_prim), **kwargs) set_articulation_properties(prim_utils.get_prim_path(child_prim), **kwargs)
except ValueError:
pass
# add all children to tree # add all children to tree
all_prims += child_prim.GetChildren() all_prims += child_prim.GetChildren()
...@@ -516,10 +515,8 @@ def set_nested_rigid_body_properties(prim_path: str, **kwargs): ...@@ -516,10 +515,8 @@ def set_nested_rigid_body_properties(prim_path: str, **kwargs):
# get current prim # get current prim
child_prim = all_prims.pop(0) child_prim = all_prims.pop(0)
# set rigid-body properties # set rigid-body properties
try: with contextlib.suppress(ValueError):
set_rigid_body_properties(prim_utils.get_prim_path(child_prim), **kwargs) set_rigid_body_properties(prim_utils.get_prim_path(child_prim), **kwargs)
except ValueError:
pass
# add all children to tree # add all children to tree
all_prims += child_prim.GetChildren() all_prims += child_prim.GetChildren()
...@@ -550,9 +547,7 @@ def set_nested_collision_properties(prim_path: str, **kwargs): ...@@ -550,9 +547,7 @@ def set_nested_collision_properties(prim_path: str, **kwargs):
# get current prim # get current prim
child_prim = all_prims.pop(0) child_prim = all_prims.pop(0)
# set collider properties # set collider properties
try: with contextlib.suppress(ValueError):
set_collision_properties(prim_utils.get_prim_path(child_prim), **kwargs) set_collision_properties(prim_utils.get_prim_path(child_prim), **kwargs)
except ValueError:
pass
# add all children to tree # add all children to tree
all_prims += child_prim.GetChildren() all_prims += child_prim.GetChildren()
...@@ -112,7 +112,6 @@ def copysign(mag: float, other: torch.Tensor) -> torch.Tensor: ...@@ -112,7 +112,6 @@ def copysign(mag: float, other: torch.Tensor) -> torch.Tensor:
Returns: Returns:
torch.Tensor: The output tensor. torch.Tensor: The output tensor.
""" """
mag = torch.tensor(mag, device=other.device, dtype=torch.float).repeat(other.shape[0]) mag = torch.tensor(mag, device=other.device, dtype=torch.float).repeat(other.shape[0])
return torch.abs(mag) * torch.sign(other) return torch.abs(mag) * torch.sign(other)
...@@ -288,7 +287,7 @@ def quat_apply_yaw(quat: torch.Tensor, vec: torch.Tensor) -> torch.Tensor: ...@@ -288,7 +287,7 @@ def quat_apply_yaw(quat: torch.Tensor, vec: torch.Tensor) -> torch.Tensor:
@torch.jit.script @torch.jit.script
def quat_box_minus(q1: torch.Tensor, q2: torch.Tensor) -> torch.Tensor: def quat_box_minus(q1: torch.Tensor, q2: torch.Tensor) -> torch.Tensor:
"""Implements box-minus operator (quaternion difference) """Implements box-minus operator (quaternion difference).
Args: Args:
q1 (torch.Tensor): A (N, 4) tensor for quaternion (x, y, z, w) q1 (torch.Tensor): A (N, 4) tensor for quaternion (x, y, z, w)
...@@ -350,7 +349,7 @@ Transformations ...@@ -350,7 +349,7 @@ Transformations
def combine_frame_transforms( def combine_frame_transforms(
t01: torch.Tensor, q01: torch.Tensor, t12: torch.Tensor = None, q12: torch.Tensor = None t01: torch.Tensor, q01: torch.Tensor, t12: torch.Tensor = None, q12: torch.Tensor = None
) -> Tuple[torch.Tensor, torch.Tensor]: ) -> Tuple[torch.Tensor, torch.Tensor]:
"""Combine transformations between two reference frames into a stationary frame. r"""Combine transformations between two reference frames into a stationary frame.
It performs the following transformation operation: :math:`T_{02} = T_{01} \\times T_{12}`, It performs the following transformation operation: :math:`T_{02} = T_{01} \\times T_{12}`,
where :math:`T_{AB}` is the homogenous transformation matrix from frame A to B. where :math:`T_{AB}` is the homogenous transformation matrix from frame A to B.
...@@ -383,7 +382,7 @@ def combine_frame_transforms( ...@@ -383,7 +382,7 @@ def combine_frame_transforms(
def subtract_frame_transforms( def subtract_frame_transforms(
t01: torch.Tensor, q01: torch.Tensor, t02: torch.Tensor, q02: torch.Tensor t01: torch.Tensor, q01: torch.Tensor, t02: torch.Tensor, q02: torch.Tensor
) -> Tuple[torch.Tensor, torch.Tensor]: ) -> Tuple[torch.Tensor, torch.Tensor]:
"""Subtract transformations between two reference frames into a stationary frame. r"""Subtract transformations between two reference frames into a stationary frame.
It performs the following transformation operation: :math:`T_{12} = T_{01}^{-1} \\times T_{02}`, It performs the following transformation operation: :math:`T_{12} = T_{01}^{-1} \\times T_{02}`,
where :math:`T_{AB}` is the homogenous transformation matrix from frame A to B. where :math:`T_{AB}` is the homogenous transformation matrix from frame A to B.
...@@ -522,9 +521,7 @@ def random_orientation(num: int, device: str) -> torch.Tensor: ...@@ -522,9 +521,7 @@ def random_orientation(num: int, device: str) -> torch.Tensor:
# sample random orientation from normal distribution # sample random orientation from normal distribution
quat = torch.randn((num, 4), dtype=torch.float, device=device) quat = torch.randn((num, 4), dtype=torch.float, device=device)
# normalize the quaternion # normalize the quaternion
quat = torch.nn.functional.normalize(quat, p=2.0, dim=-1, eps=1e-12) return torch.nn.functional.normalize(quat, p=2.0, dim=-1, eps=1e-12)
return quat
@torch.jit.script @torch.jit.script
......
...@@ -3,6 +3,9 @@ ...@@ -3,6 +3,9 @@
# #
# SPDX-License-Identifier: BSD-3-Clause # SPDX-License-Identifier: BSD-3-Clause
"""Observation manager for computing observation signals for a given world."""
import copy import copy
import functools import functools
import inspect import inspect
......
...@@ -3,6 +3,9 @@ ...@@ -3,6 +3,9 @@
# #
# SPDX-License-Identifier: BSD-3-Clause # SPDX-License-Identifier: BSD-3-Clause
"""Reward manager for computing reward signals for a given world."""
import copy import copy
import inspect import inspect
import torch import torch
......
...@@ -12,7 +12,7 @@ from typing import Any, Optional ...@@ -12,7 +12,7 @@ from typing import Any, Optional
class TimerError(Exception): class TimerError(Exception):
"""A custom exception used to report errors in use of Timer class""" """A custom exception used to report errors in use of :class:`Timer` class."""
pass pass
...@@ -23,6 +23,10 @@ class Timer(ContextDecorator): ...@@ -23,6 +23,10 @@ class Timer(ContextDecorator):
A class to keep track of time for performance measurement. A class to keep track of time for performance measurement.
It allows timing via context managers and decorators as well. It allows timing via context managers and decorators as well.
It uses the `time.perf_counter` function to measure time. This function
returns the number of seconds since the epoch as a float. It has the
highest resolution available on the system.
As a regular object: As a regular object:
.. code-block:: python .. code-block:: python
...@@ -56,8 +60,11 @@ class Timer(ContextDecorator): ...@@ -56,8 +60,11 @@ class Timer(ContextDecorator):
""" """
def __init__(self, msg: Optional[str] = None): def __init__(self, msg: Optional[str] = None):
""" """Initializes the timer.
Initializes the class variables
Args:
msg (Optional[str], optional): The message to display when using the timer
class in a context manager. Defaults to None.
""" """
self._msg = msg self._msg = msg
self._start_time = None self._start_time = None
...@@ -65,9 +72,10 @@ class Timer(ContextDecorator): ...@@ -65,9 +72,10 @@ class Timer(ContextDecorator):
self._elapsed_time = None self._elapsed_time = None
def __str__(self) -> str: def __str__(self) -> str:
""" """A string representation of the class object.
Returns: Returns:
str -- String representation of the class object. str: A string containing the elapsed time.
""" """
return f"{self.time_elapsed:0.6f} seconds" return f"{self.time_elapsed:0.6f} seconds"
...@@ -101,7 +109,7 @@ class Timer(ContextDecorator): ...@@ -101,7 +109,7 @@ class Timer(ContextDecorator):
self._start_time = time.perf_counter() self._start_time = time.perf_counter()
def stop(self): def stop(self):
"""Stop timing""" """Stop timing."""
if self._start_time is None: if self._start_time is None:
raise TimerError("Timer is not running. Use .start() to start it") raise TimerError("Timer is not running. Use .start() to start it")
......
...@@ -90,7 +90,7 @@ class IsaacEnv(gym.Env): ...@@ -90,7 +90,7 @@ class IsaacEnv(gym.Env):
self.rendering_dt = self.cfg.sim.dt * self.cfg.sim.substeps self.rendering_dt = self.cfg.sim.dt * self.cfg.sim.substeps
# print useful information # print useful information
print("[INFO]: Isaac Sim Environment:") print("[INFO]: Isaac Orbit environment:")
print(f"\t\t Number of instances : {self.num_envs}") print(f"\t\t Number of instances : {self.num_envs}")
print(f"\t\t Environment device : {self.device}") print(f"\t\t Environment device : {self.device}")
print(f"\t\t Physics step-size : {self.physics_dt}") print(f"\t\t Physics step-size : {self.physics_dt}")
......
...@@ -176,6 +176,7 @@ class ControlCfg: ...@@ -176,6 +176,7 @@ class ControlCfg:
@configclass @configclass
class VelocityEnvCfg(IsaacEnvCfg): class VelocityEnvCfg(IsaacEnvCfg):
"""Configuration for the locomotion velocity environment."""
# General Settings # General Settings
env: EnvCfg = EnvCfg(num_envs=4096, env_spacing=2.5, episode_length_s=20.0) env: EnvCfg = EnvCfg(num_envs=4096, env_spacing=2.5, episode_length_s=20.0)
......
...@@ -222,8 +222,7 @@ class VelocityEnv(IsaacEnv): ...@@ -222,8 +222,7 @@ class VelocityEnv(IsaacEnv):
self._push_interval = math.ceil(self.cfg.randomization.push_robot["interval_s"] / self.dt) self._push_interval = math.ceil(self.cfg.randomization.push_robot["interval_s"] / self.dt)
def _initialize_views(self) -> None: def _initialize_views(self) -> None:
"""Creates views and extract useful quantities from them""" """Creates views and extract useful quantities from them."""
# play the simulator to activate physics handles # play the simulator to activate physics handles
# note: this activates the physics simulation view that exposes TensorAPIs # note: this activates the physics simulation view that exposes TensorAPIs
self.sim.reset() self.sim.reset()
...@@ -477,7 +476,7 @@ class LocomotionVelocityRewardManager(RewardManager): ...@@ -477,7 +476,7 @@ class LocomotionVelocityRewardManager(RewardManager):
return torch.sum(out_of_limits, dim=1) return torch.sum(out_of_limits, dim=1)
def dof_vel_limits(self, env: VelocityEnv, soft_ratio: float): def dof_vel_limits(self, env: VelocityEnv, soft_ratio: float):
"""Penalize dof velocities too close to the limit """Penalize dof velocities too close to the limit.
Args: Args:
soft_ratio (float): Defines the soft limit as a percentage of the hard limit. soft_ratio (float): Defines the soft limit as a percentage of the hard limit.
......
...@@ -182,6 +182,7 @@ class ControlCfg: ...@@ -182,6 +182,7 @@ class ControlCfg:
@configclass @configclass
class LiftEnvCfg(IsaacEnvCfg): class LiftEnvCfg(IsaacEnvCfg):
"""Configuration for the Lift environment."""
# General Settings # General Settings
env: EnvCfg = EnvCfg(num_envs=1024, env_spacing=2.5, episode_length_s=4.0) env: EnvCfg = EnvCfg(num_envs=1024, env_spacing=2.5, episode_length_s=4.0)
......
...@@ -227,8 +227,7 @@ class LiftEnv(IsaacEnv): ...@@ -227,8 +227,7 @@ class LiftEnv(IsaacEnv):
setattr(config, attr, torch.tensor(getattr(config, attr), device=self.device, requires_grad=False)) setattr(config, attr, torch.tensor(getattr(config, attr), device=self.device, requires_grad=False))
def _initialize_views(self) -> None: def _initialize_views(self) -> None:
"""Creates views and extract useful quantities from them""" """Creates views and extract useful quantities from them."""
# play the simulator to activate physics handles # play the simulator to activate physics handles
# note: this activates the physics simulation view that exposes TensorAPIs # note: this activates the physics simulation view that exposes TensorAPIs
self.sim.reset() self.sim.reset()
......
...@@ -128,6 +128,7 @@ class ControlCfg: ...@@ -128,6 +128,7 @@ class ControlCfg:
@configclass @configclass
class ReachEnvCfg(IsaacEnvCfg): class ReachEnvCfg(IsaacEnvCfg):
"""Configuration for the reach environment."""
# General Settings # General Settings
env: EnvCfg = EnvCfg(num_envs=2048, env_spacing=2.5, episode_length_s=4.0) env: EnvCfg = EnvCfg(num_envs=2048, env_spacing=2.5, episode_length_s=4.0)
......
...@@ -199,8 +199,7 @@ class ReachEnv(IsaacEnv): ...@@ -199,8 +199,7 @@ class ReachEnv(IsaacEnv):
setattr(config, attr, torch.tensor(getattr(config, attr), device=self.device, requires_grad=False)) setattr(config, attr, torch.tensor(getattr(config, attr), device=self.device, requires_grad=False))
def _initialize_views(self) -> None: def _initialize_views(self) -> None:
"""Creates views and extract useful quantities from them""" """Creates views and extract useful quantities from them."""
# play the simulator to activate physics handles # play the simulator to activate physics handles
# note: this activates the physics simulation view that exposes TensorAPIs # note: this activates the physics simulation view that exposes TensorAPIs
self.sim.reset() self.sim.reset()
...@@ -320,8 +319,7 @@ class ReachRewardManager(RewardManager): ...@@ -320,8 +319,7 @@ class ReachRewardManager(RewardManager):
def tracking_robot_position_l2(self, env: ReachEnv): def tracking_robot_position_l2(self, env: ReachEnv):
"""Penalize tracking position error using L2-kernel.""" """Penalize tracking position error using L2-kernel."""
# compute error # compute error
error = torch.sum(torch.square(env.ee_des_pose_w[:, :3] - env.robot.data.ee_state_w[:, 0:3]), dim=1) return torch.sum(torch.square(env.ee_des_pose_w[:, :3] - env.robot.data.ee_state_w[:, 0:3]), dim=1)
return error
def tracking_robot_position_exp(self, env: ReachEnv, sigma: float): def tracking_robot_position_exp(self, env: ReachEnv, sigma: float):
"""Penalize tracking position error using exp-kernel.""" """Penalize tracking position error using exp-kernel."""
......
...@@ -132,12 +132,12 @@ class RlGamesVecEnvWrapper(gym.Wrapper): ...@@ -132,12 +132,12 @@ class RlGamesVecEnvWrapper(gym.Wrapper):
Operations - MDP Operations - MDP
""" """
def reset(self): def reset(self): # noqa: D102
obs_dict = self.env.reset() obs_dict = self.env.reset()
# process observations and states # process observations and states
return self._process_obs(obs_dict) return self._process_obs(obs_dict)
def step(self, actions): def step(self, actions): # noqa: D102
# clip the actions # clip the actions
actions = torch.clamp(actions.clone(), -self._clip_actions, self._clip_actions) actions = torch.clamp(actions.clone(), -self._clip_actions, self._clip_actions)
# perform environment step # perform environment step
...@@ -212,16 +212,32 @@ class RlGamesGpuEnv(IVecEnv): ...@@ -212,16 +212,32 @@ class RlGamesGpuEnv(IVecEnv):
# TODO: Adding this for now but do we really need this? # TODO: Adding this for now but do we really need this?
def __init__(self, config_name: str, num_actors: int, **kwargs): def __init__(self, config_name: str, num_actors: int, **kwargs):
"""Initialize the environment.
Args:
config_name (str): The name of the environment configuration.
num_actors (int): The number of actors in the environment. This is not used in this wrapper.
"""
self.env: RlGamesVecEnvWrapper = env_configurations.configurations[config_name]["env_creator"](**kwargs) self.env: RlGamesVecEnvWrapper = env_configurations.configurations[config_name]["env_creator"](**kwargs)
def step(self, action): def step(self, action): # noqa: D102
return self.env.step(action) return self.env.step(action)
def reset(self): def reset(self): # noqa: D102
return self.env.reset() return self.env.reset()
def get_number_of_agents(self): def get_number_of_agents(self) -> int:
"""Get number of agents in the environment.
Returns:
int: The number of agents in the environment.
"""
return self.env.get_number_of_agents() return self.env.get_number_of_agents()
def get_env_info(self): def get_env_info(self) -> dict:
"""Get the Gym spaces for the environment.
Returns:
dict: The Gym spaces for the environment.
"""
return self.env.get_env_info() return self.env.get_env_info()
...@@ -58,6 +58,15 @@ class RslRlVecEnvWrapper(gym.Wrapper, VecEnv): ...@@ -58,6 +58,15 @@ class RslRlVecEnvWrapper(gym.Wrapper, VecEnv):
""" """
def __init__(self, env: IsaacEnv): def __init__(self, env: IsaacEnv):
"""Initializes the wrapper.
Args:
env (IsaacEnv): The environment to wrap around.
Raises:
ValueError: When the environment is not an instance of :class:`IsaacEnv`.
ValueError: When the observation space is not a :obj:`gym.spaces.Box`.
"""
# check that input is valid # check that input is valid
if not isinstance(env.unwrapped, IsaacEnv): if not isinstance(env.unwrapped, IsaacEnv):
raise ValueError(f"The environment must be inherited from IsaacEnv. Environment type: {type(env)}") raise ValueError(f"The environment must be inherited from IsaacEnv. Environment type: {type(env)}")
...@@ -100,13 +109,13 @@ class RslRlVecEnvWrapper(gym.Wrapper, VecEnv): ...@@ -100,13 +109,13 @@ class RslRlVecEnvWrapper(gym.Wrapper, VecEnv):
Operations - MDP Operations - MDP
""" """
def reset(self) -> VecEnvObs: def reset(self) -> VecEnvObs: # noqa: D102
# reset the environment # reset the environment
obs_dict = self.env.reset() obs_dict = self.env.reset()
# return observations # return observations
return self._process_obs(obs_dict) return self._process_obs(obs_dict)
def step(self, actions: torch.Tensor) -> VecEnvStepReturn: def step(self, actions: torch.Tensor) -> VecEnvStepReturn: # noqa: D102
# record step information # record step information
obs_dict, rew, dones, extras = self.env.step(actions) obs_dict, rew, dones, extras = self.env.step(actions)
# process observations # process observations
......
...@@ -46,7 +46,7 @@ class Sb3VecEnvWrapper(gym.Wrapper, VecEnv): ...@@ -46,7 +46,7 @@ class Sb3VecEnvWrapper(gym.Wrapper, VecEnv):
We also add monitoring functionality that computes the un-discounted episode We also add monitoring functionality that computes the un-discounted episode
return and length. This information is added to the info dicts under key `episode`. return and length. This information is added to the info dicts under key `episode`.
In contrast to Isaac Sim environment, stable-baselines expect the following: In contrast to Isaac Orbit environment, stable-baselines expect the following:
1. numpy datatype for MDP signals 1. numpy datatype for MDP signals
2. a list of info dicts for each sub-environment (instead of a dict) 2. a list of info dicts for each sub-environment (instead of a dict)
...@@ -67,6 +67,14 @@ class Sb3VecEnvWrapper(gym.Wrapper, VecEnv): ...@@ -67,6 +67,14 @@ class Sb3VecEnvWrapper(gym.Wrapper, VecEnv):
""" """
def __init__(self, env: IsaacEnv): def __init__(self, env: IsaacEnv):
"""Initialize the wrapper.
Args:
env: The environment to wrap around.
Raises:
ValueError: When the environment is not an instance of :class:`IsaacEnv`.
"""
# check that input is valid # check that input is valid
if not isinstance(env.unwrapped, IsaacEnv): if not isinstance(env.unwrapped, IsaacEnv):
raise ValueError(f"The environment must be inherited from IsaacEnv. Environment type: {type(env)}") raise ValueError(f"The environment must be inherited from IsaacEnv. Environment type: {type(env)}")
...@@ -94,12 +102,12 @@ class Sb3VecEnvWrapper(gym.Wrapper, VecEnv): ...@@ -94,12 +102,12 @@ class Sb3VecEnvWrapper(gym.Wrapper, VecEnv):
Operations - MDP Operations - MDP
""" """
def reset(self) -> VecEnvObs: def reset(self) -> VecEnvObs: # noqa: D102
obs_dict = self.env.reset() obs_dict = self.env.reset()
# convert data types to numpy depending on backend # convert data types to numpy depending on backend
return self._process_obs(obs_dict) return self._process_obs(obs_dict)
def step(self, actions: np.ndarray) -> VecEnvStepReturn: def step(self, actions: np.ndarray) -> VecEnvStepReturn: # noqa: D102
# convert input to numpy array # convert input to numpy array
actions = np.asarray(actions) actions = np.asarray(actions)
# convert to tensor # convert to tensor
...@@ -130,25 +138,25 @@ class Sb3VecEnvWrapper(gym.Wrapper, VecEnv): ...@@ -130,25 +138,25 @@ class Sb3VecEnvWrapper(gym.Wrapper, VecEnv):
Unused methods. Unused methods.
""" """
def step_async(self, actions): def step_async(self, actions): # noqa: D102
self._async_actions = actions self._async_actions = actions
def step_wait(self): def step_wait(self): # noqa: D102
return self.step(self._async_actions) return self.step(self._async_actions)
def get_attr(self, attr_name, indices): def get_attr(self, attr_name, indices): # noqa: D102
raise NotImplementedError raise NotImplementedError
def set_attr(self, attr_name, value, indices=None): def set_attr(self, attr_name, value, indices=None): # noqa: D102
raise NotImplementedError raise NotImplementedError
def env_method(self, method_name: str, *method_args, indices=None, **method_kwargs): def env_method(self, method_name: str, *method_args, indices=None, **method_kwargs): # noqa: D102
raise NotImplementedError raise NotImplementedError
def env_is_wrapped(self, wrapper_class, indices=None): def env_is_wrapped(self, wrapper_class, indices=None): # noqa: D102
raise NotImplementedError raise NotImplementedError
def get_images(self): def get_images(self): # noqa: D102
raise NotImplementedError raise NotImplementedError
""" """
......
"""Wrapper to configure an :class:`IsaacEnv` instance to skrl environment """Wrapper to configure an :class:`IsaacEnv` instance to skrl environment.
The following example shows how to wrap an environment for skrl: The following example shows how to wrap an environment for skrl:
...@@ -48,6 +48,12 @@ def SkrlVecEnvWrapper(env: IsaacEnv): ...@@ -48,6 +48,12 @@ def SkrlVecEnvWrapper(env: IsaacEnv):
is maintained for compatibility with the structure of the extension that contains it. is maintained for compatibility with the structure of the extension that contains it.
Internally it calls the :func:`wrap_env` from the skrl library API. Internally it calls the :func:`wrap_env` from the skrl library API.
Args:
env: The environment to wrap around.
Raises:
ValueError: When the environment is not an instance of :class:`IsaacEnv`.
Reference: Reference:
https://skrl.readthedocs.io/en/latest/modules/skrl.envs.wrapping.html https://skrl.readthedocs.io/en/latest/modules/skrl.envs.wrapping.html
""" """
......
...@@ -22,7 +22,6 @@ import omni.isaac.orbit_envs # noqa: F401 ...@@ -22,7 +22,6 @@ import omni.isaac.orbit_envs # noqa: F401
def main(): def main():
"""Print all environments registered in `omni.isaac.orbit_envs` extension.""" """Print all environments registered in `omni.isaac.orbit_envs` extension."""
# print all the available environments # print all the available environments
table = PrettyTable(["S. No.", "Task Name", "Entry Point", "Config"]) table = PrettyTable(["S. No.", "Task Name", "Entry Point", "Config"])
table.title = "Available Environments in ORBIT" table.title = "Available Environments in ORBIT"
......
...@@ -3,9 +3,7 @@ ...@@ -3,9 +3,7 @@
# #
# SPDX-License-Identifier: BSD-3-Clause # SPDX-License-Identifier: BSD-3-Clause
""" """Script to an environment with random action agent."""
Script to an environment with random action agent.
"""
"""Launch Isaac Sim Simulator first.""" """Launch Isaac Sim Simulator first."""
...@@ -38,8 +36,7 @@ from omni.isaac.orbit_envs.utils import parse_env_cfg ...@@ -38,8 +36,7 @@ from omni.isaac.orbit_envs.utils import parse_env_cfg
def main(): def main():
"""Random actions agent with Isaac Sim environment.""" """Random actions agent with Isaac Orbit environment."""
# parse configuration # parse configuration
env_cfg = parse_env_cfg(args_cli.task, use_gpu=not args_cli.cpu, num_envs=args_cli.num_envs) env_cfg = parse_env_cfg(args_cli.task, use_gpu=not args_cli.cpu, num_envs=args_cli.num_envs)
# create environment # create environment
......
...@@ -3,9 +3,7 @@ ...@@ -3,9 +3,7 @@
# #
# SPDX-License-Identifier: BSD-3-Clause # SPDX-License-Identifier: BSD-3-Clause
""" """Script to run a keyboard teleoperation with Orbit manipulation environments."""
Script to run a keyboard teleoperation with Orbit manipulation environments.
"""
"""Launch Isaac Sim Simulator first.""" """Launch Isaac Sim Simulator first."""
...@@ -60,7 +58,6 @@ def pre_process_actions(delta_pose: torch.Tensor, gripper_command: bool) -> torc ...@@ -60,7 +58,6 @@ def pre_process_actions(delta_pose: torch.Tensor, gripper_command: bool) -> torc
def main(): def main():
"""Running keyboard teleoperation with Orbit manipulation environment.""" """Running keyboard teleoperation with Orbit manipulation environment."""
# parse configuration # parse configuration
env_cfg = parse_env_cfg(args_cli.task, use_gpu=not args_cli.cpu, num_envs=args_cli.num_envs) env_cfg = parse_env_cfg(args_cli.task, use_gpu=not args_cli.cpu, num_envs=args_cli.num_envs)
# modify configuration # modify configuration
......
...@@ -3,9 +3,7 @@ ...@@ -3,9 +3,7 @@
# #
# SPDX-License-Identifier: BSD-3-Clause # SPDX-License-Identifier: BSD-3-Clause
""" """Script to run an environment with zero action agent."""
Script to run an environment with zero action agent.
"""
"""Launch Isaac Sim Simulator first.""" """Launch Isaac Sim Simulator first."""
...@@ -38,8 +36,7 @@ from omni.isaac.orbit_envs.utils import parse_env_cfg ...@@ -38,8 +36,7 @@ from omni.isaac.orbit_envs.utils import parse_env_cfg
def main(): def main():
"""Zero actions agent with Isaac Sim environment.""" """Zero actions agent with Isaac Orbit environment."""
# parse configuration # parse configuration
env_cfg = parse_env_cfg(args_cli.task, use_gpu=not args_cli.cpu, num_envs=args_cli.num_envs) env_cfg = parse_env_cfg(args_cli.task, use_gpu=not args_cli.cpu, num_envs=args_cli.num_envs)
# create environment # create environment
......
...@@ -3,6 +3,8 @@ ...@@ -3,6 +3,8 @@
# #
# SPDX-License-Identifier: BSD-3-Clause # SPDX-License-Identifier: BSD-3-Clause
"""Utility functions for parsing rl-games configuration files."""
import os import os
import yaml import yaml
......
...@@ -3,9 +3,7 @@ ...@@ -3,9 +3,7 @@
# #
# SPDX-License-Identifier: BSD-3-Clause # SPDX-License-Identifier: BSD-3-Clause
""" """Script to play a checkpoint if an RL agent from RL-Games."""
Script to play a checkpoint if an RL agent from RL-Games.
"""
"""Launch Isaac Sim Simulator first.""" """Launch Isaac Sim Simulator first."""
...@@ -53,7 +51,6 @@ from config import parse_rlg_cfg ...@@ -53,7 +51,6 @@ from config import parse_rlg_cfg
def main(): def main():
"""Play with RL-Games agent.""" """Play with RL-Games agent."""
# parse env configuration # parse env configuration
env_cfg = parse_env_cfg(args_cli.task, use_gpu=not args_cli.cpu, num_envs=args_cli.num_envs) env_cfg = parse_env_cfg(args_cli.task, use_gpu=not args_cli.cpu, num_envs=args_cli.num_envs)
agent_cfg = parse_rlg_cfg(args_cli.task) agent_cfg = parse_rlg_cfg(args_cli.task)
......
...@@ -3,9 +3,7 @@ ...@@ -3,9 +3,7 @@
# #
# SPDX-License-Identifier: BSD-3-Clause # SPDX-License-Identifier: BSD-3-Clause
""" """Script to train RL agent with RL-Games."""
Script to train RL agent with RL-Games.
"""
"""Launch Isaac Sim Simulator first.""" """Launch Isaac Sim Simulator first."""
...@@ -58,7 +56,7 @@ from config import parse_rlg_cfg ...@@ -58,7 +56,7 @@ from config import parse_rlg_cfg
def main(): def main():
"""Train with RL-Games agent.""" """Train with RL-Games agent."""
# parse seed from command line
args_cli_seed = args_cli.seed args_cli_seed = args_cli.seed
# parse configuration # parse configuration
...@@ -73,10 +71,7 @@ def main(): ...@@ -73,10 +71,7 @@ def main():
log_root_path = os.path.abspath(log_root_path) log_root_path = os.path.abspath(log_root_path)
print(f"[INFO] Logging experiment in directory: {log_root_path}") print(f"[INFO] Logging experiment in directory: {log_root_path}")
# specify directory for logging runs # specify directory for logging runs
if "full_experiment_name" not in agent_cfg["params"]["config"]: log_dir = agent_cfg["params"]["config"].get("full_experiment_name", datetime.now().strftime("%b%d_%H-%M-%S"))
log_dir = datetime.now().strftime("%b%d_%H-%M-%S")
else:
log_dir = agent_cfg["params"]["config"]["full_experiment_name"]
# set directory into agent config # set directory into agent config
# logging directory path: <train_dir>/<full_experiment_name> # logging directory path: <train_dir>/<full_experiment_name>
agent_cfg["params"]["config"]["train_dir"] = log_root_path agent_cfg["params"]["config"]["train_dir"] = log_root_path
......
...@@ -30,6 +30,7 @@ simulation_app = SimulationApp(config) ...@@ -30,6 +30,7 @@ simulation_app = SimulationApp(config)
"""Rest everything follows.""" """Rest everything follows."""
import contextlib
import gym import gym
import os import os
import torch import torch
...@@ -109,7 +110,7 @@ def main(): ...@@ -109,7 +110,7 @@ def main():
collector_interface.reset() collector_interface.reset()
# simulate environment # simulate environment
try: with contextlib.suppress(KeyboardInterrupt):
while not collector_interface.is_stopped(): while not collector_interface.is_stopped():
# get keyboard command # get keyboard command
delta_pose, gripper_command = teleop_interface.advance() delta_pose, gripper_command = teleop_interface.advance()
...@@ -151,8 +152,6 @@ def main(): ...@@ -151,8 +152,6 @@ def main():
# flush data from collector for successful environments # flush data from collector for successful environments
reset_env_ids = dones.nonzero(as_tuple=False).squeeze(-1) reset_env_ids = dones.nonzero(as_tuple=False).squeeze(-1)
collector_interface.flush(reset_env_ids) collector_interface.flush(reset_env_ids)
except KeyboardInterrupt:
pass
# close the simulator # close the simulator
collector_interface.close() collector_interface.close()
......
...@@ -3,6 +3,9 @@ ...@@ -3,6 +3,9 @@
# #
# SPDX-License-Identifier: BSD-3-Clause # SPDX-License-Identifier: BSD-3-Clause
"""Utility functions for parsing robomimic configuration files."""
import os import os
from omni.isaac.orbit_envs import ORBIT_ENVS_DATA_DIR from omni.isaac.orbit_envs import ORBIT_ENVS_DATA_DIR
......
...@@ -3,9 +3,7 @@ ...@@ -3,9 +3,7 @@
# #
# SPDX-License-Identifier: BSD-3-Clause # SPDX-License-Identifier: BSD-3-Clause
""" """Script to run a trained policy from robomimic."""
Script to run a trained policy from robomimic.
"""
"""Launch Isaac Sim Simulator first.""" """Launch Isaac Sim Simulator first."""
...@@ -30,7 +28,6 @@ simulation_app = SimulationApp(config) ...@@ -30,7 +28,6 @@ simulation_app = SimulationApp(config)
import gym import gym
import math
import torch import torch
import robomimic # noqa: F401 import robomimic # noqa: F401
...@@ -43,8 +40,7 @@ from omni.isaac.orbit_envs.utils import parse_env_cfg ...@@ -43,8 +40,7 @@ from omni.isaac.orbit_envs.utils import parse_env_cfg
def main(): def main():
"""Run a trained policy from robomimic with Isaac Sim environment.""" """Run a trained policy from robomimic with Isaac Orbit environment."""
# parse configuration # parse configuration
env_cfg = parse_env_cfg(args_cli.task, use_gpu=not args_cli.cpu, num_envs=1) env_cfg = parse_env_cfg(args_cli.task, use_gpu=not args_cli.cpu, num_envs=1)
# modify configuration # modify configuration
...@@ -53,17 +49,7 @@ def main(): ...@@ -53,17 +49,7 @@ def main():
env_cfg.terminations.episode_timeout = False env_cfg.terminations.episode_timeout = False
env_cfg.terminations.is_success = True env_cfg.terminations.is_success = True
env_cfg.observations.return_dict_obs_in_group = True env_cfg.observations.return_dict_obs_in_group = True
# FIXME: Teleop works Spawn the Franka s.t. it is axis aligned with world frame.
env_cfg.robot.init_state.dof_pos = {
"panda_joint1": 0.0,
"panda_joint2": math.pi / 16.0,
"panda_joint3": 0.0,
"panda_joint4": -math.pi / 2.0 - math.pi / 3.0,
"panda_joint5": 0.0,
"panda_joint6": math.pi - 0.2,
"panda_joint7": math.pi / 4,
"panda_finger_joint*": 0.035,
}
# create environment # create environment
env = gym.make(args_cli.task, cfg=env_cfg, headless=args_cli.headless) env = gym.make(args_cli.task, cfg=env_cfg, headless=args_cli.headless)
......
...@@ -3,9 +3,7 @@ ...@@ -3,9 +3,7 @@
# #
# SPDX-License-Identifier: BSD-3-Clause # SPDX-License-Identifier: BSD-3-Clause
""" """Tool to merge multiple episodes with single trajectory into one episode with multiple trajectories."""
Tool to merge multiple episodes with single trajectory into one episode with multiple trajectories.
"""
import argparse import argparse
...@@ -17,13 +15,19 @@ if __name__ == "__main__": ...@@ -17,13 +15,19 @@ if __name__ == "__main__":
# parse arguments # parse arguments
parser = argparse.ArgumentParser("Welcome to Orbit: Omniverse Robotics Environments!") parser = argparse.ArgumentParser("Welcome to Orbit: Omniverse Robotics Environments!")
parser.add_argument( parser.add_argument(
"--dir", type=str, default=None, help="PATH to directory that contains all single episode hdf5 files" "--dir", type=str, default=None, help="Path to directory that contains all single episode hdf5 files"
) )
parser.add_argument("--task", type=str, default=None, help="Name of the task.")
parser.add_argument("--out", type=str, default="merged_dataset.hdf5", help="output hdf5 file") parser.add_argument("--out", type=str, default="merged_dataset.hdf5", help="output hdf5 file")
args_cli = parser.parse_args() args_cli = parser.parse_args()
# read arguments
parent_dir = args_cli.dir parent_dir = args_cli.dir
merged_dataset_name = args_cli.out merged_dataset_name = args_cli.out
task_name = args_cli.task
# check valid task name
if task_name is None:
raise ValueError("Please specify a valid task name.")
# get hdf5 entries from specified directory # get hdf5 entries from specified directory
entries = [i for i in os.listdir(parent_dir) if i.endswith(".hdf5")] entries = [i for i in os.listdir(parent_dir) if i.endswith(".hdf5")]
...@@ -31,22 +35,21 @@ if __name__ == "__main__": ...@@ -31,22 +35,21 @@ if __name__ == "__main__":
# create new hdf5 file for merging episodes # create new hdf5 file for merging episodes
fp = h5py.File(parent_dir + merged_dataset_name, "a") fp = h5py.File(parent_dir + merged_dataset_name, "a")
# init # initiate data group
f_grp = fp.create_group("data") f_grp = fp.create_group("data")
f_grp.attrs["num_samples"] = 0 f_grp.attrs["num_samples"] = 0
count = 0
for entry in entries: # merge all episodes
for count, entry in enumerate(entries):
fc = h5py.File(parent_dir + entry, "r") fc = h5py.File(parent_dir + entry, "r")
# find total number of samples in all demos # find total number of samples in all demos
f_grp.attrs["num_samples"] = f_grp.attrs["num_samples"] + fc["data"]["demo_0"].attrs["num_samples"] f_grp.attrs["num_samples"] = f_grp.attrs["num_samples"] + fc["data"]["demo_0"].attrs["num_samples"]
fc.copy("data/demo_0", fp["data"], "demo_" + str(count)) fc.copy("data/demo_0", fp["data"], "demo_" + str(count))
count += 1
# This is needed to run env in robomimic # This is needed to run env in robomimic
fp["data"].attrs["env_args"] = json.dumps({"env_name": "Isaac-Lift-v0", "type": 2, "env_kwargs": {}}) fp["data"].attrs["env_args"] = json.dumps({"env_name": task_name, "type": 2, "env_kwargs": {}})
fp.close() fp.close()
......
...@@ -3,9 +3,7 @@ ...@@ -3,9 +3,7 @@
# #
# SPDX-License-Identifier: BSD-3-Clause # SPDX-License-Identifier: BSD-3-Clause
""" """Tool to check structure of hdf5 files."""
Tool to check structure of hdf5 files.
"""
import argparse import argparse
......
...@@ -63,7 +63,6 @@ def split_train_val_from_hdf5(hdf5_path, val_ratio=0.1, filter_key=None): ...@@ -63,7 +63,6 @@ def split_train_val_from_hdf5(hdf5_path, val_ratio=0.1, filter_key=None):
filter_key (str): if provided, split the subset of demonstration keys stored filter_key (str): if provided, split the subset of demonstration keys stored
under mask/@filter_key instead of the full set of demonstrations under mask/@filter_key instead of the full set of demonstrations
""" """
# retrieve number of demos # retrieve number of demos
f = h5py.File(hdf5_path, "r") f = h5py.File(hdf5_path, "r")
if filter_key is not None: if filter_key is not None:
......
...@@ -62,10 +62,7 @@ from config import ROBOMIMIC_CONFIG_FILES_DICT ...@@ -62,10 +62,7 @@ from config import ROBOMIMIC_CONFIG_FILES_DICT
def train(config, device): def train(config, device):
""" """Train a model using the algorithm."""
Train a model using the algorithm.
"""
# first set seeds # first set seeds
np.random.seed(config.train.seed) np.random.seed(config.train.seed)
torch.manual_seed(config.train.seed) torch.manual_seed(config.train.seed)
...@@ -331,10 +328,13 @@ def train(config, device): ...@@ -331,10 +328,13 @@ def train(config, device):
def main(args): def main(args):
"""Train a model on a task using a specified algorithm."""
# load config
if args.task is not None: if args.task is not None:
ext_cfg = json.load(open(ROBOMIMIC_CONFIG_FILES_DICT[args.task][args.algo])) # load config from json file
config = config_factory(ext_cfg["algo_name"]) with open(ROBOMIMIC_CONFIG_FILES_DICT[args.task][args.algo]) as f:
ext_cfg = json.load(f)
config = config_factory(ext_cfg["algo_name"])
# update config with external json - this will throw errors if # update config with external json - this will throw errors if
# the external config has keys not present in the base algo config # the external config has keys not present in the base algo config
with config.values_unlocked(): with config.values_unlocked():
......
...@@ -3,6 +3,9 @@ ...@@ -3,6 +3,9 @@
# #
# SPDX-License-Identifier: BSD-3-Clause # SPDX-License-Identifier: BSD-3-Clause
"""Utility functions for parsing rsl-rl configuration files."""
import os import os
import yaml import yaml
......
...@@ -3,9 +3,7 @@ ...@@ -3,9 +3,7 @@
# #
# SPDX-License-Identifier: BSD-3-Clause # SPDX-License-Identifier: BSD-3-Clause
""" """Script to play a checkpoint if an RL agent from RSL-RL."""
Script to play a checkpoint if an RL agent from RSL-RL.
"""
"""Launch Isaac Sim Simulator first.""" """Launch Isaac Sim Simulator first."""
...@@ -45,7 +43,6 @@ from config import parse_rslrl_cfg ...@@ -45,7 +43,6 @@ from config import parse_rslrl_cfg
def main(): def main():
"""Play with RSL-RL agent.""" """Play with RSL-RL agent."""
# parse configuration # parse configuration
env_cfg = parse_env_cfg(args_cli.task, use_gpu=not args_cli.cpu, num_envs=args_cli.num_envs) env_cfg = parse_env_cfg(args_cli.task, use_gpu=not args_cli.cpu, num_envs=args_cli.num_envs)
agent_cfg = parse_rslrl_cfg(args_cli.task) agent_cfg = parse_rslrl_cfg(args_cli.task)
......
...@@ -3,9 +3,7 @@ ...@@ -3,9 +3,7 @@
# #
# SPDX-License-Identifier: BSD-3-Clause # SPDX-License-Identifier: BSD-3-Clause
""" """Script to train RL agent with RSL-RL."""
Script to train RL agent with RSL-RL.
"""
"""Launch Isaac Sim Simulator first.""" """Launch Isaac Sim Simulator first."""
...@@ -54,7 +52,6 @@ from config import parse_rslrl_cfg ...@@ -54,7 +52,6 @@ from config import parse_rslrl_cfg
def main(): def main():
"""Train with RSL-RL agent.""" """Train with RSL-RL agent."""
# parse configuration # parse configuration
env_cfg = parse_env_cfg(args_cli.task, use_gpu=not args_cli.cpu, num_envs=args_cli.num_envs) env_cfg = parse_env_cfg(args_cli.task, use_gpu=not args_cli.cpu, num_envs=args_cli.num_envs)
agent_cfg = parse_rslrl_cfg(args_cli.task) agent_cfg = parse_rslrl_cfg(args_cli.task)
......
...@@ -3,6 +3,8 @@ ...@@ -3,6 +3,8 @@
# #
# SPDX-License-Identifier: BSD-3-Clause # SPDX-License-Identifier: BSD-3-Clause
"""Utility functions for parsing sb3 configuration files."""
import os import os
import yaml import yaml
from torch import nn as nn # noqa: F401 from torch import nn as nn # noqa: F401
...@@ -40,6 +42,9 @@ def parse_sb3_cfg(task_name) -> dict: ...@@ -40,6 +42,9 @@ def parse_sb3_cfg(task_name) -> dict:
# parse agent configuration # parse agent configuration
with open(config_file) as f: with open(config_file) as f:
cfg = yaml.load(f, Loader=yaml.FullLoader) cfg = yaml.load(f, Loader=yaml.FullLoader)
# check config is valid
if cfg is None:
raise ValueError(f"Config file is empty: {config_file}")
# post-process certain arguments # post-process certain arguments
# reference: https://github.com/DLR-RM/rl-baselines3-zoo/blob/0e5eb145faefa33e7d79c7f8c179788574b20da5/utils/exp_manager.py#L358 # reference: https://github.com/DLR-RM/rl-baselines3-zoo/blob/0e5eb145faefa33e7d79c7f8c179788574b20da5/utils/exp_manager.py#L358
......
...@@ -3,9 +3,7 @@ ...@@ -3,9 +3,7 @@
# #
# SPDX-License-Identifier: BSD-3-Clause # SPDX-License-Identifier: BSD-3-Clause
""" """Script to train RL agent with Stable Baselines3."""
Script to train RL agent with Stable Baselines3.
"""
"""Launch Isaac Sim Simulator first.""" """Launch Isaac Sim Simulator first."""
...@@ -59,7 +57,6 @@ from config import parse_sb3_cfg ...@@ -59,7 +57,6 @@ from config import parse_sb3_cfg
def main(): def main():
"""Train with stable-baselines agent.""" """Train with stable-baselines agent."""
# parse configuration # parse configuration
env_cfg = parse_env_cfg(args_cli.task, use_gpu=not args_cli.cpu, num_envs=args_cli.num_envs) env_cfg = parse_env_cfg(args_cli.task, use_gpu=not args_cli.cpu, num_envs=args_cli.num_envs)
agent_cfg = parse_sb3_cfg(args_cli.task) agent_cfg = parse_sb3_cfg(args_cli.task)
......
# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
"""Utility functions for parsing skrl configuration files."""
import os import os
import yaml import yaml
...@@ -82,6 +90,4 @@ def convert_skrl_cfg(cfg): ...@@ -82,6 +90,4 @@ def convert_skrl_cfg(cfg):
d["rewards_shaper"] = reward_shaper_function(value) d["rewards_shaper"] = reward_shaper_function(value)
# parse agent configuration and convert to classes # parse agent configuration and convert to classes
update_dict(cfg) return update_dict(cfg)
# return the updated configuration
return cfg
# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
""" """
Script to play a checkpoint if an RL agent from skrl. Script to play a checkpoint of an RL agent from skrl.
Visit the skrl documentation (https://skrl.readthedocs.io) to see the examples structured in a more user-friendly way
Visit the skrl documentation (https://skrl.readthedocs.io) to see the examples structured in
a more user-friendly way.
""" """
"""Launch Isaac Sim Simulator first.""" """Launch Isaac Sim Simulator first."""
...@@ -42,7 +49,6 @@ from config import convert_skrl_cfg, parse_skrl_cfg ...@@ -42,7 +49,6 @@ from config import convert_skrl_cfg, parse_skrl_cfg
def main(): def main():
"""Play with skrl agent.""" """Play with skrl agent."""
# parse env configuration # parse env configuration
env_cfg = parse_env_cfg(args_cli.task, use_gpu=not args_cli.cpu, num_envs=args_cli.num_envs) env_cfg = parse_env_cfg(args_cli.task, use_gpu=not args_cli.cpu, num_envs=args_cli.num_envs)
experiment_cfg = parse_skrl_cfg(args_cli.task) experiment_cfg = parse_skrl_cfg(args_cli.task)
......
# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
""" """
Script to train RL agent with skrl. Script to train RL agent with skrl.
Visit the skrl documentation (https://skrl.readthedocs.io) to see the examples structured in a more user-friendly way
Visit the skrl documentation (https://skrl.readthedocs.io) to see the examples structured in
a more user-friendly way.
""" """
"""Launch Isaac Sim Simulator first.""" """Launch Isaac Sim Simulator first."""
...@@ -53,7 +60,7 @@ from config import convert_skrl_cfg, parse_skrl_cfg ...@@ -53,7 +60,7 @@ from config import convert_skrl_cfg, parse_skrl_cfg
def main(): def main():
"""Train with skrl agent.""" """Train with skrl agent."""
# read the seed from command line
args_cli_seed = args_cli.seed args_cli_seed = args_cli.seed
# parse configuration # parse configuration
......
...@@ -5,7 +5,7 @@ ...@@ -5,7 +5,7 @@
# SPDX-License-Identifier: BSD-3-Clause # SPDX-License-Identifier: BSD-3-Clause
""" """
Convert a mesh file to `.obj` using blender Convert a mesh file to `.obj` using blender.
This file processes a given dae mesh file and saves the resulting mesh file in obj format. This file processes a given dae mesh file and saves the resulting mesh file in obj format.
...@@ -59,7 +59,7 @@ def parse_cli_args(): ...@@ -59,7 +59,7 @@ def parse_cli_args():
def convert_to_obj(in_file: str, out_file: str, save_usd: bool = False): def convert_to_obj(in_file: str, out_file: str, save_usd: bool = False):
"""Convert a mesh file to `.obj` using blender """Convert a mesh file to `.obj` using blender.
Args: Args:
in_file (str): Input mesh file to process. in_file (str): Input mesh file to process.
......
...@@ -42,6 +42,7 @@ Output from the above commands: ...@@ -42,6 +42,7 @@ Output from the above commands:
import argparse import argparse
import contextlib
# omni-isaac-orbit # omni-isaac-orbit
from omni.isaac.kit import SimulationApp from omni.isaac.kit import SimulationApp
...@@ -74,7 +75,6 @@ from omni.isaac.orbit.utils.nucleus import check_file_path ...@@ -74,7 +75,6 @@ from omni.isaac.orbit.utils.nucleus import check_file_path
def main(): def main():
"""Spawns the USD asset robot and clones it using Isaac Gym Cloner API.""" """Spawns the USD asset robot and clones it using Isaac Gym Cloner API."""
# check valid file path # check valid file path
if not check_file_path(args_cli.input): if not check_file_path(args_cli.input):
raise ValueError(f"Invalid file path: {args_cli.input}") raise ValueError(f"Invalid file path: {args_cli.input}")
...@@ -116,12 +116,10 @@ def main(): ...@@ -116,12 +116,10 @@ def main():
# Simulate scene (if not headless) # Simulate scene (if not headless)
if not args_cli.headless: if not args_cli.headless:
try: with contextlib.suppress(KeyboardInterrupt):
while True: while True:
# perform step # perform step
sim.step() sim.step()
except KeyboardInterrupt:
pass
if __name__ == "__main__": if __name__ == "__main__":
......
File mode changed from 100644 to 100755
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment