From eef0eb62f334626fe1f5c47318dcb587be839745 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Sun, 5 Mar 2023 16:55:15 +0100 Subject: [PATCH 01/12] adds physics material for tool sites in single arm --- .../orbit/robots/single_arm/single_arm.py | 30 ++++++++++++++++++- .../orbit/robots/single_arm/single_arm_cfg.py | 20 +++++++++++++ 2 files changed, 49 insertions(+), 1 deletion(-) diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/single_arm/single_arm.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/single_arm/single_arm.py index b66ca500ccf3..bde022d782be 100644 --- a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/single_arm/single_arm.py +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/single_arm/single_arm.py @@ -5,10 +5,13 @@ import re import torch -from typing import Dict, Optional +from typing import Dict, Optional, Sequence +from omni.isaac.core.materials import PhysicsMaterial from omni.isaac.core.prims import RigidPrimView +from pxr import PhysxSchema +import omni.isaac.orbit.utils.kit as kit_utils from omni.isaac.orbit.utils.math import combine_frame_transforms, quat_rotate_inverse, subtract_frame_transforms from ..robot_base import RobotBase @@ -64,6 +67,31 @@ def data(self) -> SingleArmManipulatorData: Operations. """ + def spawn(self, prim_path: str, translation: Sequence[float] = None, orientation: Sequence[float] = None): + # spawn the robot and set its location + super().spawn(prim_path, translation, orientation) + # alter physics collision properties + kit_utils.set_nested_collision_properties(prim_path, contact_offset=0.02, rest_offset=0.0) + # add physics material to the tool sites bodies! + if self.cfg.physics_material is not None and self.cfg.meta_info.tool_sites_names is not None: + # -- resolve material path + material_path = self.cfg.physics_material.prim_path + if not material_path.startswith("/"): + material_path = prim_path + "/" + material_path + # -- create material + material = PhysicsMaterial( + prim_path=material_path, + static_friction=self.cfg.physics_material.static_friction, + dynamic_friction=self.cfg.physics_material.dynamic_friction, + restitution=self.cfg.physics_material.restitution, + ) + # -- enable patch-friction: yields better results! + physx_material_api = PhysxSchema.PhysxMaterialAPI.Apply(material.prim) + physx_material_api.CreateImprovePatchFrictionAttr().Set(True) + # -- bind material to feet + for site_name in self.cfg.meta_info.tool_sites_names: + kit_utils.apply_nested_physics_material(f"{prim_path}/{site_name}", material.prim_path) + def initialize(self, prim_paths_expr: Optional[str] = None): # initialize parent handles super().initialize(prim_paths_expr) diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/single_arm/single_arm_cfg.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/single_arm/single_arm_cfg.py index 58ab933210bf..7eaa50cbb320 100644 --- a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/single_arm/single_arm_cfg.py +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/single_arm/single_arm_cfg.py @@ -56,6 +56,24 @@ class DataInfoCfg: enable_gravity: bool = False """Fill in generalized gravity forces into data buffers. Defaults to False.""" + @configclass + class PhysicsMaterialCfg: + """Physics material applied to the tool sites of the robot.""" + + prim_path = "/World/Materials/toolMaterial" + """Path to the physics material prim. Defaults to /World/Materials/toolMaterial. + + Note: + If the prim path is not absolute, it will be resolved relative to the path specified when spawning + the object. + """ + static_friction: float = 1.0 + """Static friction coefficient. Defaults to 1.0.""" + dynamic_friction: float = 1.0 + """Dynamic friction coefficient. Defaults to 1.0.""" + restitution: float = 0.0 + """Restitution coefficient. Defaults to 0.0.""" + ## # Initialize configurations. ## @@ -66,3 +84,5 @@ class DataInfoCfg: """Information about the end-effector frame location.""" data_info: DataInfoCfg = DataInfoCfg() """Information about what all data to read from simulator.""" + physics_material: PhysicsMaterialCfg = PhysicsMaterialCfg() + """Physics material applied to the tool sites of the robot.""" From 702fb66aa2f4bd1f3e0a76163abea09420d32404 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Sun, 5 Mar 2023 16:56:12 +0100 Subject: [PATCH 02/12] adds collision props settings to robot base --- .../orbit/robots/legged_robot/legged_robot.py | 2 -- .../omni/isaac/orbit/robots/robot_base.py | 34 +++++++------------ .../omni/isaac/orbit/robots/robot_base_cfg.py | 17 ++++++++++ 3 files changed, 29 insertions(+), 24 deletions(-) diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/legged_robot/legged_robot.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/legged_robot/legged_robot.py index 20fdb36247d2..c76f57d0d5d5 100644 --- a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/legged_robot/legged_robot.py +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/legged_robot/legged_robot.py @@ -62,8 +62,6 @@ def feet_names(self) -> Sequence[str]: def spawn(self, prim_path: str, translation: Sequence[float] = None, orientation: Sequence[float] = None): # spawn the robot and set its location super().spawn(prim_path, translation, orientation) - # alter physics collision properties - kit_utils.set_nested_collision_properties(prim_path, contact_offset=0.02, rest_offset=0.0) # add physics material to the feet bodies! if self.cfg.physics_material is not None: # -- resolve material path diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/robot_base.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/robot_base.py index 9021d72da5da..0fdcf3a350cb 100644 --- a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/robot_base.py +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/robot_base.py @@ -67,7 +67,7 @@ def count(self) -> int: @property def device(self) -> str: """Memory device for computation.""" - return self.articulations._device + return self.articulations._device # noqa: W0212 @property def body_names(self) -> List[str]: @@ -132,23 +132,11 @@ def spawn(self, prim_path: str, translation: Sequence[float] = None, orientation carb.log_warn(f"A prim already exists at prim path: '{prim_path}'. Skipping...") # apply rigid body properties - kit_utils.set_nested_rigid_body_properties( - prim_path, - linear_damping=self.cfg.rigid_props.linear_damping, - angular_damping=self.cfg.rigid_props.angular_damping, - max_linear_velocity=self.cfg.rigid_props.max_linear_velocity, - max_angular_velocity=self.cfg.rigid_props.max_angular_velocity, - max_depenetration_velocity=self.cfg.rigid_props.max_depenetration_velocity, - disable_gravity=self.cfg.rigid_props.disable_gravity, - retain_accelerations=self.cfg.rigid_props.retain_accelerations, - ) + kit_utils.set_nested_rigid_body_properties(prim_path, **self.cfg.rigid_props.to_dict()) + # apply collision properties + kit_utils.set_nested_collision_properties(prim_path, **self.cfg.collision_props.to_dict()) # articulation root settings - kit_utils.set_articulation_properties( - prim_path, - enable_self_collisions=self.cfg.articulation_props.enable_self_collisions, - solver_position_iteration_count=self.cfg.articulation_props.solver_position_iteration_count, - solver_velocity_iteration_count=self.cfg.articulation_props.solver_velocity_iteration_count, - ) + kit_utils.set_articulation_properties(prim_path, **self.cfg.articulation_props.to_dict()) # set spawned to true self._is_spawned = True @@ -204,6 +192,7 @@ def reset_buffers(self, env_ids: Optional[Sequence[int]] = None): # reset history self._previous_dof_vel[env_ids] = 0 # TODO: Reset other cached variables. + self.articulations.set_joint_efforts(self._ZERO_JOINT_EFFORT, indices=self._ALL_INDICES) # reset actuators for group in self.actuator_groups.values(): group.reset(env_ids) @@ -220,11 +209,6 @@ def apply_action(self, actions: torch.Tensor): # slice actions per actuator group group_actions_dims = [group.control_dim for group in self.actuator_groups.values()] all_group_actions = torch.split(actions, group_actions_dims, dim=-1) - # silence the physics sim for warnings that make no sense :) - self.articulations._physics_sim_view.enable_warnings(False) - # note (18.08.2022): Saw a difference of up to 5 ms per step when using Isaac Sim - # interfaces compared to direct PhysX calls. Thus, this function is optimized. - # acquire tensors for whole robot # note: we use internal buffers to deal with the resets() as the buffers aren't forwarded # unit the next simulation step. dof_pos = self._data.dof_pos @@ -250,6 +234,11 @@ def apply_action(self, actions: torch.Tensor): self._data.gear_ratio[:, group.dof_indices] = group.gear_ratio if group.velocity_limit is not None: self._data.soft_dof_vel_limits[:, group.dof_indices] = group.velocity_limit + # silence the physics sim for warnings that make no sense :) + # note (18.08.2022): Saw a difference of up to 5 ms per step when using Isaac Sim + # ArticulationView.apply_action() method compared to direct PhysX calls. Thus, + # this function is optimized to apply actions for the whole robot. + self.articulations._physics_sim_view.enable_warnings(False) # apply actions into sim if self.sim_dof_control_modes["position"]: self.articulations._physics_view.set_dof_position_targets(self._data.dof_pos_targets, self._ALL_INDICES) @@ -472,6 +461,7 @@ def _create_buffers(self): self._previous_dof_vel = torch.zeros(self.count, self.num_dof, dtype=torch.float, device=self.device) # constants self._ALL_INDICES = torch.arange(self.count, dtype=torch.long, device=self.device) + self._ZERO_JOINT_EFFORT = torch.zeros(self.count, self.num_dof, dtype=torch.float, device=self.device) # -- frame states self._data.root_state_w = torch.zeros(self.count, 13, dtype=torch.float, device=self.device) diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/robot_base_cfg.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/robot_base_cfg.py index bc7fd670dffb..990b1d6f97a0 100644 --- a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/robot_base_cfg.py +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/robot_base_cfg.py @@ -43,6 +43,21 @@ class RigidBodyPropertiesCfg: retain_accelerations: Optional[bool] = None """Carries over forces/accelerations over sub-steps.""" + @configclass + class CollisionPropertiesCfg: + """Properties to apply to all collisions in the articulation.""" + + collision_enabled: Optional[bool] = None + """Whether to enable or disable collisions.""" + contact_offset: Optional[float] = None + """Contact offset for the collision shape.""" + rest_offset: Optional[float] = None + """Rest offset for the collision shape.""" + torsional_patch_radius: Optional[float] = None + """Radius of the contact patch for applying torsional friction.""" + min_torsional_patch_radius: Optional[float] = None + """Minimum radius of the contact patch for applying torsional friction.""" + @configclass class ArticulationRootPropertiesCfg: """Properties to apply to articulation.""" @@ -85,6 +100,8 @@ class InitialStateCfg: """Initial state of the robot.""" rigid_props: RigidBodyPropertiesCfg = RigidBodyPropertiesCfg() """Properties to apply to all rigid bodies in the articulation.""" + collision_props: CollisionPropertiesCfg = CollisionPropertiesCfg() + """Properties to apply to all collisions in the articulation.""" articulation_props: ArticulationRootPropertiesCfg = ArticulationRootPropertiesCfg() """Properties to apply to articulation.""" actuator_groups: Dict[str, ActuatorGroupCfg] = MISSING From 448146b4594826aca02e319d102a2e4ef22fe189 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Sun, 5 Mar 2023 19:48:32 +0100 Subject: [PATCH 03/12] makes zero correspond to open command --- .../isaac/orbit/actuators/config/franka.py | 2 +- .../orbit/actuators/group/gripper_group.py | 25 +++++++++++++------ 2 files changed, 18 insertions(+), 9 deletions(-) diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/config/franka.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/config/franka.py index 0abe8e67e5e4..1fc8f9240f1a 100644 --- a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/config/franka.py +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/config/franka.py @@ -13,7 +13,7 @@ PANDA_HAND_MIMIC_GROUP_CFG = GripperActuatorGroupCfg( dof_names=["panda_finger_joint[1-2]"], model_cfg=ImplicitActuatorCfg(velocity_limit=0.2, torque_limit=200), - control_cfg=ActuatorControlCfg(command_types=["v_abs"], stiffness={".*": 1e5}, damping={".*": 1e3}), + control_cfg=ActuatorControlCfg(command_types=["p_abs"], stiffness={".*": 1e5}, damping={".*": 1e3}), mimic_multiplier={"panda_finger_joint.*": 1.0}, speed=0.1, open_dof_pos=0.04, diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/group/gripper_group.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/group/gripper_group.py index 4120699673a5..76de871651cc 100644 --- a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/group/gripper_group.py +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/group/gripper_group.py @@ -72,6 +72,8 @@ def __init__(self, cfg: GripperActuatorGroupCfg, view: ArticulationView): # create buffers self._previous_dof_targets = torch.zeros(self.num_articulation, self.num_actuators, device=self.device) + # constants + self._ALL_INDICES = torch.arange(self.view.count, device=self.view._device, dtype=torch.long) def __str__(self) -> str: """String representation of the actuator group.""" @@ -109,18 +111,25 @@ def _format_command(self, command: torch.Tensor) -> torch.Tensor: We consider the following convention: - * Positive command -> open grippers - * Negative command -> close grippers + * Non-negative command (includes 0): open grippers + * Negative command: close grippers Returns: torch.Tensor: The target joint commands for the gripper. """ # FIXME: mimic joint positions -- Gazebo plugin seems to do this. - # The following is commented out because Isaac Sim doesn't support setting joint positions - # of particular dof indices properly. It sets joint positions and joint position targets for - # the whole robot, i.e. all previous position targets is lost. - # mimic_dof_pos = self._dof_pos[:, 0] * self._mimic_multiplier - # self.view.set_joint_positions(mimic_dof_pos, joint_indices=self.dof_indices) + # The following is commented out because Isaac Sim doesn't support setting joint positions + # of particular dof indices properly. It sets joint positions and joint position targets for + # the whole robot, i.e. all previous position targets is lost. This affects resetting the robot + # to a particular joint position. + # self.view._physics_sim_view.enable_warnings(False) + # # get current joint positions + # new_dof_pos = self.view._physics_view.get_dof_positions() + # # set joint positions of the mimic joints + # new_dof_pos[:, self.dof_indices] = self._dof_pos[:, 0].unsqueeze(1) * self._mimic_multiplier + # # set joint positions to the physics view + # self.view.set_joint_positions(new_dof_pos, self._ALL_INDICES) + # self.view._physics_sim_view.enable_warnings(True) # process actions if self.control_mode == "velocity": @@ -134,7 +143,7 @@ def _format_command(self, command: torch.Tensor) -> torch.Tensor: return dof_vel_targets else: # compute new command - dof_pos_targets = torch.where(command > 0, self._open_dof_pos, self._close_dof_pos) + dof_pos_targets = torch.where(command >= 0, self._open_dof_pos, self._close_dof_pos) dof_pos_targets = dof_pos_targets * self._mimic_multiplier # store new command self._previous_dof_targets[:] = dof_pos_targets From 27b73ddf5731e8a944ec078faca7be735ef36750 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Sun, 5 Mar 2023 19:48:50 +0100 Subject: [PATCH 04/12] adds collision props to franka, anymal and a1 --- .../omni/isaac/orbit/robots/config/anymal.py | 8 ++++++++ .../omni/isaac/orbit/robots/config/franka.py | 9 ++++++++- .../omni/isaac/orbit/robots/config/unitree.py | 4 ++++ 3 files changed, 20 insertions(+), 1 deletion(-) diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/config/anymal.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/config/anymal.py index dcf93730335b..c8240dd65f04 100644 --- a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/config/anymal.py +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/config/anymal.py @@ -90,6 +90,10 @@ def euler_rpy_apply(rpy, xyz, degrees=False): max_angular_velocity=1000.0, max_depenetration_velocity=1.0, ), + collision_props=LeggedRobotCfg.CollisionPropertiesCfg( + contact_offset=0.02, + rest_offset=0.0, + ), articulation_props=LeggedRobotCfg.ArticulationRootPropertiesCfg( enable_self_collisions=True, solver_position_iteration_count=4, solver_velocity_iteration_count=1 ), @@ -141,6 +145,10 @@ def euler_rpy_apply(rpy, xyz, degrees=False): max_angular_velocity=1000.0, max_depenetration_velocity=1.0, ), + collision_props=LeggedRobotCfg.CollisionPropertiesCfg( + contact_offset=0.02, + rest_offset=0.0, + ), articulation_props=LeggedRobotCfg.ArticulationRootPropertiesCfg( enable_self_collisions=True, solver_position_iteration_count=4, solver_velocity_iteration_count=1 ), diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/config/franka.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/config/franka.py index e95417807718..4ff839fe931c 100644 --- a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/config/franka.py +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/config/franka.py @@ -40,13 +40,20 @@ "panda_joint5": 0.0, "panda_joint6": 3.037, "panda_joint7": 0.741, - "panda_finger_joint*": 0.035, + "panda_finger_joint*": 0.04, }, dof_vel={".*": 0.0}, ), ee_info=SingleArmManipulatorCfg.EndEffectorFrameCfg( body_name="panda_hand", pos_offset=(0.0, 0.0, 0.1034), rot_offset=(1.0, 0.0, 0.0, 0.0) ), + rigid_props=SingleArmManipulatorCfg.RigidBodyPropertiesCfg( + max_depenetration_velocity=5.0, + ), + collision_props=SingleArmManipulatorCfg.CollisionPropertiesCfg( + contact_offset=0.005, + rest_offset=0.0, + ), actuator_groups={ "panda_shoulder": ActuatorGroupCfg( dof_names=["panda_joint[1-4]"], diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/config/unitree.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/config/unitree.py index 765ea792dfcf..b0b3255496f1 100644 --- a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/config/unitree.py +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/config/unitree.py @@ -54,6 +54,10 @@ max_angular_velocity=1000.0, max_depenetration_velocity=1.0, ), + collision_props=LeggedRobotCfg.CollisionPropertiesCfg( + contact_offset=0.02, + rest_offset=0.0, + ), articulation_props=LeggedRobotCfg.ArticulationRootPropertiesCfg( enable_self_collisions=True, solver_position_iteration_count=4, solver_velocity_iteration_count=1 ), From fd9775cbd25971eef00b2a88f41047d5d98c590a Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Sun, 5 Mar 2023 22:02:58 +0100 Subject: [PATCH 05/12] cleans up setting of prim cfg in object classes --- .../objects/articulated/articulated_object.py | 18 ++---------------- .../isaac/orbit/objects/rigid/rigid_object.py | 8 +------- .../orbit/objects/rigid/rigid_object_cfg.py | 4 ++++ 3 files changed, 7 insertions(+), 23 deletions(-) diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/articulated/articulated_object.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/articulated/articulated_object.py index 714f9fb4e18c..7d94bec61445 100644 --- a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/articulated/articulated_object.py +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/articulated/articulated_object.py @@ -132,23 +132,9 @@ def spawn(self, prim_path: str, translation: Sequence[float] = None, orientation # TODO: What if prim already exists in the stage and spawn isn't called? # apply rigid body properties - kit_utils.set_nested_rigid_body_properties( - prim_path, - linear_damping=self.cfg.rigid_props.linear_damping, - angular_damping=self.cfg.rigid_props.angular_damping, - max_linear_velocity=self.cfg.rigid_props.max_linear_velocity, - max_angular_velocity=self.cfg.rigid_props.max_angular_velocity, - max_depenetration_velocity=self.cfg.rigid_props.max_depenetration_velocity, - disable_gravity=self.cfg.rigid_props.disable_gravity, - retain_accelerations=self.cfg.rigid_props.retain_accelerations, - ) + kit_utils.set_nested_rigid_body_properties(prim_path, **self.cfg.rigid_props.to_dict()) # articulation root settings - kit_utils.set_articulation_properties( - prim_path, - enable_self_collisions=self.cfg.articulation_props.enable_self_collisions, - solver_position_iteration_count=self.cfg.articulation_props.solver_position_iteration_count, - solver_velocity_iteration_count=self.cfg.articulation_props.solver_velocity_iteration_count, - ) + kit_utils.set_articulation_properties(prim_path, **self.cfg.articulation_props.to_dict()) def initialize(self, prim_paths_expr: Optional[str] = None): """Initializes the PhysX handles and internal buffers. diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/rigid/rigid_object.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/rigid/rigid_object.py index 49e2a99cc0dd..b58a7e36ec5c 100644 --- a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/rigid/rigid_object.py +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/rigid/rigid_object.py @@ -110,13 +110,7 @@ def spawn(self, prim_path: str, translation: Sequence[float] = None, orientation # apply rigid body properties API RigidPrim(prim_path=prim_path) # -- set rigid body properties - kit_utils.set_nested_rigid_body_properties( - prim_path, - max_linear_velocity=self.cfg.rigid_props.max_linear_velocity, - max_angular_velocity=self.cfg.rigid_props.max_angular_velocity, - max_depenetration_velocity=self.cfg.rigid_props.max_depenetration_velocity, - disable_gravity=self.cfg.rigid_props.disable_gravity, - ) + kit_utils.set_nested_rigid_body_properties(prim_path, **self.cfg.rigid_props.to_dict()) # create physics material if self.cfg.physics_material is not None: # -- resolve material path diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/rigid/rigid_object_cfg.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/rigid/rigid_object_cfg.py index 3d184efa77f3..7348e6e31331 100644 --- a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/rigid/rigid_object_cfg.py +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/rigid/rigid_object_cfg.py @@ -26,6 +26,10 @@ class MetaInfoCfg: class RigidBodyPropertiesCfg: """Properties to apply to the rigid body.""" + solver_position_iteration_count: Optional[int] = None + """Solver position iteration counts for the body.""" + solver_velocity_iteration_count: Optional[int] = None + """Solver position iteration counts for the body.""" max_linear_velocity: Optional[float] = 1000.0 """Maximum linear velocity for rigid bodies. Defaults to 1000.0.""" max_angular_velocity: Optional[float] = 1000.0 From f2e9c89e763b543b5ac7eb87f62b852c71dc2920 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Mon, 6 Mar 2023 08:58:46 +0100 Subject: [PATCH 06/12] tunes lift env to work with rsl-rl --- .../data/rl_games/lift_ppo.yaml | 12 +- .../data/rsl_rl/lift_ppo.yaml | 4 +- .../orbit_envs/manipulation/lift/lift_cfg.py | 54 +++++---- .../orbit_envs/manipulation/lift/lift_env.py | 107 ++++++++++++++---- 4 files changed, 130 insertions(+), 47 deletions(-) diff --git a/source/extensions/omni.isaac.orbit_envs/data/rl_games/lift_ppo.yaml b/source/extensions/omni.isaac.orbit_envs/data/rl_games/lift_ppo.yaml index fe6933fff4dd..dedc892c3b7a 100644 --- a/source/extensions/omni.isaac.orbit_envs/data/rl_games/lift_ppo.yaml +++ b/source/extensions/omni.isaac.orbit_envs/data/rl_games/lift_ppo.yaml @@ -49,18 +49,18 @@ params: mixed_precision: False normalize_input: True normalize_value: True - value_bootstrap: False + value_bootstrap: True num_actors: -1 reward_shaper: - scale_value: 0.01 + scale_value: 1.0 normalize_advantage: True gamma: 0.99 tau: 0.95 - learning_rate: 3e-4 + learning_rate: 5e-4 lr_schedule: adaptive schedule_type: legacy kl_threshold: 0.008 - score_to_win: 100000000 + score_to_win: 10000 max_epochs: 5000 save_best_after: 200 save_frequency: 100 @@ -69,9 +69,9 @@ params: entropy_coef: 0.0 truncate_grads: True e_clip: 0.2 - horizon_length: 16 + horizon_length: 32 minibatch_size: 2048 - mini_epochs: 8 + mini_epochs: 5 critic_coef: 4 clip_value: True seq_len: 4 diff --git a/source/extensions/omni.isaac.orbit_envs/data/rsl_rl/lift_ppo.yaml b/source/extensions/omni.isaac.orbit_envs/data/rsl_rl/lift_ppo.yaml index 20f63fce447a..ca814415cc66 100644 --- a/source/extensions/omni.isaac.orbit_envs/data/rsl_rl/lift_ppo.yaml +++ b/source/extensions/omni.isaac.orbit_envs/data/rsl_rl/lift_ppo.yaml @@ -32,8 +32,8 @@ algorithm: runner: policy_class_name: "ActorCritic" algorithm_class_name: "PPO" - num_steps_per_env: 192 # per iteration - max_iterations: 1500 # number of policy updates + num_steps_per_env: 96 # per iteration + max_iterations: 1000 # number of policy updates # logging save_interval: 50 # check for potential saves every this many iterations diff --git a/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/manipulation/lift/lift_cfg.py b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/manipulation/lift/lift_cfg.py index 736cbc06ec29..ed6c9b3b0cd0 100644 --- a/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/manipulation/lift/lift_cfg.py +++ b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/manipulation/lift/lift_cfg.py @@ -37,9 +37,11 @@ class ManipulationObjectCfg(RigidObjectCfg): pos=(0.4, 0.0, 0.075), rot=(1.0, 0.0, 0.0, 0.0), lin_vel=(0.0, 0.0, 0.0), ang_vel=(0.0, 0.0, 0.0) ) rigid_props = RigidObjectCfg.RigidBodyPropertiesCfg( + solver_position_iteration_count=8, + solver_velocity_iteration_count=0, max_angular_velocity=1000.0, max_linear_velocity=1000.0, - max_depenetration_velocity=10.0, + max_depenetration_velocity=5.0, disable_gravity=False, ) physics_material = RigidObjectCfg.PhysicsMaterialCfg( @@ -85,7 +87,7 @@ class ObjectInitialPoseCfg: orientation_cat: str = "default" # randomize position: "default", "uniform" # randomize position position_uniform_min = [0.25, -0.25, 0.25] # position (x,y,z) - position_uniform_max = [0.5, 0.25, 0.5] # position (x,y,z) + position_uniform_max = [0.5, 0.25, 0.25] # position (x,y,z) @configclass class ObjectDesiredPoseCfg: @@ -117,13 +119,21 @@ class PolicyCfg: # global group settings enable_corruption: bool = True # observation terms - arm_dof_pos_scaled = {"scale": 1.0, "noise": {"name": "uniform", "min": -0.01, "max": 0.01}} - tool_dof_pos_scaled = {"scale": 1.0} + # -- joint state + # arm_dof_pos_scaled = {"scale": 1.0, "noise": {"name": "uniform", "min": -0.01, "max": 0.01}} arm_dof_vel = {"scale": 0.5, "noise": {"name": "uniform", "min": -0.1, "max": 0.1}} - tool_positions = {} - object_positions = {} - object_desired_positions = {} - actions = {} + tool_dof_pos_scaled = {"scale": 1.0} + # -- end effector state + tool_positions = {"scale": 1.0} + tool_orientations = {"scale": 1.0} + # -- object state + object_relative_tool_positions = {"scale": 1.0} + # object_orientations = {"scale": 1.0} + # -- object desired state + object_desired_positions = {"scale": 1.0} + # -- previous action + arm_actions = {"scale": 1.0} + tool_actions = {"scale": 1.0} # global observation settings return_dict_obs_in_group = False @@ -137,14 +147,17 @@ class RewardsCfg: """Reward terms for the MDP.""" # robot-centric - reaching_object_position_l2 = {"weight": 0.0} - reaching_object_position_exp = {"weight": 2.5, "sigma": 0.25} - penalizing_robot_dof_velocity_l2 = {"weight": 1e-4} - penalizing_robot_dof_acceleration_l2 = {"weight": 1e-7} - penalizing_action_rate_l2 = {"weight": 1e-2} + # reaching_object_position_l2 = {"weight": 0.0} + # reaching_object_position_exp = {"weight": 2.5, "sigma": 0.25} + reaching_object_position_tanh = {"weight": 1.5, "sigma": 0.1} + # penalizing_arm_dof_velocity_l2 = {"weight": 1e-5} + # penalizing_tool_dof_velocity_l2 = {"weight": 1e-5} + # penalizing_robot_dof_acceleration_l2 = {"weight": 1e-7} + penalizing_arm_action_rate_l2 = {"weight": 1e-2} # object-centric - tracking_object_position_exp = {"weight": 2.5, "sigma": 0.5} - lifting_object_success = {"weight": 0.0, "threshold": 1e-3} + # tracking_object_position_exp = {"weight": 5.0, "sigma": 0.25, "threshold": 0.08} + tracking_object_position_tanh = {"weight": 5.0, "sigma": 0.1, "threshold": 0.08} + lifting_object_success = {"weight": 2.5, "threshold": 0.08} @configclass @@ -184,15 +197,18 @@ class LiftEnvCfg(IsaacEnvCfg): """Configuration for the Lift environment.""" # General Settings - env: EnvCfg = EnvCfg(num_envs=1024, env_spacing=2.5, episode_length_s=4.0) + env: EnvCfg = EnvCfg(num_envs=4096, env_spacing=2.5, episode_length_s=8.0) viewer: ViewerCfg = ViewerCfg(debug_vis=True, eye=(7.5, 7.5, 7.5), lookat=(0.0, 0.0, 0.0)) # Physics settings sim: SimCfg = SimCfg( - dt=1.0 / 60.0, + dt=0.01, substeps=1, physx=PhysxCfg( - gpu_found_lost_aggregate_pairs_capacity=512 * 1024, - gpu_total_aggregate_pairs_capacity=6 * 1024, + gpu_found_lost_aggregate_pairs_capacity=1024 * 1024 * 4, + gpu_total_aggregate_pairs_capacity=16 * 1024, + friction_correlation_distance=0.00625, + friction_offset_threshold=0.01, + bounce_threshold_velocity=0.2, ), ) diff --git a/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/manipulation/lift/lift_env.py b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/manipulation/lift/lift_env.py index 8fd00df8d6e4..27d3a1fe67db 100644 --- a/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/manipulation/lift/lift_env.py +++ b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/manipulation/lift/lift_env.py @@ -16,7 +16,7 @@ from omni.isaac.orbit.objects import RigidObject from omni.isaac.orbit.robots.single_arm import SingleArmManipulator from omni.isaac.orbit.utils.dict import class_to_dict -from omni.isaac.orbit.utils.math import random_orientation, sample_uniform, scale_transform +from omni.isaac.orbit.utils.math import quat_inv, quat_mul, random_orientation, sample_uniform, scale_transform from omni.isaac.orbit.utils.mdp import ObservationManager, RewardManager from omni.isaac.orbit_envs.isaac_env import IsaacEnv, VecEnvIndices, VecEnvObs @@ -60,7 +60,9 @@ def __init__(self, cfg: LiftEnvCfg = None, headless: bool = False): # compute the action space self.action_space = gym.spaces.Box(low=-1.0, high=1.0, shape=(self.num_actions,)) print("[INFO]: Completed setting up the environment...") + # Take an initial step to initialize the scene. + # This is required to compute quantities like Jacobians used in step(). self.sim.step() # -- fill up buffers self.object.update_buffers(self.dt) @@ -151,7 +153,7 @@ def _step_impl(self, actions: torch.Tensor): # offset actuator command with position offsets dof_pos_offset = self.robot.data.actuator_pos_offset self.robot_actions[:, : self.robot.arm_num_dof] -= dof_pos_offset[:, : self.robot.arm_num_dof] - # we assume last command is gripper action so don't change that + # we assume last command is tool action so don't change that self.robot_actions[:, -1] = self.actions[:, -1] elif self.cfg.control.control_type == "default": self.robot_actions[:] = self.actions @@ -252,6 +254,8 @@ def _initialize_views(self) -> None: self.robot_actions = torch.zeros((self.num_envs, self.robot.num_actions), device=self.device) # commands self.object_des_pose_w = torch.zeros((self.num_envs, 7), device=self.device) + # buffers + self.object_root_pose_ee = torch.zeros((self.num_envs, 7), device=self.device) # time-step = 0 self.object_init_pose_w = torch.zeros((self.num_envs, 7), device=self.device) @@ -376,17 +380,54 @@ def tool_positions(self, env: LiftEnv): """Current end-effector position of the arm.""" return env.robot.data.ee_state_w[:, :3] - env.envs_positions + def tool_orientations(self, env: LiftEnv): + """Current end-effector orientation of the arm.""" + # make the first element positive + quat_w = env.robot.data.ee_state_w[:, 3:7] + quat_w[quat_w[:, 0] < 0] *= -1 + return quat_w + def object_positions(self, env: LiftEnv): """Current object position.""" return env.object.data.root_pos_w - env.envs_positions + def object_orientations(self, env: LiftEnv): + """Current object orientation.""" + # make the first element positive + quat_w = env.object.data.root_quat_w + quat_w[quat_w[:, 0] < 0] *= -1 + return quat_w + + def object_relative_tool_positions(self, env: LiftEnv): + """Current object position w.r.t. end-effector frame.""" + return env.object.data.root_pos_w - env.robot.data.ee_state_w[:, :3] + + def object_relative_tool_orientations(self, env: LiftEnv): + """Current object orientation w.r.t. end-effector frame.""" + # compute the relative orientation + quat_ee = quat_mul(quat_inv(env.robot.data.ee_state_w[:, 3:7]), env.object.data.root_quat_w) + # make the first element positive + quat_ee[quat_ee[:, 0] < 0] *= -1 + return quat_ee + def object_desired_positions(self, env: LiftEnv): """Desired object position.""" return env.object_des_pose_w[:, 0:3] - env.envs_positions - def actions(self, env: LiftEnv): - """Last actions provided to env.""" - return env.actions + def object_desired_orientations(self, env: LiftEnv): + """Desired object orientation.""" + # make the first element positive + quat_w = env.object_des_pose_w[:, 3:7] + quat_w[quat_w[:, 0] < 0] *= -1 + return quat_w + + def arm_actions(self, env: LiftEnv): + """Last arm actions provided to env.""" + return env.actions[:, :-1] + + def tool_actions(self, env: LiftEnv): + """Last tool actions provided to env.""" + return torch.sign(env.actions[:, -1]).unsqueeze(1) class LiftRewardManager(RewardManager): @@ -398,27 +439,53 @@ def reaching_object_position_l2(self, env: LiftEnv): def reaching_object_position_exp(self, env: LiftEnv, sigma: float): """Penalize end-effector tracking position error using exp-kernel.""" - return torch.sum(torch.square(env.robot.data.ee_state_w[:, 0:3] - env.object.data.root_pos_w), dim=1) - - def penalizing_robot_dof_velocity_l2(self, env: LiftEnv): + error = torch.sum(torch.square(env.robot.data.ee_state_w[:, 0:3] - env.object.data.root_pos_w), dim=1) + return torch.exp(-error / sigma) + + def reaching_object_position_tanh(self, env: LiftEnv, sigma: float): + """Penalize tool sites tracking position error using tanh-kernel.""" + # distance of end-effector to the object: (num_envs,) + ee_distance = torch.norm(env.robot.data.ee_state_w[:, 0:3] - env.object.data.root_pos_w, dim=1) + # distance of the tool sites to the object: (num_envs, num_tool_sites) + object_root_pos = env.object.data.root_pos_w.unsqueeze(1) # (num_envs, 1, 3) + tool_sites_distance = torch.norm(env.robot.data.tool_sites_state_w[:, :, :3] - object_root_pos, dim=-1) + # average distance of the tool sites to the object: (num_envs,) + # note: we add the ee distance to the average to make sure that the ee is always closer to the object + num_tool_sites = tool_sites_distance.shape[1] + average_distance = (ee_distance + torch.sum(tool_sites_distance, dim=1)) / (num_tool_sites + 1) + + return 1 - torch.tanh(average_distance / sigma) + + def penalizing_arm_dof_velocity_l2(self, env: LiftEnv): """Penalize large movements of the robot arm.""" return -torch.sum(torch.square(env.robot.data.arm_dof_vel), dim=1) - def penalizing_robot_dof_acceleration_l2(self, env: LiftEnv): - """Penalize fast movements of the robot arm.""" - return -torch.sum(torch.square(env.robot.data.dof_acc), dim=1) + def penalizing_tool_dof_velocity_l2(self, env: LiftEnv): + """Penalize large movements of the robot tool.""" + return -torch.sum(torch.square(env.robot.data.tool_dof_vel), dim=1) + + def penalizing_arm_action_rate_l2(self, env: LiftEnv): + """Penalize large variations in action commands besides tool.""" + return -torch.sum(torch.square(env.actions[:, :-1] - env.previous_actions[:, :-1]), dim=1) - def penalizing_action_rate_l2(self, env: LiftEnv): - """Penalize large variations in action commands.""" - return -torch.sum(torch.square(env.actions - env.previous_actions), dim=1) + def penalizing_tool_action_l2(self, env: LiftEnv): + """Penalize large variations in action commands in the tool.""" + return -torch.square(env.actions[:, -1] - env.previous_actions[:, -1]) - def tracking_object_position_exp(self, env: LiftEnv, sigma: float): + def tracking_object_position_exp(self, env: LiftEnv, sigma: float, threshold: float): """Penalize tracking object position error using exp-kernel.""" - initial_error = torch.sum(torch.square(env.object_des_pose_w[:, 0:3] - env.object_init_pose_w[:, 0:3]), dim=1) - current_error = torch.sum(torch.square(env.object_des_pose_w[:, 0:3] - env.object.data.root_pos_w), dim=1) - return torch.exp(-current_error / sigma) - torch.exp(-initial_error / sigma) + # distance of the end-effector to the object: (num_envs,) + error = torch.sum(torch.square(env.object_des_pose_w[:, 0:3] - env.object.data.root_pos_w), dim=1) + # rewarded if the object is lifted above the threshold + return (env.object.data.root_pos_w[:, 2] > threshold) * torch.exp(-error / sigma) + + def tracking_object_position_tanh(self, env: LiftEnv, sigma: float, threshold: float): + """Penalize tracking object position error using tanh-kernel.""" + # distance of the end-effector to the object: (num_envs,) + distance = torch.norm(env.object_des_pose_w[:, 0:3] - env.object.data.root_pos_w, dim=1) + # rewarded if the object is lifted above the threshold + return (env.object.data.root_pos_w[:, 2] > threshold) * (1 - torch.tanh(distance / sigma)) def lifting_object_success(self, env: LiftEnv, threshold: float): """Sparse reward if object is lifted successfully.""" - error = torch.sum(torch.square(env.object.data.root_pos_w - env.object_des_pose_w[:, 0:3]), dim=1) - return torch.where(error < threshold, 1.0, 0.0) + return torch.where(env.object.data.root_pos_w[:, 2] > threshold, 1.0, 0.0) From 23453654945dc397ba65c3e6d7c5b9725523b4f0 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Mon, 6 Mar 2023 16:39:45 +0100 Subject: [PATCH 07/12] enable self collisions in franka --- .../omni.isaac.orbit/omni/isaac/orbit/robots/config/franka.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/config/franka.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/config/franka.py index 4ff839fe931c..cf462b462335 100644 --- a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/config/franka.py +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/config/franka.py @@ -54,6 +54,9 @@ contact_offset=0.005, rest_offset=0.0, ), + articulation_props=SingleArmManipulatorCfg.ArticulationRootPropertiesCfg( + enable_self_collisions=True, + ), actuator_groups={ "panda_shoulder": ActuatorGroupCfg( dof_names=["panda_joint[1-4]"], From 228e1e50d91f3c97bd5a767fd1c4dcba628367aa Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Mon, 6 Mar 2023 20:05:53 +0100 Subject: [PATCH 08/12] adds units for physics-related params --- .../articulated/articulated_object_cfg.py | 7 +++-- .../orbit/objects/rigid/rigid_object_cfg.py | 7 +++-- .../omni/isaac/orbit/robots/robot_base_cfg.py | 7 +++-- .../omni/isaac/orbit/utils/kit.py | 28 +++++++++---------- 4 files changed, 26 insertions(+), 23 deletions(-) diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/articulated/articulated_object_cfg.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/articulated/articulated_object_cfg.py index 048beee653fa..1e1a6564985e 100644 --- a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/articulated/articulated_object_cfg.py +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/articulated/articulated_object_cfg.py @@ -33,11 +33,12 @@ class RigidBodyPropertiesCfg: angular_damping: Optional[float] = None """Angular damping coefficient.""" max_linear_velocity: Optional[float] = 1000.0 - """Maximum linear velocity for rigid bodies. Defaults to 1000.0.""" + """Maximum linear velocity for rigid bodies (in m/s). Defaults to 1000.0.""" max_angular_velocity: Optional[float] = 1000.0 - """Maximum angular velocity for rigid bodies. Defaults to 1000.0.""" + """Maximum angular velocity for rigid bodies (in rad/s). Defaults to 1000.0.""" max_depenetration_velocity: Optional[float] = 10.0 - """Maximum depenetration velocity permitted to be introduced by the solver. Defaults to 10.0.""" + """Maximum depenetration velocity permitted to be introduced by the solver (in m/s). + Defaults to 10.0.""" disable_gravity: Optional[bool] = False """Disable gravity for the actor. Defaults to False.""" retain_accelerations: Optional[bool] = None diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/rigid/rigid_object_cfg.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/rigid/rigid_object_cfg.py index 7348e6e31331..61b9f6716b9a 100644 --- a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/rigid/rigid_object_cfg.py +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/rigid/rigid_object_cfg.py @@ -31,11 +31,12 @@ class RigidBodyPropertiesCfg: solver_velocity_iteration_count: Optional[int] = None """Solver position iteration counts for the body.""" max_linear_velocity: Optional[float] = 1000.0 - """Maximum linear velocity for rigid bodies. Defaults to 1000.0.""" + """Maximum linear velocity for rigid bodies (in m/s). Defaults to 1000.0.""" max_angular_velocity: Optional[float] = 1000.0 - """Maximum angular velocity for rigid bodies. Defaults to 1000.0.""" + """Maximum angular velocity for rigid bodies (in rad/s). Defaults to 1000.0.""" max_depenetration_velocity: Optional[float] = 10.0 - """Maximum depenetration velocity permitted to be introduced by the solver. Defaults to 10.0.""" + """Maximum depenetration velocity permitted to be introduced by the solver (in m/s). + Defaults to 10.0.""" disable_gravity: Optional[bool] = False """Disable gravity for the actor. Defaults to False.""" diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/robot_base_cfg.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/robot_base_cfg.py index 990b1d6f97a0..3cd69f019149 100644 --- a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/robot_base_cfg.py +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/robot_base_cfg.py @@ -33,11 +33,12 @@ class RigidBodyPropertiesCfg: angular_damping: Optional[float] = None """Angular damping coefficient.""" max_linear_velocity: Optional[float] = 1000.0 - """Maximum linear velocity for rigid bodies. Defaults to 1000.0.""" + """Maximum linear velocity for rigid bodies (in m/s). Defaults to 1000.0.""" max_angular_velocity: Optional[float] = 1000.0 - """Maximum angular velocity for rigid bodies. Defaults to 1000.0.""" + """Maximum angular velocity for rigid bodies (in rad/s). Defaults to 1000.0.""" max_depenetration_velocity: Optional[float] = 10.0 - """Maximum depenetration velocity permitted to be introduced by the solver. Defaults to 10.0.""" + """Maximum depenetration velocity permitted to be introduced by the solver (in m/s). + Defaults to 10.0.""" disable_gravity: Optional[bool] = False """Disable gravity for the actor. Defaults to False.""" retain_accelerations: Optional[bool] = None diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/utils/kit.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/utils/kit.py index b0cc0ee28d35..2ce2c5aaa2b6 100644 --- a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/utils/kit.py +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/utils/kit.py @@ -322,14 +322,14 @@ def set_rigid_body_properties( solver_velocity_iteration_count (Optional[int]): Solver velocity iteration counts for the body. linear_damping (Optional[float]): Linear damping coefficient. angular_damping (Optional[float]): Angular damping coefficient. - max_linear_velocity (Optional[float]): Max allowable linear velocity for rigid body. - max_angular_velocity (Optional[float]): Max allowable angular velocity for rigid body. + max_linear_velocity (Optional[float]): Max allowable linear velocity for rigid body (in m/s). + max_angular_velocity (Optional[float]): Max allowable angular velocity for rigid body (in rad/s). sleep_threshold (Optional[float]): Mass-normalized kinetic energy threshold below which an actor may go to sleep. stabilization_threshold (Optional[float]): Mass-normalized kinetic energy threshold below which an actor may participate in stabilization. max_depenetration_velocity (Optional[float]): The maximum depenetration velocity permitted to - be introduced by the solver. + be introduced by the solver (in m/s). max_contact_impulse (Optional[float]): The limit on the impulse that may be applied at a contact. enable_gyroscopic_forces (Optional[bool]): Enables computation of gyroscopic forces on the rigid body. @@ -408,12 +408,12 @@ def set_collision_properties( Args: prim_path (str): The prim path of parent. collision_enabled (Optional[bool], optional): Whether to enable/disable collider. - contact_offset (Optional[float], optional): Contact offset of a collision shape. - rest_offset (Optional[float], optional): Rest offset of a collision shape. + contact_offset (Optional[float], optional): Contact offset of a collision shape (in m). + rest_offset (Optional[float], optional): Rest offset of a collision shape (in m). torsional_patch_radius (Optional[float], optional): Defines the radius of the contact patch - used to apply torsional friction. + used to apply torsional friction (in m). min_torsional_patch_radius (Optional[float], optional): Defines the minimum radius of the - contact patch used to apply torsional friction. + contact patch used to apply torsional friction (in m). Raises: ValueError: When no collision schema found at specified prim path. @@ -536,14 +536,14 @@ def set_nested_rigid_body_properties(prim_path: str, **kwargs): solver_velocity_iteration_count (Optional[int]): Solver velocity iteration counts for the body. linear_damping (Optional[float]): Linear damping coefficient. angular_damping (Optional[float]): Angular damping coefficient. - max_linear_velocity (Optional[float]): Max allowable linear velocity for rigid body. - max_angular_velocity (Optional[float]): Max allowable angular velocity for rigid body. + max_linear_velocity (Optional[float]): Max allowable linear velocity for rigid body (in m/s). + max_angular_velocity (Optional[float]): Max allowable angular velocity for rigid body (in rad/s). sleep_threshold (Optional[float]): Mass-normalized kinetic energy threshold below which an actor may go to sleep. stabilization_threshold (Optional[float]): Mass-normalized kinetic energy threshold below which an actor may participate in stabilization. max_depenetration_velocity (Optional[float]): The maximum depenetration velocity permitted to - be introduced by the solver. + be introduced by the solver (in m/s). max_contact_impulse (Optional[float]): The limit on the impulse that may be applied at a contact. enable_gyroscopic_forces (Optional[bool]): Enables computation of gyroscopic forces on the rigid body. @@ -575,12 +575,12 @@ def set_nested_collision_properties(prim_path: str, **kwargs): Keyword Args: collision_enabled (Optional[bool], optional): Whether to enable/disable collider. - contact_offset (Optional[float], optional): Contact offset of a collision shape. - rest_offset (Optional[float], optional): Rest offset of a collision shape. + contact_offset (Optional[float], optional): Contact offset of a collision shape (in m). + rest_offset (Optional[float], optional): Rest offset of a collision shape (in m). torsional_patch_radius (Optional[float], optional): Defines the radius of the contact patch - used to apply torsional friction. + used to apply torsional friction (in m). min_torsional_patch_radius (Optional[float], optional): Defines the minimum radius of the - contact patch used to apply torsional friction. + contact patch used to apply torsional friction (in m). """ # get USD prim prim = prim_utils.get_prim_at_path(prim_path) From 22ce9a6cdc4fead7fdd35e687a3d38d8357f9cc6 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Mon, 6 Mar 2023 22:00:38 +0100 Subject: [PATCH 09/12] tunes ppo for lift to work --- .../data/rsl_rl/lift_ppo.yaml | 2 +- .../orbit_envs/manipulation/lift/lift_cfg.py | 35 +++++++++++-------- .../orbit_envs/manipulation/lift/lift_env.py | 12 +++++-- 3 files changed, 31 insertions(+), 18 deletions(-) diff --git a/source/extensions/omni.isaac.orbit_envs/data/rsl_rl/lift_ppo.yaml b/source/extensions/omni.isaac.orbit_envs/data/rsl_rl/lift_ppo.yaml index ca814415cc66..013b9da48d98 100644 --- a/source/extensions/omni.isaac.orbit_envs/data/rsl_rl/lift_ppo.yaml +++ b/source/extensions/omni.isaac.orbit_envs/data/rsl_rl/lift_ppo.yaml @@ -33,7 +33,7 @@ runner: policy_class_name: "ActorCritic" algorithm_class_name: "PPO" num_steps_per_env: 96 # per iteration - max_iterations: 1000 # number of policy updates + max_iterations: 700 # number of policy updates # logging save_interval: 50 # check for potential saves every this many iterations diff --git a/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/manipulation/lift/lift_cfg.py b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/manipulation/lift/lift_cfg.py index ed6c9b3b0cd0..ba912d2c5602 100644 --- a/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/manipulation/lift/lift_cfg.py +++ b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/manipulation/lift/lift_cfg.py @@ -37,8 +37,8 @@ class ManipulationObjectCfg(RigidObjectCfg): pos=(0.4, 0.0, 0.075), rot=(1.0, 0.0, 0.0, 0.0), lin_vel=(0.0, 0.0, 0.0), ang_vel=(0.0, 0.0, 0.0) ) rigid_props = RigidObjectCfg.RigidBodyPropertiesCfg( - solver_position_iteration_count=8, - solver_velocity_iteration_count=0, + solver_position_iteration_count=16, + solver_velocity_iteration_count=1, max_angular_velocity=1000.0, max_linear_velocity=1000.0, max_depenetration_velocity=5.0, @@ -86,8 +86,8 @@ class ObjectInitialPoseCfg: position_cat: str = "default" # randomize position: "default", "uniform" orientation_cat: str = "default" # randomize position: "default", "uniform" # randomize position - position_uniform_min = [0.25, -0.25, 0.25] # position (x,y,z) - position_uniform_max = [0.5, 0.25, 0.25] # position (x,y,z) + position_uniform_min = [0.4, -0.25, 0.075] # position (x,y,z) + position_uniform_max = [0.6, 0.25, 0.075] # position (x,y,z) @configclass class ObjectDesiredPoseCfg: @@ -98,8 +98,8 @@ class ObjectDesiredPoseCfg: orientation_cat: str = "default" # randomize position: "default", "uniform" # randomize position position_default = [0.5, 0.0, 0.5] # position default (x,y,z) - position_uniform_min = [0.25, -0.25, 0.25] # position (x,y,z) - position_uniform_max = [0.5, 0.25, 0.5] # position (x,y,z) + position_uniform_min = [0.4, -0.25, 0.25] # position (x,y,z) + position_uniform_max = [0.6, 0.25, 0.5] # position (x,y,z) # randomize orientation orientation_default = [1.0, 0.0, 0.0, 0.0] # orientation default @@ -120,15 +120,18 @@ class PolicyCfg: enable_corruption: bool = True # observation terms # -- joint state - # arm_dof_pos_scaled = {"scale": 1.0, "noise": {"name": "uniform", "min": -0.01, "max": 0.01}} - arm_dof_vel = {"scale": 0.5, "noise": {"name": "uniform", "min": -0.1, "max": 0.1}} + arm_dof_pos = {"scale": 1.0} + # arm_dof_pos_scaled = {"scale": 1.0} + # arm_dof_vel = {"scale": 0.5, "noise": {"name": "uniform", "min": -0.01, "max": 0.01}} tool_dof_pos_scaled = {"scale": 1.0} # -- end effector state tool_positions = {"scale": 1.0} tool_orientations = {"scale": 1.0} # -- object state - object_relative_tool_positions = {"scale": 1.0} + # object_positions = {"scale": 1.0} # object_orientations = {"scale": 1.0} + object_relative_tool_positions = {"scale": 1.0} + # object_relative_tool_orientations = {"scale": 1.0} # -- object desired state object_desired_positions = {"scale": 1.0} # -- previous action @@ -146,18 +149,20 @@ class PolicyCfg: class RewardsCfg: """Reward terms for the MDP.""" - # robot-centric + # -- robot-centric # reaching_object_position_l2 = {"weight": 0.0} # reaching_object_position_exp = {"weight": 2.5, "sigma": 0.25} - reaching_object_position_tanh = {"weight": 1.5, "sigma": 0.1} + reaching_object_position_tanh = {"weight": 2.5, "sigma": 0.1} # penalizing_arm_dof_velocity_l2 = {"weight": 1e-5} # penalizing_tool_dof_velocity_l2 = {"weight": 1e-5} # penalizing_robot_dof_acceleration_l2 = {"weight": 1e-7} + # -- action-centric penalizing_arm_action_rate_l2 = {"weight": 1e-2} - # object-centric + # penalizing_tool_action_l2 = {"weight": 1e-2} + # -- object-centric # tracking_object_position_exp = {"weight": 5.0, "sigma": 0.25, "threshold": 0.08} - tracking_object_position_tanh = {"weight": 5.0, "sigma": 0.1, "threshold": 0.08} - lifting_object_success = {"weight": 2.5, "threshold": 0.08} + tracking_object_position_tanh = {"weight": 5.0, "sigma": 0.2, "threshold": 0.08} + lifting_object_success = {"weight": 3.5, "threshold": 0.08} @configclass @@ -197,7 +202,7 @@ class LiftEnvCfg(IsaacEnvCfg): """Configuration for the Lift environment.""" # General Settings - env: EnvCfg = EnvCfg(num_envs=4096, env_spacing=2.5, episode_length_s=8.0) + env: EnvCfg = EnvCfg(num_envs=4096, env_spacing=2.5, episode_length_s=5.0) viewer: ViewerCfg = ViewerCfg(debug_vis=True, eye=(7.5, 7.5, 7.5), lookat=(0.0, 0.0, 0.0)) # Physics settings sim: SimCfg = SimCfg( diff --git a/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/manipulation/lift/lift_env.py b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/manipulation/lift/lift_env.py index 27d3a1fe67db..c63e5ca77cb5 100644 --- a/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/manipulation/lift/lift_env.py +++ b/source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/manipulation/lift/lift_env.py @@ -356,6 +356,10 @@ def _randomize_object_desired_pose(self, env_ids: torch.Tensor, cfg: Randomizati class LiftObservationManager(ObservationManager): """Reward manager for single-arm reaching environment.""" + def arm_dof_pos(self, env: LiftEnv): + """DOF positions for the arm.""" + return env.robot.data.arm_dof_pos + def arm_dof_pos_scaled(self, env: LiftEnv): """DOF positions for the arm normalized to its max and min ranges.""" return scale_transform( @@ -427,6 +431,10 @@ def arm_actions(self, env: LiftEnv): def tool_actions(self, env: LiftEnv): """Last tool actions provided to env.""" + return env.actions[:, -1].unsqueeze(1) + + def tool_actions_bool(self, env: LiftEnv): + """Last tool actions transformed to a boolean command.""" return torch.sign(env.actions[:, -1]).unsqueeze(1) @@ -469,8 +477,8 @@ def penalizing_arm_action_rate_l2(self, env: LiftEnv): return -torch.sum(torch.square(env.actions[:, :-1] - env.previous_actions[:, :-1]), dim=1) def penalizing_tool_action_l2(self, env: LiftEnv): - """Penalize large variations in action commands in the tool.""" - return -torch.square(env.actions[:, -1] - env.previous_actions[:, -1]) + """Penalize large values in action commands for the tool.""" + return -torch.square(env.actions[:, -1]) def tracking_object_position_exp(self, env: LiftEnv, sigma: float, threshold: float): """Penalize tracking object position error using exp-kernel.""" From b796c15fa525fc3ea1181d74f00422488e13a511 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Mon, 6 Mar 2023 23:32:03 +0100 Subject: [PATCH 10/12] adds collision props to articulated and rigid objects --- .../objects/articulated/articulated_object.py | 2 ++ .../articulated/articulated_object_cfg.py | 17 +++++++++++++++++ .../isaac/orbit/objects/rigid/rigid_object.py | 2 ++ .../orbit/objects/rigid/rigid_object_cfg.py | 17 +++++++++++++++++ 4 files changed, 38 insertions(+) diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/articulated/articulated_object.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/articulated/articulated_object.py index 7d94bec61445..c2421a385703 100644 --- a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/articulated/articulated_object.py +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/articulated/articulated_object.py @@ -133,6 +133,8 @@ def spawn(self, prim_path: str, translation: Sequence[float] = None, orientation # TODO: What if prim already exists in the stage and spawn isn't called? # apply rigid body properties kit_utils.set_nested_rigid_body_properties(prim_path, **self.cfg.rigid_props.to_dict()) + # apply collision properties + kit_utils.set_nested_collision_properties(prim_path, **self.cfg.collision_props.to_dict()) # articulation root settings kit_utils.set_articulation_properties(prim_path, **self.cfg.articulation_props.to_dict()) diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/articulated/articulated_object_cfg.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/articulated/articulated_object_cfg.py index 1e1a6564985e..e4aba75ffd95 100644 --- a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/articulated/articulated_object_cfg.py +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/articulated/articulated_object_cfg.py @@ -44,6 +44,21 @@ class RigidBodyPropertiesCfg: retain_accelerations: Optional[bool] = None """Carries over forces/accelerations over sub-steps.""" + @configclass + class CollisionPropertiesCfg: + """Properties to apply to all collisions in the articulation.""" + + collision_enabled: Optional[bool] = None + """Whether to enable or disable collisions.""" + contact_offset: Optional[float] = None + """Contact offset for the collision shape.""" + rest_offset: Optional[float] = None + """Rest offset for the collision shape.""" + torsional_patch_radius: Optional[float] = None + """Radius of the contact patch for applying torsional friction.""" + min_torsional_patch_radius: Optional[float] = None + """Minimum radius of the contact patch for applying torsional friction.""" + @configclass class ArticulationRootPropertiesCfg: """Properties to apply to articulation.""" @@ -86,5 +101,7 @@ class InitialStateCfg: """Initial state of the articulated object.""" rigid_props: RigidBodyPropertiesCfg = RigidBodyPropertiesCfg() """Properties to apply to all rigid bodies in the articulation.""" + collision_props: CollisionPropertiesCfg = CollisionPropertiesCfg() + """Properties to apply to all collisions in the articulation.""" articulation_props: ArticulationRootPropertiesCfg = ArticulationRootPropertiesCfg() """Properties to apply to articulation.""" diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/rigid/rigid_object.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/rigid/rigid_object.py index b58a7e36ec5c..8c127a09b8ae 100644 --- a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/rigid/rigid_object.py +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/rigid/rigid_object.py @@ -111,6 +111,8 @@ def spawn(self, prim_path: str, translation: Sequence[float] = None, orientation RigidPrim(prim_path=prim_path) # -- set rigid body properties kit_utils.set_nested_rigid_body_properties(prim_path, **self.cfg.rigid_props.to_dict()) + # apply collision properties + kit_utils.set_nested_collision_properties(prim_path, **self.cfg.collision_props.to_dict()) # create physics material if self.cfg.physics_material is not None: # -- resolve material path diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/rigid/rigid_object_cfg.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/rigid/rigid_object_cfg.py index 61b9f6716b9a..ea766789fba4 100644 --- a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/rigid/rigid_object_cfg.py +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/objects/rigid/rigid_object_cfg.py @@ -40,6 +40,21 @@ class RigidBodyPropertiesCfg: disable_gravity: Optional[bool] = False """Disable gravity for the actor. Defaults to False.""" + @configclass + class CollisionPropertiesCfg: + """Properties to apply to all collisions in the articulation.""" + + collision_enabled: Optional[bool] = None + """Whether to enable or disable collisions.""" + contact_offset: Optional[float] = None + """Contact offset for the collision shape.""" + rest_offset: Optional[float] = None + """Rest offset for the collision shape.""" + torsional_patch_radius: Optional[float] = None + """Radius of the contact patch for applying torsional friction.""" + min_torsional_patch_radius: Optional[float] = None + """Minimum radius of the contact patch for applying torsional friction.""" + @configclass class PhysicsMaterialCfg: """Physics material applied to the rigid object.""" @@ -84,6 +99,8 @@ class InitialStateCfg: """Initial state of the rigid object.""" rigid_props: RigidBodyPropertiesCfg = RigidBodyPropertiesCfg() """Properties to apply to all rigid bodies in the object.""" + collision_props: CollisionPropertiesCfg = CollisionPropertiesCfg() + """Properties to apply to all collisions in the articulation.""" physics_material: Optional[PhysicsMaterialCfg] = PhysicsMaterialCfg() """Settings for the physics material to apply to the rigid object. From 18ec00842f7465c132688027a94ce4bc563f9892 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Mon, 6 Mar 2023 23:42:28 +0100 Subject: [PATCH 11/12] updates changelog --- .../omni.isaac.orbit/config/extension.toml | 2 +- .../omni.isaac.orbit/docs/CHANGELOG.rst | 16 ++++++++++++++++ .../omni.isaac.orbit_envs/config/extension.toml | 2 +- .../omni.isaac.orbit_envs/docs/CHANGELOG.rst | 8 ++++++++ 4 files changed, 26 insertions(+), 2 deletions(-) diff --git a/source/extensions/omni.isaac.orbit/config/extension.toml b/source/extensions/omni.isaac.orbit/config/extension.toml index 31d2515236b0..3aa4ecd6e2db 100644 --- a/source/extensions/omni.isaac.orbit/config/extension.toml +++ b/source/extensions/omni.isaac.orbit/config/extension.toml @@ -1,7 +1,7 @@ [package] # Note: Semantic Versioning is used: https://semver.org/ -version = "0.2.4" +version = "0.2.5" # Description title = "ORBIT framework for Robot Learning" diff --git a/source/extensions/omni.isaac.orbit/docs/CHANGELOG.rst b/source/extensions/omni.isaac.orbit/docs/CHANGELOG.rst index d4b1e929e1eb..2fd0d5905679 100644 --- a/source/extensions/omni.isaac.orbit/docs/CHANGELOG.rst +++ b/source/extensions/omni.isaac.orbit/docs/CHANGELOG.rst @@ -1,6 +1,22 @@ Changelog --------- +0.2.5 (2023-03-06) +~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added the :class:`CollisionPropertiesCfg` to rigid/articulated object and robot base classes. +* Added the :class:`PhysicsMaterialCfg` to the :class:`SingleArm` class for tool sites. + +Changed +^^^^^^^ + +* Changed the default control mode of the :obj:`PANDA_HAND_MIMIC_GROUP_CFG` to be from ``"v_abs"`` to ``"p_abs"``. + Using velocity control for the mimic group can cause the hand to move in a jerky manner. + + 0.2.4 (2023-03-04) ~~~~~~~~~~~~~~~~~~ diff --git a/source/extensions/omni.isaac.orbit_envs/config/extension.toml b/source/extensions/omni.isaac.orbit_envs/config/extension.toml index 164a8f78ef82..a73541a2e3b7 100644 --- a/source/extensions/omni.isaac.orbit_envs/config/extension.toml +++ b/source/extensions/omni.isaac.orbit_envs/config/extension.toml @@ -1,7 +1,7 @@ [package] # Note: Semantic Versioning is used: https://semver.org/ -version = "0.2.2" +version = "0.2.3" # Description title = "ORBIT Environments" diff --git a/source/extensions/omni.isaac.orbit_envs/docs/CHANGELOG.rst b/source/extensions/omni.isaac.orbit_envs/docs/CHANGELOG.rst index a7c13476dad7..c601e7d922a6 100644 --- a/source/extensions/omni.isaac.orbit_envs/docs/CHANGELOG.rst +++ b/source/extensions/omni.isaac.orbit_envs/docs/CHANGELOG.rst @@ -1,6 +1,14 @@ Changelog --------- +0.2.3 (2023-03-06) +~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Tuned the observations and rewards for ``Isaac-Lift-Franka-v0`` environment. + 0.2.2 (2023-03-04) ~~~~~~~~~~~~~~~~~~ From 418eea196a2522cd2668a051085e4e2c075a1de1 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Thu, 16 Mar 2023 13:04:50 +0100 Subject: [PATCH 12/12] adds warp-based state machine --- .../environments/state_machine/play_lift.py | 282 ++++++++++++++++++ 1 file changed, 282 insertions(+) create mode 100644 source/standalone/environments/state_machine/play_lift.py diff --git a/source/standalone/environments/state_machine/play_lift.py b/source/standalone/environments/state_machine/play_lift.py new file mode 100644 index 000000000000..351b9bbda5b9 --- /dev/null +++ b/source/standalone/environments/state_machine/play_lift.py @@ -0,0 +1,282 @@ +""" +Script to run an environment with a pick and lift state machine. + +The state machine is implemented in the kernel function `infer_state_machine`. +It uses the `warp` library to run the state machine in parallel on the GPU. + +Usage: + python play_lift.py --num_envs 128 +""" + +"""Launch Omniverse Toolkit first.""" + +import argparse + +from omni.isaac.kit import SimulationApp + +# add argparse arguments +parser = argparse.ArgumentParser("Welcome to Orbit: Omniverse Robotics Environments!") +parser.add_argument("--headless", action="store_true", default=False, help="Force display off at all times.") +parser.add_argument("--cpu", action="store_true", default=False, help="Use CPU pipeline.") +parser.add_argument("--num_envs", type=int, default=None, help="Number of environments to simulate.") +args_cli = parser.parse_args() + +# launch the simulator +config = {"headless": args_cli.headless} +simulation_app = SimulationApp(config) + +"""Rest everything else.""" + +import gym +import torch +from enum import Enum +from typing import Sequence, Union + +import warp as wp + +from omni.isaac.orbit.utils.timer import Timer + +import omni.isaac.orbit_envs # noqa: F401 +from omni.isaac.orbit_envs.utils.parse_cfg import parse_env_cfg + +# initialize warp +wp.init() + + +class GripperState(Enum): + """States for the gripper.""" + + OPEN = wp.constant(1.0) + CLOSE = wp.constant(-1.0) + + +class PickSmState(Enum): + """States for the pick state machine.""" + + REST = wp.constant(0) + APPROACH_ABOVE_OBJECT = wp.constant(1) + APPROACH_OBJECT = wp.constant(2) + GRASP_OBJECT = wp.constant(3) + LIFT_OBJECT = wp.constant(4) + DROP_OBJECT = wp.constant(5) + + +class PickSmWaitTime(Enum): + """Additional wait times (in s) for states for before switching.""" + + REST = wp.constant(1.0) + APPROACH_ABOVE_OBJECT = wp.constant(0.5) + APPROACH_OBJECT = wp.constant(0.3) + GRASP_OBJECT = wp.constant(0.3) + LIFT_OBJECT = wp.constant(2.0) + DROP_OBJECT = wp.constant(0.2) + + +@wp.kernel +def infer_state_machine( + dt: wp.array(dtype=wp.float32), + sm_state: wp.array(dtype=wp.int32), + sm_wait_time: wp.array(dtype=wp.float32), + ee_pose: wp.array(dtype=wp.transform), + object_pose: wp.array(dtype=wp.transform), + des_object_pose: wp.array(dtype=wp.transform), + des_ee_pose: wp.array(dtype=wp.transform), + gripper_state: wp.array(dtype=wp.float32), + offset: wp.array(dtype=wp.transform), +): + # retrieve thread id + tid = wp.tid() + # retrieve state machine state + state = sm_state[tid] + # decide next state + if state == PickSmState.REST.value: + des_ee_pose[tid] = ee_pose[tid] + gripper_state[tid] = GripperState.OPEN.value + # wait for a while + if sm_wait_time[tid] >= PickSmWaitTime.REST.value: + # move to next state and reset wait time + sm_state[tid] = PickSmState.APPROACH_ABOVE_OBJECT.value + sm_wait_time[tid] = 0.0 + elif state == PickSmState.APPROACH_ABOVE_OBJECT.value: + des_ee_pose[tid] = object_pose[tid] + # TODO: This is causing issues. + # des_ee_pose[tid] = wp.transform_multiply(des_ee_pose[tid], offset[tid]) + gripper_state[tid] = GripperState.OPEN.value + # TODO: error between current and desired ee pose below threshold + # wait for a while + if sm_wait_time[tid] >= PickSmWaitTime.APPROACH_OBJECT.value: + # move to next state and reset wait time + sm_state[tid] = PickSmState.APPROACH_OBJECT.value + sm_wait_time[tid] = 0.0 + elif state == PickSmState.APPROACH_OBJECT.value: + des_ee_pose[tid] = object_pose[tid] + gripper_state[tid] = GripperState.OPEN.value + # TODO: error between current and desired ee pose below threshold + # wait for a while + if sm_wait_time[tid] >= PickSmWaitTime.APPROACH_OBJECT.value: + # move to next state and reset wait time + sm_state[tid] = PickSmState.GRASP_OBJECT.value + sm_wait_time[tid] = 0.0 + elif state == PickSmState.GRASP_OBJECT.value: + des_ee_pose[tid] = object_pose[tid] + gripper_state[tid] = GripperState.CLOSE.value + # wait for a while + if sm_wait_time[tid] >= PickSmWaitTime.GRASP_OBJECT.value: + # move to next state and reset wait time + sm_state[tid] = PickSmState.LIFT_OBJECT.value + sm_wait_time[tid] = 0.0 + elif state == PickSmState.LIFT_OBJECT.value: + des_ee_pose[tid] = des_object_pose[tid] + gripper_state[tid] = GripperState.CLOSE.value + # TODO: error between current and desired ee pose below threshold + # wait for a while + if sm_wait_time[tid] >= PickSmWaitTime.LIFT_OBJECT.value: + # move to next state and reset wait time + sm_state[tid] = PickSmState.DROP_OBJECT.value + sm_wait_time[tid] = 0.0 + elif state == PickSmState.DROP_OBJECT.value: + des_ee_pose[tid] = des_object_pose[tid] + gripper_state[tid] = GripperState.OPEN.value + # wait for a while + if sm_wait_time[tid] >= PickSmWaitTime.DROP_OBJECT.value: + # move to next state and reset wait time + sm_state[tid] = PickSmState.REST.value + sm_wait_time[tid] = 0.0 + # increment wait time + sm_wait_time[tid] = sm_wait_time[tid] + dt[tid] + + +class PickAndLiftSm: + """A simple state machine in a robot's task space to pick and lift an object. + + The state machine is implemented as a warp kernel. It takes in the current state of + the robot's end-effector and the object, and outputs the desired state of the robot's + end-effector and the gripper. The state machine is implemented as a finite state + machine with the following states: + + 1. REST: The robot is at rest. + 2. APPROACH_ABOVE_OBJECT: The robot moves above the object. + 3. APPROACH_OBJECT: The robot moves to the object. + 4. GRASP_OBJECT: The robot grasps the object. + 5. LIFT_OBJECT: The robot lifts the object. + 6. DROP_OBJECT: The robot drops the object. + """ + + def __init__(self, dt: float, num_envs: int, device: Union[torch.device, str] = "cpu"): + """Initialize the state machine. + + Args: + dt (float): The environment time step. + num_envs (int): The number of environments to simulate. + device (Union[torch.device, str], optional): The device to run the state machine on. + """ + # save parameters + self.dt = dt + self.num_envs = num_envs + self.device = device + # initialize state machine + self.sm_dt = torch.full((self.num_envs,), self.dt, dtype=torch.float32, device=self.device) + self.sm_state = torch.full((self.num_envs,), 0, dtype=torch.int32, device=self.device) + self.sm_wait_time = torch.zeros((self.num_envs,), dtype=torch.float32, device=self.device) + # desired state + self.des_ee_pose = torch.zeros((self.num_envs, 7), dtype=torch.float32, device=self.device) + self.des_gripper_state = torch.full((self.num_envs,), 0, dtype=torch.float32, device=self.device) + # approach above object offset + self.offset = torch.zeros((self.num_envs, 7), dtype=torch.float32, device=self.device) + self.offset[:, 2] = 0.1 + # convert to warp + self.sm_dt_wp = wp.from_torch(self.sm_dt, wp.float32) + self.sm_state_wp = wp.from_torch(self.sm_state, wp.int32) + self.sm_wait_time_wp = wp.from_torch(self.sm_wait_time, wp.float32) + self.des_ee_pose_wp = wp.from_torch(self.des_ee_pose, wp.transform) + self.des_gripper_state_wp = wp.from_torch(self.des_gripper_state, wp.float32) + self.offset_wp = wp.from_torch(self.offset, wp.transform) + + def reset_idx(self, env_ids: Sequence[int] = None): + """Reset the state machine.""" + if env_ids is None: + env_ids = ... + self.sm_state[env_ids] = 0 + self.sm_wait_time[env_ids] = 0.0 + + def compute(self, ee_pose: torch.Tensor, object_pose: torch.Tensor, des_object_pose: torch.Tensor): + """Compute the desired state of the robot's end-effector and the gripper.""" + # convert to warp + ee_pose_wp = wp.from_torch(ee_pose.contiguous(), wp.transform) + object_pose_wp = wp.from_torch(object_pose.contiguous(), wp.transform) + des_object_pose_wp = wp.from_torch(des_object_pose.contiguous(), wp.transform) + # run state machine + wp.launch( + kernel=infer_state_machine, + dim=self.num_envs, + inputs=[ + self.sm_dt_wp, + self.sm_state_wp, + self.sm_wait_time_wp, + ee_pose_wp, + object_pose_wp, + des_object_pose_wp, + self.des_ee_pose_wp, + self.des_gripper_state_wp, + self.offset_wp, + ], + ) + wp.synchronize() + # convert to torch + return torch.cat([self.des_ee_pose, self.des_gripper_state.unsqueeze(-1)], dim=-1) + + +def main(): + # parse configuration + env_cfg = parse_env_cfg("Isaac-Lift-Franka-v0", use_gpu=not args_cli.cpu, num_envs=args_cli.num_envs) + # -- control configuration + env_cfg.control.control_type = "inverse_kinematics" + env_cfg.control.inverse_kinematics.command_type = "position_abs" + # -- randomization configuration + env_cfg.randomization.object_initial_pose.position_cat = "uniform" + env_cfg.randomization.object_desired_pose.position_cat = "uniform" + # -- termination configuration + env_cfg.terminations.episode_timeout = False + # -- robot configuration + env_cfg.robot.robot_type = "franka" + # create environment + env = gym.make("Isaac-Lift-Franka-v0", cfg=env_cfg, headless=args_cli.headless) + + # create action buffers + actions = torch.zeros((env.num_envs, env.action_space.shape[0]), device=env.device) + # create state machine + pick_sm = PickAndLiftSm(env.dt, env.num_envs, env.device) + + # reset environment + env.reset() + + # run state machine + for _ in range(10000): + # step environment + dones = env.step(actions)[-2] + + # observations + ee_pose = env.robot.data.ee_state_w[:, :7].clone() + object_pose = env.object.data.root_state_w[:, :7].clone() + des_object_pose = env.object_des_pose_w.clone() + # transform from world to base frames + ee_pose[:, :3] -= env.robot.data.root_pos_w + object_pose[:, :3] -= env.robot.data.root_pos_w + des_object_pose[:, :3] -= env.robot.data.root_pos_w + # advance state machine + with Timer("state machine"): + sm_actions = pick_sm.compute(ee_pose, object_pose, des_object_pose) + + # set actions for IK with positions + actions[:, :3] = sm_actions[:, :3] + actions[:, -1] = sm_actions[:, -1] + # reset state machine + if dones.any(): + pick_sm.reset_idx(dones.nonzero(as_tuple=False).squeeze(-1)) + + +if __name__ == "__main__": + # run main function + main() + # close simulation + simulation_app.close()