Motion Planning Slow with Large Meshes

[frederiknolte]

Hi, first of all, thank you for the cool package!

I'm also using a 4090 and in fact I'm seeing similar benchmark results when running with --mesh:

+---------------------+-----------------------------------------------------------------+
| Metric              | Value                                                           |
+=====================+=================================================================+
| Success %           | 99.83                                                           |
+---------------------+-----------------------------------------------------------------+
| Plan Time (s)       | mean: 0.067 ± 0.049 median:0.056 75%: 0.074 98%: 0.167          |
+---------------------+-----------------------------------------------------------------+
| Motion Time(s)      | mean: 1.093 ± 0.328 median:1.039 75%: 1.297 98%: 1.904          |
+---------------------+-----------------------------------------------------------------+
| Path Length (rad.)  | mean: 2.837 ± 0.992 median:2.847 75%: 3.516 98%: 4.847          |
+---------------------+-----------------------------------------------------------------+
| Jerk                | mean: 104.805 ± 39.669 median:101.962 75%: 134.906 98%: 180.546 |
+---------------------+-----------------------------------------------------------------+
| Position Error (mm) | mean: 0.119 ± 0.325 median:0.041 75%: 0.097 98%: 0.819          |
+---------------------+-----------------------------------------------------------------+
+---------------------+---------------------------------------------------------------+
| Metric              | Value                                                         |
+=====================+===============================================================+
| Success %           | 99.88                                                         |
+---------------------+---------------------------------------------------------------+
| Plan Time (s)       | mean: 0.069 ± 0.057 median:0.057 75%: 0.072 98%: 0.180        |
+---------------------+---------------------------------------------------------------+
| Motion Time(s)      | mean: 1.178 ± 0.344 median:1.178 75%: 1.365 98%: 2.061        |
+---------------------+---------------------------------------------------------------+
| Path Length (rad.)  | mean: 3.140 ± 1.015 median:3.248 75%: 3.770 98%: 5.101        |
+---------------------+---------------------------------------------------------------+
| Jerk                | mean: 95.607 ± 39.045 median:91.989 75%: 123.495 98%: 177.400 |
+---------------------+---------------------------------------------------------------+
| Position Error (mm) | mean: 0.110 ± 0.283 median:0.045 75%: 0.097 98%: 0.745        |
+---------------------+---------------------------------------------------------------+

However, when using the motion planner in my custom IsaacSim environment, motion planning is suddenly incredibly slow. I never see motion planning times below 2.5 seconds, but regularly around 6 seconds, and sometimes even 14 seconds or more.

I do have meshes with perhaps 3k-4k faces in total in my environment. When I replace them with a single cuboid mesh, motion planning is much faster and completes in about 0.4 seconds. When downsampling my meshes to about 1500 faces in total, I do not see a speedup compared to 3k-4k faces which suggests that significant overhead happens somewhere between the simple cuboid with 12 faces and my downsampled environment with 1500 faces. Downsampling the meshes any further is not practical as the mesh quality becomes too bad. There is a pick-and-place video on the curobo website involving a mug and a small shelf, I suppose that scene has a comparable amount of faces as my setup?

I have three meshes in the environment (two small items and one shelf), as well as a cuboid for the ground plane). I tried initialising with both

n_obstacle_cuboids=1
n_obstacle_mesh=3

as well as

n_obstacle_cuboids=30
n_obstacle_mesh=100

My configuration is as follows:

world_cfg_plane = WorldConfig.from_dict(
            load_yaml(join_path(get_world_configs_path(), "collision_table.yml"))
        )
self._world_cfg_plane = world_cfg_plane
        
world_cfg1 = WorldConfig.from_dict(
            load_yaml(join_path(get_world_configs_path(), "collision_table.yml"))
        ).get_mesh_world()
world_cfg1.mesh[0].pose[2] = -10.5

self._world_cfg = WorldConfig(cuboid=world_cfg_plane.cuboid, mesh=world_cfg1.mesh)

motion_gen_config = MotionGenConfig.load_from_robot_config(
            self.robot_cfg,
            self._world_cfg,
            self.tensor_args,
            trajopt_tsteps=32,
            collision_checker_type=CollisionCheckerType.MESH,
            use_cuda_graph=True,
            interpolation_dt=0.01,
            collision_cache={"obb": n_obstacle_cuboids, "mesh": n_obstacle_mesh},
            store_ik_debug=self._save_log,
            store_trajopt_debug=self._save_log,
            velocity_scale=0.75,
            num_ik_seeds=16,
            num_trajopt_seeds=32,
            # trajopt_dt=0.15,
            # collision_activation_distance=0.01,
            # high_precision=True,
        )
self.motion_gen = MotionGen(motion_gen_config)
print("warming up motion planner...")
self.motion_gen.warmup(parallel_finetune=True)

pose_metric = None
if constrain_grasp_approach:  # Typically false as it slightly increases motion planning time 
    pose_metric = PoseCostMetric.create_grasp_approach_metric(
                offset_position=0.1, tstep_fraction=0.8
            )

self.plan_config = MotionGenPlanConfig(
            enable_graph=False,
            max_attempts=10,
            enable_graph_attempt=None,
            enable_finetune_trajopt=True,
            partial_ik_opt=False,
            parallel_finetune=True,
            pose_cost_metric=pose_metric,
        )

Setting with_graph=True generally appears to slow down motion planning.

I really hope you can help me as I've already spent a few weeks looking at other posts and trying different settings!

EDIT: Since I am using IsaacSim, there is occupied memory on the GPU that is unrelated to curobo (about 10GB). However, right before motion planning is performed the computational load tends towards 0% as not much happens asynchronously.

[balakumar-s]

Few changes that can help:

1. Leave velocity_scale as default (1.0) and instead use time_dilation_factor in MotionGenPlanConfig to slow down trajectories. This can lead to improved success rate.
2. num_trajopt_seeds=4 has shown to be sufficient for many planning problems, so try using that instead of 32. num_ik_seeds=32 could help improve success rate by having a higher chance of finding good seeds.
3. I am assuming self._save_log = False. If this was set to True then cuRobo will generate log data and that can cause significant slow down.
4. With MotionGenPlanConfig, it might be helpful to use enable_graph_attempt=3 to fallback to a graph planner when standard trajectory optimization fails.

In addition to these settings, did you try running curobo_benchmark.py --mesh when isaac sim is running to see if isaac sim is taking significant GPU resources, slowing down the benchmark to 10 seconds per plan?

If it's only slow with your specific meshes, you can alternatively use the voxel collision checker. An example is here:

curobo/benchmark/curobo_voxel_benchmark.py

Line 310 in a027cbc

world_voxel_collision.update_voxel_data(esdf)

[frederiknolte]

I tried all of your suggestions but motion planning is just as slow as before. Also, I more often than not get motion planning failures suggesting that trajectory finetuning failed. When manually setting the robot to the pose that the IK solver finds (before motion planning fails), I cannot see any collisions between the robot and objects or the robot and the ground.

I ran the benchmark while IsaacSim was populating the GPU and the benchmarking results are the same as when IsaacSim is not running (my previous results above; results are the same up to a couple of thousands of a second).