Surface gripper bug

2022-10-20 07:46:47 [56,019ms] [Error] [omni.isaac.dynamic_control.plugin] Failed to register rigid body at ‘/World/defaultGroundPlane/GroundPlane/CollisionPlane’
2022-10-20 07:46:47 [56,019ms] [Error] [omni.isaac.dynamic_control.plugin] DcGetRigidBodyPath: Invalid or expired body handle
2022-10-20 07:46:47 [56,020ms] [Error] [omni.isaac.dynamic_control.plugin] DcGetRigidBodyPose: Invalid or expired body handle
2022-10-20 07:46:47 [56,020ms] [Error] [omni.isaac.dynamic_control.plugin] DcSetRigidBodyDisableGravity: Invalid or expired body handle
2022-10-20 07:46:47 [56,020ms] [Error] [omni.physx.plugin] PhysX error: PxD6JointCreate: actors must be different, FILE /buildAgent/work/99bede84aa0a52c2/source/physxextensions/src/ExtD6Joint.cpp, LINE 45


after some digging this is due to suction gripper try to close.
and there is nothing there, it just try to grasp the ground plane

another bug. when suction gripper is trying to grasp part of the robot itself I will see this error:

2022-10-21 06:07:13 [38,135ms] [Error] [omni.physx.plugin] PhysX error: PxD6JointCreate: actors must be different, FILE /buildAgent/work/99bede84aa0a52c2/source/physxextensions/src/ExtD6Joint.cpp, LINE 45

However, there is really no way I can predict what the ML model is trying to grasp.



From the error it looks like your offset point is contained inside the collision mesh for the gripper, You need to place it out so it can find other elements when you call close.
as an aside, it should not give you an error if it tries to close with the ground plane or any other static mesh. I’ll check that to make sure it behaves as expected.

PhysX error: PxD6JointCreate: actors must be different, FILE /buildAgent/work/99bede84aa0a52c2/source/physxextensions/src/ExtD6Joint.cpp, LINE 45
For this error.
what’s happinging is the gripper offset collides with the robot itself. The gripper is trying to grasp the robot.
the error does not happen if it’s just grasping a cube.

it’s from the predition of a ML model. so I cannot control where to grasp.


I am using the default UR10 robot without changing the gripper offset.

Here is a screen shot for the crash to happen:

ok my temporatory solution is to do raycasting from the tip of the gripper (0.5 cm forward of the gripper tip) to the gripper forward direction for 3.5 cm. If the raycasting returns trying to grasp a robot, then I will not execute close gripper command.

But I feel this is something that the surface gripper is supposed to handle.

Hi - Apologies for the delay in the response. Are you still seeing this issue with the latest Isaac Sim 2022.2.1 release?

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
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).
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__(
    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

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.
        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
        carb.log_error("Direction specified for the surface gripper doesn't exist")
    virtual_gripper_props.offset = tr
    virtual_gripper = Surface_Gripper(self._dc_interface)
    self._virtual_gripper = virtual_gripper
    if self._default_state is None:
        self._default_state = not self.is_closed()

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 (
    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/
    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"]:
        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)
    if not self.is_closed() and ok_to_grasp:
        print("try close")
    if not self.is_closed():
        carb.log_warn("gripper didn't close successfully")

def open(self) -> None:
    """Applies actions to the articulation that opens the gripper (ex: to release an object held).
    result =
    if not result:
        carb.log_warn("gripper didn't open successfully")


def update(self) -> None:

def is_closed(self) -> bool:
    return self._virtual_gripper.is_closed()

def set_translate(self, value: float) -> None:
    self._translate = value

def set_direction(self, value: float) -> None:
    self._direction = value

def set_force_limit(self, value: float) -> None:
    self._force_limit = value

def set_torque_limit(self, value: float) -> None:
    self._torque_limit = value

def set_default_state(self, opened: bool):
    """Sets the default state of the gripper
        opened (bool): True if the surface gripper should start in an opened state. False otherwise.
    self._default_state = opened

def get_default_state(self) -> dict:
    """Gets the default state of the gripper
        dict: key is "opened" and value would be true if the surface gripper should start in an opened state. False otherwise.

Thanks for the feedback! I’ll file an internal bug to track and fix this behavior