Debugging self-collision?

[peter-mitrano-bg]

What would be an efficient way to debug self-collision? I've set up my spheres in isaac sim (a bit slow and painful 😬 ), but now I'm seeing self-collision issues when trying to collision check. I know I need to probably tweak the yml, but without being able to visualize the self-collisions I fear this will take forever. Are there existing gui tools for helping fix issues like this? Thanks!

[balakumar-s]

One way to check is by setting the robot at a configuration that you know is collision free and then use this class's self collision check:

curobo/src/curobo/wrap/model/robot_world.py

Line 231 in d6e600c

def get_self_collision_distance(self, x_sph: torch.Tensor) -> torch.Tensor:

to know if self collision is zero.

Here is how to create an instance of RobotWorld:

curobo/examples/collision_check_example.py

Line 49 in d6e600c

d = curobo_fn.get_collision_distance(q_sph)

Then remove link from collision_link_names one by one until you find the link that's causing the problem.

[peter-mitrano-bg]

Hm ok, yea that could work, but it's a bit slow and painful since that function doesn't seem to tell you which links are causing the problem, even though the underlying code must somewhere know this information. I am going to investigate going a little deeper into curobo to get this information. This kind of self-collision information should probably be generated automatically, like the moveit_setup_assistant does, by sampling many configurations, and excluding any pair of links that is always found to be in self-collision

[balakumar-s]

Here is a gradient vector of dimensions [n_links,4] that contains which link is in collision:

curobo/src/curobo/curobolib/geom.py

Line 117 in d6e600c

ctx.save_for_backward(out_vec)

It however does not say which link pairs are in collision as we compute that inside a cuda kernel and only write out the largest collision for each link:

curobo/src/curobo/curobolib/cpp/self_collision_kernel.cu

Line 309 in d6e600c

__global__ void self_collision_distance_kernel7(

[peter-mitrano-bg]

I see -- so the information is being thrown away already inside the kernel. I wonder if a debugging version of the kernel could be written that returns all the information? For example, it could return a matrix coll where coll[i,j] indicates that spheres i and j are in have distance < 0?

I've set up a lot of different robots in different robotics software, and setting up proper collision/self-collision is always a pain point, primarily because insufficient debugging/visualization tools are written. MoveIt Setup Assistant is actually one of the better tools I've used. So I do think it would be worth adding this information somewhere, so that better tools could be built around it to make setting up new robots easier!

[peter-mitrano-bg]

In case others find this useful, this is what I came up with -- I realized by setting state.robot_spheres.requires_grad = True the CC kernel will fill out rollout.robot_self_collision_constraint._sparse_sphere_idx, which I can then use to see which sphere have collision. Here's the full script I'm using:

import time

import torch
from yaml.parser import ParserError
from yaml.scanner import ScannerError

from curobo.geom.sdf.utils import create_collision_checker
from curobo.geom.sdf.world import WorldCollisionConfig, CollisionCheckerType

# CuRobo
from curobo.rollout.arm_base import ArmBase, ArmBaseConfig
from curobo.types.base import TensorDeviceType
from curobo.types.robot import RobotConfig
from curobo.util.logger import setup_curobo_logger
from curobo.util_file import (
    get_robot_configs_path,
    get_world_configs_path,
    join_path,
    load_yaml,
    get_task_configs_path,
)

torch.backends.cudnn.benchmark = True
torch.backends.cuda.matmul.allow_tf32 = True
torch.backends.cudnn.allow_tf32 = True


def main():
    setup_curobo_logger("warn")
    robot_file = "my_robot.yml"
    world_file = "collision_test.yml"

    tensor_args = TensorDeviceType()

    while True:
        try:
            robot_data = load_yaml(join_path(get_robot_configs_path(), robot_file))["robot_cfg"]
        except (ParserError, ScannerError):
            print("Error parsing robot file")
            continue
        robot_data["kinematics"]["collision_sphere_buffer"] = 0.0
        robot_cfg = RobotConfig.from_dict(robot_data)
        ik_config_data = load_yaml(join_path(get_task_configs_path(), "gradient_ik.yml"))
        base_config_data = load_yaml(join_path(get_task_configs_path(), "base_cfg.yml"))
        base_config_data["world_collision_checker_cfg"]["checker_type"] = CollisionCheckerType.MESH
        world_model = load_yaml(join_path(get_world_configs_path(), world_file))

        world_coll_cfg = WorldCollisionConfig.load_from_dict(
            base_config_data["world_collision_checker_cfg"], world_model, tensor_args
        )
        world_coll_checker = create_collision_checker(world_coll_cfg)

        cfg = ArmBaseConfig.from_dict(
            robot_cfg=robot_cfg,
            model_data_dict=ik_config_data["model"],
            cost_data_dict=base_config_data["cost"],
            constraint_data_dict=base_config_data["constraint"],
            convergence_data_dict=base_config_data["convergence"],
            world_coll_checker=world_coll_checker,
            tensor_args=tensor_args,
        )

        rollout = ArmBase(cfg)

        kin_cfg = robot_cfg.kinematics.kinematics_config
        samples = torch.zeros([100, kin_cfg.n_dof], **tensor_args.as_torch_dict())
        state = rollout.dynamics_model.forward(rollout.start_state, samples.unsqueeze(1))
        state.robot_spheres.requires_grad = True
        rollout.robot_self_collision_constraint.forward(state.robot_spheres)
        colliding_sphere_indicators = (
            rollout.robot_self_collision_constraint._sparse_sphere_idx.squeeze(1)
        )

        offenses = colliding_sphere_indicators.sum(dim=0)
        offending_indices = torch.argwhere(offenses).squeeze()

        if offenses.sum() == 0:
            continue

        offending_link_indices = kin_cfg.link_sphere_idx_map[offending_indices]

        link_idx_to_name_map = {v: k for k, v in kin_cfg.link_name_to_idx_map.items()}
        offending_link_names = [link_idx_to_name_map[idx.item()] for idx in offending_link_indices]

        print(offenses)
        print(offending_link_names)


if __name__ == "__main__":
    main()