How to get distance to the deformable body?

I want to set distance senor to robot. I used raycast_closest(origin, rayDir, distance) and get distance to the rigid body, but I can’t get distance to the deformable body.

Frequently Used Python Snippets — isaacsim 2022.2.1 documentation (nvidia.com)

On the other hand, I found another API which called ray_cast().
Core [omni.isaac.core] — isaac_sim 2022.2.1-beta.29 documentation (nvidia.com)
My code is here.

        s_pos = np.array([-1, 0, 0.1]) 
        s_ori = np.array([1.0, 0.0, 0.0]) 
        s_off = np.array([0, 0, 0])
        s_max = 100
        
        anpn = ray_cast(s_pos, s_ori, s_off, s_max)
        
        print(anpn)

But, I have the following error.

[Error] [asyncio] Task exception was never retrieved
future: <Task finished coro=<BaseSampleExtension._on_load_world.<locals>._on_load_world_async() done, defined at c:\users\rr011\appdata\local\ov\pkg\isaac_sim-2022.2.1\exts\omni.isaac.examples\omni\isaac\examples\base_sample\base_sample_extension.py:150> exception=ArgumentError('Python argument types in\n    Quatf.__init__(Quatf, float, float, float)\ndid not match C++ signature:\n    __init__(struct _object * __ptr64, class pxrInternal_v0_20__pxrReserved__::GfQuatd)\n    __init__(struct _object * __ptr64, float real, float i, float j, float k)\n    __init__(struct _object * __ptr64, float real, class pxrInternal_v0_20__pxrReserved__::GfVec3f imaginary)\n    __init__(struct _object * __ptr64, float real)\n    __init__(struct _object * __ptr64, class pxrInternal_v0_20__pxrReserved__::GfQuatf)\n    __init__(class boost::python::api::object)')>
Traceback (most recent call last):
  File "c:\users\rr011\appdata\local\ov\pkg\isaac_sim-2022.2.1\exts\omni.isaac.examples\omni\isaac\examples\base_sample\base_sample_extension.py", line 151, in _on_load_world_async
    await self._sample.load_world_async()
  File "c:\users\rr011\appdata\local\ov\pkg\isaac_sim-2022.2.1\exts\omni.isaac.examples\omni\isaac\examples\base_sample\base_sample.py", line 43, in load_world_async
    self.setup_scene()
  File "c:\users\rr011\appdata\local\ov\pkg\isaac_sim-2022.2.1\exts\omni.isaac.examples\omni\isaac\examples\hello_world\hello_world.py", line 110, in setup_scene
    anpn = ray_cast(s_pos, s_ori, s_off, s_max)
  File "c:\users\rr011\appdata\local\ov\pkg\isaac_sim-2022.2.1\exts\omni.isaac.core\omni\isaac\core\utils\collisions.py", line 41, in ray_cast
    input_tr.SetRotateOnly(Gf.Quatf(*orientation.tolist()))
Boost.Python.ArgumentError: Python argument types in
    Quatf.__init__(Quatf, float, float, float)
did not match C++ signature:
    __init__(struct _object * __ptr64, class pxrInternal_v0_20__pxrReserved__::GfQuatd)
    __init__(struct _object * __ptr64, float real, float i, float j, float k)
    __init__(struct _object * __ptr64, float real, class pxrInternal_v0_20__pxrReserved__::GfVec3f imaginary)
    __init__(struct _object * __ptr64, float real)
    __init__(struct _object * __ptr64, class pxrInternal_v0_20__pxrReserved__::GfQuatf)
    __init__(class boost::python::api::object)

What should I do to get distance to the deformable body?
Please tell me the solution.

Hi @Yuya_t - The error message you’re seeing is due to a mismatch in the argument types for the Gf.Quatf function. This function is used to create a quaternion, which represents a rotation in 3D space. The error message indicates that the function expects either a single float (for a quaternion representing no rotation), four floats (representing the real part and the i, j, k imaginary parts of the quaternion), or a GfVec3f object (representing the imaginary part of the quaternion).

