Running collision aware ik on only mesh crashes/doesn't avoid the mesh

[blooop]

If it’s not a bug, please use discussions: https://github.com/NVlabs/curobo/discussions

Please provide the below information in addition to your issue:

1. cuRobo installation mode (choose from [python, isaac sim, docker python, docker isaac sim]): docker python
2. python version: 3.10
3. Isaac Sim version (if using): none

---

Issue Details

I am trying to run collision aware IK on some meshes, but the robot goes through the mesh. Also I get run time errors if my world config only contains a mesh and no cuboids. I think this must be because the collision config is only looking at primitives and ignoring the mesh, but after trying some of the WorldConfig conversion functions I was not able to make any headway with this.

This the script I used to check if the mesh collision is behaving as expected

import torch
import numpy as np
from curobo.geom.types import WorldConfig
from curobo.types.base import TensorDeviceType
from curobo.types.math import Pose
from curobo.types.robot import RobotConfig
from curobo.util_file import get_robot_configs_path, join_path, load_yaml
from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig

tensor_args = TensorDeviceType()
robot_file = "franka.yml"
robot_cfg = RobotConfig.from_dict(
    load_yaml(join_path(get_robot_configs_path(), robot_file))["robot_cfg"]
)

def setup_ik_and_world(world_config_dict: dict):
    if world_config_dict is not None:
        world_cfg = WorldConfig.from_dict(world_config_dict)
        # world_cfg = WorldConfig.create_collision_support_world(world_cfg) #doesn't change anything
        # world_cfg = WorldConfig.create_merged_mesh_world(world_cfg) #doesn't change anything
        # world_cfg = WorldConfig.create_mesh_world(world_cfg) # fails
    else:
        world_cfg = None

    ik_config = IKSolverConfig.load_from_robot_config(
        robot_cfg,
        world_cfg,
        rotation_threshold=0.05,
        position_threshold=0.005,
        num_seeds=100,
        self_collision_check=True,
        self_collision_opt=True,
        tensor_args=tensor_args,
        use_cuda_graph=True,
    )
    return IKSolver(ik_config)

def world_config_to_mesh(world_config_dict: dict, mesh_name: str):
    world_cfg = (
        WorldConfig.from_dict(world_config_dict)
        if world_config_dict is not None
        else None
    )
    world_cfg.save_world_as_mesh(mesh_name)

def run_ik(ik_solver: IKSolver):
    # sample 3 positions.  One point to the left, one central and one right
    pos = torch.tensor(np.array([[0.3, -0.2, 0.5], [0.3, 0.0, 0.5], [0.5, 0.2, 0.5]]))

    # all the same orientation
    quat = torch.tensor(
        np.array([[1.0, 0.0, 0.0, 0.0], [1.0, 0.0, 0.0, 0.0], [1.0, 0.0, 0.0, 0.0]])
    )
    goal = Pose(tensor_args.to_device(pos), tensor_args.to_device(quat))
    result = ik_solver.solve_batch(goal)
    return result.success.cpu().numpy().squeeze()

# set up an empty world and confirm that all points are reachable
ik_solver = setup_ik_and_world(world_config_dict=None)
np.testing.assert_equal(run_ik(ik_solver), [True, True, True])  # passes

# Add a single small cuboid that will block the middle result but leave the two side results free
world_config_cube = {
    "cuboid": {
        "cube_1": {"dims": [0.05, 0.05, 0.05], "pose": [0.3, 0.0, 0.5, 1, 0, 0, 0]},
    }
}
ik_solver = setup_ik_and_world(world_config_dict=world_config_cube)
np.testing.assert_equal(run_ik(ik_solver), [True, False, True])  # passes

# save the world config as a mesh and confirm that ik gives the same results
world_config_to_mesh(world_config_cube, "single_cube_as_mesh_in_space.obj")

# I can't load just the mesh by itself as I get a ValueError, so add a dummy cuboid.  I suspect this is the source of the errors but can't find a fix
# raise ValueError("Primitive Collision has no obstacles")
# ValueError: Primitive Collision has no obstacles

world_config_cube = {
    "cuboid": {
        "cube_1": {
            "dims": [0.025, 0.025, 0.025],
            # Set the cube far away
            "pose": [0.3, 0.0, 100.5, 1, 0, 0, 0],
        },
    },
    "mesh": {
        "scene": {  # offset is already in the mesh so load at the origin
            "pose": [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0],
            "file_path": "single_cube_as_mesh_in_space.obj",
        }
    },
}

ik_solver = setup_ik_and_world(world_config_dict=world_config_cube)
np.testing.assert_equal(run_ik(ik_solver), [True, False, True])  # fails, returns [True, True, True]

[balakumar-s]

By default, we only use a primitive collision checker. You can change this to a mesh+primitive checker with https://github.com/NVlabs/curobo/blob/c09d94908d8ad52f557ed59195c1ceb2d0434d65/examples/isaac_sim/ik_reachability.py#L194

I think it makes sense to make mesh as default as it doesn't add any additional dependencies and might not cause a huge slowdown in compute time when no meshes are found.

[blooop]

Thanks for the lighting response! That works now as expected.

Just to confirm, if I want to use cuboid primitives as well as a mesh I would need to set the collision type = mesh and convert the world_cfg to only mesh with WorldConfig.create_merged_mesh_world(world_cfg)?

[blooop]

After some more testing it seems setting the mesh collision checker still works with cuboids without any conversion to meshes

[balakumar-s]

Mesh inherits cuboids. Here is a note on collision checkers https://curobo.org/source/getting_started/2c_world_collision.html