I have not tested on 2022.2.1. But the bug still exists in 2022.2.0
I will try again after my paper submission:
Here is the workaround I used:
Copyright (c) 2021, NVIDIA CORPORATION. All rights reserved.
NVIDIA CORPORATION and its licensors retain all intellectual property
and proprietary rights in and to this software, related documentation
and any modifications thereto. Any use, reproduction, disclosure or
distribution of this software and related documentation without an express
license agreement from NVIDIA CORPORATION is strictly prohibited.
from omni.isaac.dynamic_control import _dynamic_control
from omni.isaac.manipulators.grippers.gripper import Gripper
from omni.isaac.surface_gripper._surface_gripper import Surface_Gripper
from omni.isaac.surface_gripper._surface_gripper import Surface_Gripper_Properties
from omni.isaac.core.utils.types import ArticulationAction
import numpy as np
import carb
import omni.kit.app
from omni.physx.scripts.physicsUtils import *
from omni.isaac.core.prims.rigid_prim import RigidPrim
class SurfaceGripper(Gripper):
“”“Provides high level functions to set/ get properties and actions of a surface gripper
(a suction cup for example).
Args:
end_effector_prim_path (str): prim path of the Prim that corresponds to the gripper root/ end effector.
translate (float, optional): description. Defaults to 0.
direction (str, optional): description. Defaults to “x”.
grip_threshold (float, optional): description. Defaults to 0.01.
force_limit (float, optional): description. Defaults to 1.0e6.
torque_limit (float, optional): description. Defaults to 1.0e4.
bend_angle (float, optional): description. Defaults to np.pi/24.
kp (float, optional): description. Defaults to 1.0e2.
kd (float, optional): description. Defaults to 1.0e2.
disable_gravity (bool, optional): description. Defaults to True.
“””
def __init__(
self,
end_effector_prim_path: str,
translate: float = 0,
direction: str = "x",
grip_threshold: float = 0.02,
force_limit: float = 1.0e6,
torque_limit: float = 1.0e4,
bend_angle: float = np.pi / 24,
kp: float = 1.0e2,
kd: float = 1.0e2,
disable_gravity: bool = True,
) -> None:
Gripper.__init__(self, end_effector_prim_path=end_effector_prim_path)
self._dc_interface = _dynamic_control.acquire_dynamic_control_interface()
self._translate = translate
self._direction = direction
self._grip_threshold = grip_threshold
self._force_limit = force_limit
self._torque_limit = torque_limit
self._bend_angle = bend_angle
self._kp = kp
self._kd = kd
self._disable_gravity = disable_gravity
self._virtual_gripper = None
self._articulation_num_dofs = None,
self.end_effector_prim_path = end_effector_prim_path
return
def initialize(
self, physics_sim_view: omni.physics.tensors.SimulationView = None, articulation_num_dofs: int = None
) -> None:
"""Create a physics simulation view if not passed and creates a rigid prim view using physX tensor api.
This needs to be called after each hard reset (i.e stop + play on the timeline) before interacting with any
of the functions of this class.
Args:
physics_sim_view (omni.physics.tensors.SimulationView, optional): current physics simulation view. Defaults to None
articulation_num_dofs (int, optional): num of dofs of the Articulation. Defaults to None.
"""
Gripper.initialize(self, physics_sim_view=physics_sim_view)
self._articulation_num_dofs = articulation_num_dofs
virtual_gripper_props = Surface_Gripper_Properties()
virtual_gripper_props.parentPath = self._end_effector_prim_path
virtual_gripper_props.d6JointPath = virtual_gripper_props.parentPath + "/d6FixedJoint"
virtual_gripper_props.gripThreshold = self._grip_threshold
virtual_gripper_props.forceLimit = self._force_limit
virtual_gripper_props.torqueLimit = self._torque_limit
virtual_gripper_props.bendAngle = self._bend_angle
virtual_gripper_props.stiffness = self._kp
virtual_gripper_props.damping = self._kd
virtual_gripper_props.disableGravity = self._disable_gravity
tr = _dynamic_control.Transform()
if self._direction == "x":
tr.p.x = self._translate
elif self._direction == "y":
tr.p.y = self._translate
elif self._direction == "z":
tr.p.z = self._translate
else:
carb.log_error("Direction specified for the surface gripper doesn't exist")
virtual_gripper_props.offset = tr
virtual_gripper = Surface_Gripper(self._dc_interface)
virtual_gripper.initialize(virtual_gripper_props)
self._virtual_gripper = virtual_gripper
if self._default_state is None:
self._default_state = not self.is_closed()
return
def close(self) -> None:
"""Applies actions to the articulation that closes the gripper (ex: to hold an object).
"""
from omni.isaac.core.utils.stage import (
get_current_stage
)
from omni.isaac.core.utils.prims import get_prim_parent, get_prim_at_path, set_prim_property, get_prim_property
# from omni.physx import get_physx_interface, get_physx_simulation_interface
robot_name = self.end_effector_prim_path.split('/')[2]
from omni.isaac.core.utils.rotations import quat_to_rot_matrix
matrix = quat_to_rot_matrix(RigidPrim.get_world_pose(self)[1])
forward = matrix[0:3, 0]
forward = forward/np.linalg.norm(forward)
stage = get_current_stage()
gripper_tip = list(filter(lambda x: 'gripper_tip' in x.GetPath().pathString.lower() \
and robot_name in x.GetPath().pathString \
, list(stage.TraverseAll())))[0]
mat = omni.usd.utils.get_world_transform_matrix(gripper_tip)
gripper_position = np.array(mat.ExtractTranslation())
gripper_position += 0.003*forward
ok_to_grasp = True
# file:///home/nikepupu/.local/share/ov/pkg/isaac_sim-2022.1.1/kit/extsPhysics/omni.physx.demos-1.4.15-5.1/omni/physxdemos/scenes/RaycastsDemo.py
from omni.physx import get_physx_scene_query_interface
hitInfo = get_physx_scene_query_interface().raycast_closest(gripper_position, forward, 0.04)
if hitInfo["hit"]:
print(hitInfo)
if 'UR' in hitInfo['collision']:
ok_to_grasp = False
if 'ground' in hitInfo['collision'].lower():
ok_to_grasp = False
# COLOR_RED = 0xffff0000
# COLOR_GREEN = 0xff00ff00
# COLOR_BLUE = 0xff0000ff
# COLOR_YELLOW = 0xffffff00
# from omni.debugdraw import get_debug_draw_interface
# self._debugDraw = get_debug_draw_interface()
# def draw_frame(debugDraw, pos, scale=1.0):
# frameXAxis = carb.Float3(pos[0] + scale, pos[1], pos[2])
# frameYAxis = carb.Float3(pos[0], pos[1] + scale, pos[2])
# frameZAxis = carb.Float3(pos[0], pos[1], pos[2] + scale)
# debugDraw.draw_line(pos, COLOR_RED, frameXAxis, COLOR_RED)
# debugDraw.draw_line(pos, COLOR_GREEN, frameYAxis, COLOR_GREEN)
# debugDraw.draw_line(pos, COLOR_BLUE, frameZAxis, COLOR_BLUE)
# hitPos = hitInfo["position"]
# hitNormal = hitInfo["normal"]
# impactPos = carb.Float3(hitPos[0], hitPos[1], hitPos[2])
# impactNormalEnd = carb.Float3(hitPos[0] + hitNormal[0], hitPos[1] + hitNormal[1], hitPos[2] + hitNormal[2])
# self._debugDraw.draw_line(gripper_position, COLOR_YELLOW, impactPos, COLOR_YELLOW)
# self._debugDraw.draw_line(impactPos, 0xff00ffff, impactNormalEnd, 0xff00ffff)
# draw_frame(self._debugDraw, impactPos)
# while True:
# self._debugDraw.draw_line(gripper_position, COLOR_YELLOW, impactPos, COLOR_YELLOW)
# omni.kit.app.get_app().update()
if not self.is_closed() and ok_to_grasp:
print("try close")
self._virtual_gripper.close()
if not self.is_closed():
carb.log_warn("gripper didn't close successfully")
return
def open(self) -> None:
"""Applies actions to the articulation that opens the gripper (ex: to release an object held).
"""
result = self._virtual_gripper.open()
if not result:
carb.log_warn("gripper didn't open successfully")
return
def update(self) -> None:
self._virtual_gripper.update()
return
def is_closed(self) -> bool:
return self._virtual_gripper.is_closed()
def set_translate(self, value: float) -> None:
self._translate = value
return
def set_direction(self, value: float) -> None:
self._direction = value
return
def set_force_limit(self, value: float) -> None:
self._force_limit = value
return
def set_torque_limit(self, value: float) -> None:
self._torque_limit = value
return
def set_default_state(self, opened: bool):
"""Sets the default state of the gripper
Args:
opened (bool): True if the surface gripper should start in an opened state. False otherwise.
"""
self._default_state = opened
return
def get_default_state(self) -> dict:
"""Gets the default state of the gripper
Returns:
dict: key is "opened" and value would be true if the surface gripper should start in an opened state. False otherwise.
"""