In your code, you’re passing in three floats to Gf.Quatf, which is not a valid set of arguments for this function. If you’re trying to create a quaternion from Euler angles, you can use the Gf.Rotation class instead:

rotation = Gf.Rotation(Gf.Vec3d(1.0, 0.0, 0.0), angle)
quat = rotation.GetQuat()

Here, angle is the angle of rotation in degrees.

As for getting the distance to a deformable body, you can use the ray_cast function in the same way as you would for a rigid body. The function should return the distance to the closest point on the deformable body that the ray intersects. If the ray does not intersect the deformable body, the function will return a large value (e.g., inf).

If you’re not getting the expected results, make sure that the deformable body is correctly set up and that the ray is being cast in the right direction. You might also want to check if the deformable body is included in the collision group that the ray_cast function is checking against.

Thank you for reply @rthaker !

I changed the program, and new error has occurred.
My new code is this.

        s_pos = np.array([-1, 0, 0.25]) 
        
        s_off = np.array([0, 0, 0])
        s_max = 100
        
        angle = 30.0
        s_rot = Gf.Rotation(Gf.Vec3d(0.0, 0.0, 1.0), angle)
        s_quat = s_rot.GetQuat()
        
        sensor = ray_cast(s_pos, s_quat, s_off, s_max)
        
        print(sensor)

This is new error message.

 [Error] [carb.physx.python] AttributeError: 'Quatd' object has no attribute 'tolist'

At:
  c:\users\rr011\appdata\local\ov\pkg\isaac_sim-2022.2.1\exts\omni.isaac.core\omni\isaac\core\utils\collisions.py(41): ray_cast
  c:\users\rr011\appdata\local\ov\pkg\isaac_sim-2022.2.1\exts\omni.isaac.examples\omni\isaac\examples\user_examples\hello_world.py(319): robot_sim

Any idea why I got this error?

And also I want to check if the deformable body is included in the collision group that the ray_cast function is checking against, please tell me how to do.

Hi @Yuya_t - The error message you’re seeing is due to the fact that the ‘Quatd’ object does not have a ‘tolist’ method. The ‘Quatd’ object is a quaternion, and it seems like the ‘ray_cast’ function is expecting a list or array-like object, not a quaternion.

To fix this, you can convert the quaternion to a list or array. Here’s how you can do it:

s_quat_list = [s_quat.GetReal(), s_quat.GetImaginary()[0], s_quat.GetImaginary()[1], s_quat.GetImaginary()[2]]
sensor = ray_cast(s_pos, s_quat_list, s_off, s_max)

As for checking if the deformable body is included in the collision group that the ray_cast function is checking against, you would need to check the documentation or the implementation of the ‘ray_cast’ function. If the function does not provide a way to specify the collision group, you might need to modify the function or create a new one that allows you to specify the collision group.

If the ‘ray_cast’ function returns information about the object that was hit, you can check if the hit object is the deformable body. If the function does not return this information, you might need to modify the function or create a new one that does.

I add your opinion to the program, but get the error that seems to be the same.

[Error] [carb.physx.python] AttributeError: 'list' object has no attribute 'tolist'
        s_pos = np.array([-1, 0, 0.1]) 
        
        s_off = np.array([0, 0, 0])
        s_max = 100
        
        angle = 30.0
        s_rot = Gf.Rotation(Gf.Vec3d(0.0, 0.0, 1.0), angle)
        s_quat = s_rot.GetQuat()
        
        s_quat_list = [s_quat.GetReal(), s_quat.GetImaginary()[0], s_quat.GetImaginary()[1], s_quat.GetImaginary()[2]]
        sensor = ray_cast(s_pos, s_quat_list, s_off, s_max)
        print(sensor)

Why I got the same error?
However, if you prepare quaternions directly instead of Euler angles, the error did not occur.

        s_ori = np.array([1, 0, 0, 0]) 
        sensor = ray_cast(s_pos, s_ori, s_off, s_max)

Also, is it possible to visualize ray_cast() like Lidar?

Hi @Yuya_t - The error message you’re seeing is due to the fact that the ray_cast function is expecting a numpy array, not a list. When you convert the quaternion to a list and pass it to the ray_cast function, it’s still a list, not a numpy array. You can fix this by converting the list to a numpy array:

s_quat_array = np.array([s_quat.GetReal(), s_quat.GetImaginary()[0], s_quat.GetImaginary()[1], s_quat.GetImaginary()[2]])
sensor = ray_cast(s_pos, s_quat_array, s_off, s_max)

As for visualizing ray_cast like Lidar, it depends on the capabilities of the ray_cast function and the rendering system you’re using. If the ray_cast function returns the points that the ray intersects, you can use those points to create a visualization. For example, you could create a line or a series of points at the intersection points. If the ray_cast function doesn’t return the intersection points, you would need to modify the function or create a new one that does.

If you’re using a rendering system that supports it, you could also create a visualization of the ray itself, not just the intersection points. This would give you a visualization similar to Lidar. However, this would likely require a more complex implementation and a deeper understanding of the rendering system.

Thanks to you the error is resolved, but it doesn’t seem to be able to detect deformable objects.

I confirmed that a rigid body can be detected with ray_cast, but it could not be detected even if a deformable object was placed at the same position. The sensor’s ray is being cast in the right direction.

Is there something wrong with my flexible settings? Below is my flexible settings.

#make deformable cube
        result, path = omni.kit.commands.execute("CreateMeshPrimCommand", prim_type="Cube")
        
        omni.kit.commands.execute("MovePrim",  path_from=path,  path_to="/World/deformable_object")
        omni.usd.get_context().get_selection().set_selected_prim_paths([], False)
        # Get the prim
        cube_prim = stage.GetPrimAtPath("/World/deformable_object")
        
        xform = UsdGeom.Xformable(cube_prim)
        transform = xform.AddTransformOp()
        mat = Gf.Matrix4d()
        xform.AddTranslateOp().Set(Gf.Vec3d(0.0,0.0,1.0))   
        mat.SetRotateOnly(Gf.Rotation(Gf.Vec3d(0,0,0), 290))  
        xform.AddScaleOp().Set(Gf.Vec3f(0.5, 0.5, 0.5))
        transform.Set(mat)
        
        """ 
        #set rigid body
        utils.setRigidBody(cube_prim, "convexHull", False)

        mass_api = UsdPhysics.MassAPI.Apply(cube_prim)
        mass_api.CreateMassAttr(10)

        mass_api.CreateDensityAttr(1000)        
        #"""
        
        #"""
        simulation_resolution = 10

        # Apply PhysxDeformableBodyAPI and PhysxCollisionAPI to skin mesh and set parameter to default values
        success = deformableUtils.add_physx_deformable_body(
            stage,
            xform.GetPath(),
            collision_simplification=True,
            simulation_hexahedral_resolution=simulation_resolution,
            self_collision=False,
        )

        # Create a deformable body material and set it on the deformable body
        deformable_material_path = omni.usd.get_stage_next_free_path(stage, "/World/deformable_object_material", True)
        deformableUtils.add_deformable_body_material(
            stage,
            deformable_material_path,
            youngs_modulus=10000.0,
            poissons_ratio=0.49,
            damping_scale=0.0,
            dynamic_friction=0.5,
        )
        physicsUtils.add_physics_material_to_prim(stage, xform.GetPrim(), deformable_material_path) 
        #"""

If there is no problem with flexible settings, I think the ray_cast collision group is the cause, but there was no information about that in the document below.

Core [omni.isaac.core] — isaac_sim 2022.2.1-beta.29 documentation (nvidia.com)

What should I do to solve the probrem?