update to 0.6.2

benchmark/README.md

@@ -10,4 +10,12 @@ its affiliates is strictly prohibited.
 -->
 This folder contains scripts to run the motion planning benchmarks.
 
-Refer to Benchmarks & Profiling instructions in documentation for more information.
+Refer to Benchmarks & Profiling instructions: https://curobo.org/source/getting_started/4_benchmarks.html.
+
+### Note
+
+Results in the arxiv paper were obtained from v0.6.0. 
+
+v0.6.2+ has significant changes to improve motion quality with lower motion time, lower path length, higher pose accuracy (<1mm). v0.6.2+ sacrifices 30ms (RTX 4090) of compute time to achieve signficantly better solutions. The new results are yet to be added to the technical report. For now, to get the latest benchmark results follow instructions here: https://curobo.org/source/getting_started/4_benchmarks.html.
+
+To get results similar to in the technical report, pass `--report_edition` to `curobo_benchmark.py`. 

benchmark/curobo_benchmark.py

@@ -148,11 +148,16 @@ def load_curobo(
     mesh_mode: bool = False,
     cuda_graph: bool = True,
     collision_buffer: float = -0.01,
+    finetune_dt_scale: float = 0.9,
+    collision_activation_distance: float = 0.02,
+    args=None,
+    parallel_finetune=False,
 ):
     robot_cfg = load_yaml(join_path(get_robot_configs_path(), "franka.yml"))["robot_cfg"]
     robot_cfg["kinematics"]["collision_sphere_buffer"] = collision_buffer
     robot_cfg["kinematics"]["collision_spheres"] = "spheres/franka_mesh.yml"
     robot_cfg["kinematics"]["collision_link_names"].remove("attached_object")
+    robot_cfg["kinematics"]["ee_link"] = "panda_hand"
 
     # del robot_cfg["kinematics"]
 
@@ -160,11 +165,11 @@ def load_curobo(
     if graph_mode:
         trajopt_seeds = 4
     if trajopt_seeds >= 14:
-        ik_seeds = max(100, trajopt_seeds * 4)
+        ik_seeds = max(100, trajopt_seeds * 2)
     if mpinets:
         robot_cfg["kinematics"]["lock_joints"] = {
             "panda_finger_joint1": 0.025,
-            "panda_finger_joint2": -0.025,
+            "panda_finger_joint2": 0.025,
         }
     world_cfg = WorldConfig.from_dict(
         load_yaml(join_path(get_world_configs_path(), "collision_table.yml"))
@@ -182,12 +187,18 @@ def load_curobo(
     robot_cfg_instance = RobotConfig.from_dict(robot_cfg, tensor_args=TensorDeviceType())
 
     K = robot_cfg_instance.kinematics.kinematics_config.joint_limits
-    K.position[0, :] -= 0.1
-    K.position[1, :] += 0.1
-
+    K.position[0, :] -= 0.2
+    K.position[1, :] += 0.2
+    finetune_iters = None
+    grad_iters = None
+    if args.report_edition:
+        finetune_iters = 200
+        grad_iters = 125
     motion_gen_config = MotionGenConfig.load_from_robot_config(
         robot_cfg_instance,
         world_cfg,
+        finetune_trajopt_iters=finetune_iters,
+        grad_trajopt_iters=grad_iters,
         trajopt_tsteps=tsteps,
         collision_checker_type=c_checker,
         use_cuda_graph=cuda_graph,
@@ -201,13 +212,13 @@ def load_curobo(
         store_ik_debug=enable_debug,
         store_trajopt_debug=enable_debug,
         interpolation_steps=interpolation_steps,
-        collision_activation_distance=0.03,
+        collision_activation_distance=collision_activation_distance,
         trajopt_dt=0.25,
-        finetune_dt_scale=1.05,  # 1.05,
+        finetune_dt_scale=finetune_dt_scale,
         maximum_trajectory_dt=0.1,
     )
     mg = MotionGen(motion_gen_config)
-    mg.warmup(enable_graph=True, warmup_js_trajopt=False)
+    mg.warmup(enable_graph=True, warmup_js_trajopt=False, parallel_finetune=parallel_finetune)
     return mg, robot_cfg
 
 
@@ -218,16 +229,16 @@ def benchmark_mb(
     write_benchmark=False,
     plot_cost=False,
     override_tsteps: Optional[int] = None,
-    save_kpi=False,
     graph_mode=False,
     args=None,
 ):
     # load dataset:
+    force_graph = False
 
     interpolation_dt = 0.02
     # mpinets_data = True
     # if mpinets_data:
-    file_paths = [motion_benchmaker_raw, mpinets_raw][:]
+    file_paths = [motion_benchmaker_raw, mpinets_raw][:]  # [1:]
     if args.demo:
         file_paths = [demo_raw]
 
@@ -238,8 +249,10 @@ def benchmark_mb(
     og_tsteps = 32
     if override_tsteps is not None:
         og_tsteps = override_tsteps
-
+    og_finetune_dt_scale = 0.9
     og_trajopt_seeds = 12
+    og_parallel_finetune = not args.jetson
+    og_collision_activation_distance = 0.01
     for file_path in file_paths:
         all_groups = []
         mpinets_data = False
@@ -247,24 +260,52 @@ def benchmark_mb(
         if "dresser_task_oriented" in list(problems.keys()):
             mpinets_data = True
         for key, v in tqdm(problems.items()):
+            finetune_dt_scale = og_finetune_dt_scale
+            force_graph = False
             tsteps = og_tsteps
             trajopt_seeds = og_trajopt_seeds
-
+            collision_activation_distance = og_collision_activation_distance
+            parallel_finetune = og_parallel_finetune
             if "cage_panda" in key:
                 trajopt_seeds = 16
-            # else:
-            #    continue
+                finetune_dt_scale = 0.95
+                collision_activation_distance = 0.01
+                parallel_finetune = True
             if "table_under_pick_panda" in key:
                 tsteps = 44
-                trajopt_seeds = 28
+                trajopt_seeds = 24
+                finetune_dt_scale = 0.98
+                parallel_finetune = True
+            if "table_pick_panda" in key:
+                collision_activation_distance = 0.005
 
             if "cubby_task_oriented" in key and "merged" not in key:
-                trajopt_seeds = 16
+                trajopt_seeds = 24
+                finetune_dt_scale = 0.95
+                collision_activation_distance = 0.015
+                parallel_finetune = True
             if "dresser_task_oriented" in key:
-                trajopt_seeds = 16
-            scene_problems = problems[key]  # [:4]  # [:1]  # [:20]  # [0:10]
+                trajopt_seeds = 24
+                # tsteps = 40
+                finetune_dt_scale = 0.95
+                collision_activation_distance = 0.01
+                parallel_finetune = True
+            if key in [
+                "tabletop_neutral_start",
+                "merged_cubby_neutral_start",
+                "merged_cubby_task_oriented",
+                "cubby_neutral_start",
+                "cubby_neutral_goal",
+                "dresser_neutral_start",
+                "tabletop_task_oriented",
+            ]:
+                collision_activation_distance = 0.005
+            if key in ["dresser_neutral_goal"]:
+                trajopt_seeds = 24  # 24
+                tsteps = 36
+                collision_activation_distance = 0.005
+            scene_problems = problems[key]
             n_cubes = check_problems(scene_problems)
-            # torch.cuda.empty_cache()
             mg, robot_cfg = load_curobo(
                 n_cubes,
                 enable_debug,
@@ -275,6 +316,10 @@ def benchmark_mb(
                 args.mesh,
                 not args.disable_cuda_graph,
                 collision_buffer=args.collision_buffer,
+                finetune_dt_scale=finetune_dt_scale,
+                collision_activation_distance=collision_activation_distance,
+                args=args,
+                parallel_finetune=parallel_finetune,
             )
             m_list = []
             i = 0
@@ -282,23 +327,21 @@ def benchmark_mb(
             for problem in tqdm(scene_problems, leave=False):
                 i += 1
                 if problem["collision_buffer_ik"] < 0.0:
-                    #    print("collision_ik:", problem["collision_buffer_ik"])
+                    # print("collision_ik:", problem["collision_buffer_ik"])
                     continue
-                # if i != 269: # 226
-                #    continue
 
                 plan_config = MotionGenPlanConfig(
-                    max_attempts=100,  # 00,  # 00,  # 100,  # 00,  # 000,#,00,#00,  # 5000,
-                    enable_graph_attempt=3,
+                    max_attempts=100,  # 100,  # 00,  # 00,  # 100,  # 00,  # 000,#,00,#00,  # 5000,
+                    enable_graph_attempt=1,
+                    disable_graph_attempt=20,
                     enable_finetune_trajopt=True,
                     partial_ik_opt=False,
-                    enable_graph=graph_mode,
+                    enable_graph=graph_mode or force_graph,
                     timeout=60,
                     enable_opt=not graph_mode,
+                    need_graph_success=force_graph,
+                    parallel_finetune=parallel_finetune,
                 )
-                # if "table_under_pick_panda" in key:
-                #    plan_config.enable_graph = True
-                #    plan_config.partial_ik_opt = False
                 q_start = problem["start"]
                 pose = (
                     problem["goal_pose"]["position_xyz"] + problem["goal_pose"]["quaternion_wxyz"]
@@ -313,6 +356,15 @@ def benchmark_mb(
                     world = WorldConfig.from_dict(deepcopy(problem["obstacles"])).get_obb_world()
                 mg.world_coll_checker.clear_cache()
                 mg.update_world(world)
+                # from curobo.geom.types import Cuboid as curobo_Cuboid
+
+                # coll_mesh = mg.world_coll_checker.get_mesh_in_bounding_box(
+                #    curobo_Cuboid(name="test", pose=[0, 0, 0, 1, 0, 0, 0], dims=[1, 1, 1]),
+                #    voxel_size=0.01,
+                # )
+                #
+                # coll_mesh.save_as_mesh(problem_name + "debug_curobo.obj")
+                # exit()
                 # continue
                 # load obstacles
 
@@ -329,19 +381,15 @@ def benchmark_mb(
                     ik_fail += 1
                 # rint(plan_config.enable_graph, plan_config.enable_graph_attempt)
                 problem["solution"] = None
-                if plan_config.enable_finetune_trajopt:
-                    problem_name = key + "_" + str(i)
-                else:
-                    problem_name = "noft_" + key + "_" + str(i)
-                problem_name = "nosw_" + problem_name
+                problem_name = key + "_" + str(i)
+
                 if write_usd or save_log:
                     # CuRobo
                     from curobo.util.usd_helper import UsdHelper
 
                     world.randomize_color(r=[0.5, 0.9], g=[0.2, 0.5], b=[0.0, 0.2])
-
                     gripper_mesh = Mesh(
-                        name="target_gripper",
+                        name="robot_target_gripper",
                         file_path=join_path(
                             get_assets_path(),
                             "robot/franka_description/meshes/visual/hand.dae",
@@ -351,7 +399,7 @@ def benchmark_mb(
                     )
                     world.add_obstacle(gripper_mesh)
                 # get costs:
-                if plot_cost:
+                if plot_cost and not result.success.item():
                     dt = 0.5
                     problem_name = "approx_wolfe_p" + problem_name
                     if result.optimized_dt is not None:
@@ -367,7 +415,7 @@ def benchmark_mb(
                         plot_cost_iteration(
                             traj_cost,
                             title=problem_name + "_" + str(success) + "_" + str(dt),
-                            save_path=join_path("log/plot/", problem_name + "_cost"),
+                            save_path=join_path("benchmark/log/plot/", problem_name + "_cost"),
                             log_scale=False,
                         )
                         if "finetune_trajopt_result" in result.debug_info:
@@ -378,7 +426,9 @@ def benchmark_mb(
                             plot_cost_iteration(
                                 traj_cost,
                                 title=problem_name + "_" + str(success) + "_" + str(dt),
-                                save_path=join_path("log/plot/", problem_name + "_ft_cost"),
+                                save_path=join_path(
+                                    "benchmark/log/plot/", problem_name + "_ft_cost"
+                                ),
                                 log_scale=False,
                             )
                 if result.success.item():
@@ -451,6 +501,16 @@ def benchmark_mb(
                         solve_time = result.graph_time
                     else:
                         solve_time = result.solve_time
+                    # compute path length:
+                    path_length = torch.sum(
+                        torch.linalg.norm(
+                            (
+                                torch.roll(result.optimized_plan.position, -1, dims=-2)
+                                - result.optimized_plan.position
+                            )[..., :-1, :],
+                            dim=-1,
+                        )
+                    ).item()
                     current_metrics = CuroboMetrics(
                         skip=False,
                         success=True,
@@ -465,6 +525,7 @@ def benchmark_mb(
                         attempts=result.attempts,
                         motion_time=result.motion_time.item(),
                         solve_time=solve_time,
+                        cspace_path_length=path_length,
                     )
 
                     if write_usd:
@@ -477,15 +538,16 @@ def benchmark_mb(
                             start_state,
                             q_traj,
                             dt=result.interpolation_dt,
-                            save_path=join_path("log/usd/", problem_name) + ".usd",
+                            save_path=join_path("benchmark/log/usd/", problem_name) + ".usd",
                             interpolation_steps=1,
-                            write_robot_usd_path="log/usd/assets/",
+                            write_robot_usd_path="benchmark/log/usd/assets/",
                             robot_usd_local_reference="assets/",
                             base_frame="/world_" + problem_name,
-                            visualize_robot_spheres=True,
+                            visualize_robot_spheres=False,
+                            flatten_usd=False,
                         )
 
-                    if write_plot:
+                    if write_plot:  # and result.optimized_dt.item() > 0.06:
                         problem_name = problem_name
                         plot_traj(
                             result.optimized_plan,
@@ -493,23 +555,21 @@ def benchmark_mb(
                             # result.get_interpolated_plan(),
                             # result.interpolation_dt,
                             title=problem_name,
-                            save_path=join_path("log/plot/", problem_name + ".pdf"),
-                        )
-                        plot_traj(
-                            # result.optimized_plan,
-                            # result.optimized_dt.item(),
-                            result.get_interpolated_plan(),
-                            result.interpolation_dt,
-                            title=problem_name,
-                            save_path=join_path("log/plot/", problem_name + "_int.pdf"),
+                            save_path=join_path("benchmark/log/plot/", problem_name + ".png"),
                         )
+                        # plot_traj(
+                        #    # result.optimized_plan,
+                        #    # result.optimized_dt.item(),
+                        #    result.get_interpolated_plan(),
+                        #    result.interpolation_dt,
+                        #    title=problem_name,
+                        #    save_path=join_path("benchmark/log/plot/", problem_name + "_int.pdf"),
+                        # )
                         # exit()
 
                     m_list.append(current_metrics)
                     all_groups.append(current_metrics)
                 elif result.valid_query:
-                    # print("fail")
-                    # print(result.status)
                     current_metrics = CuroboMetrics()
                     debug = {
                         "used_graph": result.used_graph,
@@ -554,14 +614,14 @@ def benchmark_mb(
                             start_state,
                             q_traj,
                             dt=result.interpolation_dt,
-                            save_path=join_path("log/usd/", problem_name) + ".usd",
+                            save_path=join_path("benchmark/log/usd/", problem_name) + ".usd",
                             interpolation_steps=1,
-                            write_robot_usd_path="log/usd/assets/",
+                            write_robot_usd_path="benchmark/log/usd/assets/",
                             robot_usd_local_reference="assets/",
                             base_frame="/world_" + problem_name,
                             visualize_robot_spheres=True,
                         )
-                if save_log:  # and not result.success.item():
+                if save_log and not result.success.item():
                     # print("save log")
                     UsdHelper.write_motion_gen_log(
                         result,
@@ -569,28 +629,29 @@ def benchmark_mb(
                         world,
                         start_state,
                         Pose.from_list(pose),
-                        join_path("log/usd/", problem_name) + "_debug",
+                        join_path("benchmark/log/usd/", problem_name) + "_debug",
                         write_ik=False,
                         write_trajopt=True,
                         visualize_robot_spheres=False,
                         grid_space=2,
+                        write_robot_usd_path="benchmark/log/usd/assets/",
+                        # flatten_usd=True,
                     )
-                # exit()
+                #    print(result.status)
+                #    exit()
 
             g_m = CuroboGroupMetrics.from_list(m_list)
             if not args.kpi:
                 print(
                     key,
                     f"{g_m.success:2.2f}",
-                    # g_m.motion_time,
                     g_m.time.mean,
-                    # g_m.time.percent_75,
                     g_m.time.percent_98,
-                    g_m.position_error.percent_98,
-                    # g_m.position_error.median,
-                    g_m.orientation_error.percent_98,
-                    # g_m.orientation_error.median,
-                )  # , g_m.attempts)
+                    g_m.position_error.mean,
+                    g_m.orientation_error.mean,
+                    g_m.cspace_path_length.percent_98,
+                    g_m.motion_time.percent_98,
+                )
                 print(g_m.attempts)
             # print("MT: ", g_m.motion_time)
             # $print(ik_fail)
@@ -624,6 +685,7 @@ def benchmark_mb(
     print("######## FULL SET ############")
     print("All: ", f"{g_m.success:2.2f}")
     print("MT: ", g_m.motion_time)
+    print("path-length: ", g_m.cspace_path_length)
     print("PT:", g_m.time)
     print("ST: ", g_m.solve_time)
     print("position error (mm): ", g_m.position_error)
@@ -632,7 +694,7 @@ def benchmark_mb(
     if args.kpi:
         kpi_data = {
             "Success": g_m.success,
-            "Planning Time Mean": float(g_m.time.mean),
+            "Planning Time": float(g_m.time.mean),
             "Planning Time Std": float(g_m.time.std),
             "Planning Time Median": float(g_m.time.median),
             "Planning Time 75th": float(g_m.time.percent_75),
@@ -670,7 +732,7 @@ if __name__ == "__main__":
     parser.add_argument(
         "--collision_buffer",
         type=float,
-        default=-0.00,  # in meters
+        default=0.00,  # in meters
         help="Robot collision buffer",
     )
 
@@ -711,17 +773,40 @@ if __name__ == "__main__":
         help="When True, writes paths to file",
         default=False,
     )
+    parser.add_argument(
+        "--save_usd",
+        action="store_true",
+        help="When True, writes paths to file",
+        default=False,
+    )
+    parser.add_argument(
+        "--save_plot",
+        action="store_true",
+        help="When True, writes paths to file",
+        default=False,
+    )
+    parser.add_argument(
+        "--report_edition",
+        action="store_true",
+        help="When True, runs benchmark with parameters from technical report",
+        default=False,
+    )
+    parser.add_argument(
+        "--jetson",
+        action="store_true",
+        help="When True, runs benchmark with parameters for jetson",
+        default=False,
+    )
 
     args = parser.parse_args()
 
     setup_curobo_logger("error")
     benchmark_mb(
         save_log=False,
-        write_usd=False,
-        write_plot=False,
+        write_usd=args.save_usd,
+        write_plot=args.save_plot,
         write_benchmark=args.write_benchmark,
         plot_cost=False,
-        save_kpi=args.kpi,
         graph_mode=args.graph,
         args=args,
     )

benchmark/curobo_nvblox_benchmark.py

@@ -20,11 +20,13 @@ import numpy as np
 import torch
 from metrics import CuroboGroupMetrics, CuroboMetrics
 from nvblox_torch.datasets.mesh_dataset import MeshDataset
+from nvblox_torch.datasets.sun3d_dataset import Sun3dDataset
 from robometrics.datasets import demo_raw, motion_benchmaker_raw, mpinets_raw
 from tqdm import tqdm
 
 # CuRobo
 from curobo.geom.sdf.world import CollisionCheckerType, WorldConfig
+from curobo.geom.types import Cuboid as curobo_Cuboid
 from curobo.geom.types import Mesh
 from curobo.types.base import TensorDeviceType
 from curobo.types.camera import CameraObservation
@@ -40,6 +42,7 @@ from curobo.util_file import (
     load_yaml,
     write_yaml,
 )
+from curobo.wrap.model.robot_world import RobotWorld, RobotWorldConfig
 from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig
 
 torch.manual_seed(0)
@@ -125,28 +128,36 @@ def load_curobo(
     cuda_graph: bool = True,
 ):
     robot_cfg = load_yaml(join_path(get_robot_configs_path(), "franka.yml"))["robot_cfg"]
-    robot_cfg["kinematics"]["collision_sphere_buffer"] = -0.015
+    robot_cfg["kinematics"]["collision_sphere_buffer"] = -0.0
 
     ik_seeds = 30  # 500
     if graph_mode:
         trajopt_seeds = 4
     if trajopt_seeds >= 14:
-        ik_seeds = max(100, trajopt_seeds * 4)
+        ik_seeds = max(100, trajopt_seeds * 2)
     if mpinets:
         robot_cfg["kinematics"]["lock_joints"] = {
             "panda_finger_joint1": 0.025,
-            "panda_finger_joint2": -0.025,
+            "panda_finger_joint2": 0.025,
         }
     world_cfg = WorldConfig.from_dict(
-        load_yaml(join_path(get_world_configs_path(), "collision_nvblox_online.yml"))
+        {
+            "blox": {
+                "world": {
+                    "pose": [0, 0, 0, 1, 0, 0, 0],
+                    "integrator_type": "tsdf",
+                    "voxel_size": 0.014,
+                }
+            }
+        }
     )
     interpolation_steps = 2000
     if graph_mode:
         interpolation_steps = 100
     robot_cfg_instance = RobotConfig.from_dict(robot_cfg, tensor_args=TensorDeviceType())
     K = robot_cfg_instance.kinematics.kinematics_config.joint_limits
-    K.position[0, :] -= 0.1
-    K.position[1, :] += 0.1
+    K.position[0, :] -= 0.2
+    K.position[1, :] += 0.2
 
     motion_gen_config = MotionGenConfig.load_from_robot_config(
         robot_cfg_instance,
@@ -163,16 +174,24 @@ def load_curobo(
         store_ik_debug=enable_debug,
         store_trajopt_debug=enable_debug,
         interpolation_steps=interpolation_steps,
-        collision_activation_distance=0.025,
-        state_finite_difference_mode="CENTRAL",
+        collision_activation_distance=0.01,
         trajopt_dt=0.25,
-        minimize_jerk=True,
-        finetune_dt_scale=1.05,
+        finetune_dt_scale=1.0,
         maximum_trajectory_dt=0.1,
     )
     mg = MotionGen(motion_gen_config)
     mg.warmup(enable_graph=True, warmup_js_trajopt=False)
-    return mg, robot_cfg
+    # create a ground truth collision checker:
+    config = RobotWorldConfig.load_from_config(
+        robot_cfg,
+        "collision_table.yml",
+        collision_activation_distance=0.0,
+        collision_checker_type=CollisionCheckerType.PRIMITIVE,
+        n_cuboids=50,
+    )
+    robot_world = RobotWorld(config)
+
+    return mg, robot_cfg, robot_world
 
 
 def benchmark_mb(
@@ -187,7 +206,7 @@ def benchmark_mb(
     # load dataset:
     graph_mode = args.graph
     interpolation_dt = 0.02
-    file_paths = [demo_raw, motion_benchmaker_raw, mpinets_raw][2:]
+    file_paths = [demo_raw, motion_benchmaker_raw, mpinets_raw][1:]
 
     enable_debug = save_log or plot_cost
     all_files = []
@@ -206,7 +225,7 @@ def benchmark_mb(
         if "dresser_task_oriented" in list(problems.keys()):
             mpinets_data = True
 
-        mg, robot_cfg = load_curobo(
+        mg, robot_cfg, robot_world = load_curobo(
             1,
             enable_debug,
             og_tsteps,
@@ -217,7 +236,7 @@ def benchmark_mb(
         )
 
         for key, v in tqdm(problems.items()):
-            scene_problems = problems[key][:]  # [:1]  # [:20]  # [0:10]
+            scene_problems = problems[key]
             m_list = []
             i = 0
             ik_fail = 0
@@ -227,6 +246,7 @@ def benchmark_mb(
                 plan_config = MotionGenPlanConfig(
                     max_attempts=10,  # 00,  # 00,  # 100,  # 00,  # 000,#,00,#00,  # 5000,
                     enable_graph_attempt=3,
+                    disable_graph_attempt=20,
                     enable_finetune_trajopt=True,
                     partial_ik_opt=False,
                     enable_graph=graph_mode,
@@ -239,25 +259,35 @@ def benchmark_mb(
                     problem["goal_pose"]["position_xyz"] + problem["goal_pose"]["quaternion_wxyz"]
                 )
 
-                problem_name = "d_" + key + "_" + str(i)
+                problem_name = "nvblox_" + key + "_" + str(i)
 
                 # reset planner
                 mg.reset(reset_seed=False)
-                world = WorldConfig.from_dict(deepcopy(problem["obstacles"])).get_mesh_world(
-                    merge_meshes=True
-                )
-                mesh = world.mesh[0].get_trimesh_mesh()
+                world = WorldConfig.from_dict(problem["obstacles"])
+
+                # .get_mesh_world(
+                #    # merge_meshes=True
+                # )
+                # mesh = world.mesh[0].get_trimesh_mesh()
 
                 # world.save_world_as_mesh(problem_name + ".stl")
+                mg.world_coll_checker.update_blox_hashes()
+
                 mg.world_coll_checker.clear_cache()
-                m_dataset = MeshDataset(
-                    None, n_frames=200, image_size=640, save_data_dir=None, trimesh_mesh=mesh
-                )
+                save_path = "benchmark/log/nvblox/" + key + "_" + str(i)
+
+                m_dataset = Sun3dDataset(save_path)
+                # m_dataset = MeshDataset(
+                #    None, n_frames=100, image_size=640, save_data_dir=None, trimesh_mesh=mesh
+                # )
                 tensor_args = mg.tensor_args
-                for j in range(len(m_dataset)):
+                for j in tqdm(range(len(m_dataset)), leave=False):
                     data = m_dataset[j]
                     cam_obs = CameraObservation(
-                        rgb_image=tensor_args.to_device(data["rgba"]),
+                        rgb_image=tensor_args.to_device(data["rgba"])
+                        .squeeze()
+                        .to(dtype=torch.uint8)
+                        .permute(1, 2, 0),  # data[rgba]: 4 x H x W -> H x W x 4
                         depth_image=tensor_args.to_device(data["depth"]),
                         intrinsics=data["intrinsics"],
                         pose=Pose.from_matrix(data["pose"].to(device=mg.tensor_args.device)),
@@ -266,22 +296,38 @@ def benchmark_mb(
 
                     mg.add_camera_frame(cam_obs, "world")
 
-                    mg.process_camera_frames("world", False)
+                mg.process_camera_frames("world", False)
                 torch.cuda.synchronize()
                 mg.world_coll_checker.update_blox_hashes()
                 torch.cuda.synchronize()
-                if save_log or write_usd:
-                    # nvblox_obs = mg.world_coll_checker.get_mesh_from_blox_layer("world", mode="nvblox")
+                # mg.world_coll_checker.save_layer("world", "test.nvblx")
+
+                if save_log or write_usd:
+                    world.randomize_color(r=[0.5, 0.9], g=[0.2, 0.5], b=[0.0, 0.2])
 
-                    # nvblox_obs.save_as_mesh("debug_tsdf.obj")
                     nvblox_obs = mg.world_coll_checker.get_mesh_from_blox_layer(
-                        "world", mode="voxel"
+                        "world",
                     )
-                    nvblox_obs.color = [0.0, 0.0, 0.8, 0.8]
-                    # nvblox_obs.save_as_mesh("debug_voxel_occ.obj")
-                    # exit()
-                    nvblox_obs.name = "nvblox_world"
+                    # nvblox_obs.vertex_colors = None
+
+                    if nvblox_obs.vertex_colors is not None:
+                        nvblox_obs.vertex_colors = nvblox_obs.vertex_colors.cpu().numpy()
+                    else:
+                        nvblox_obs.color = [0.0, 0.0, 0.8, 0.8]
+
+                    nvblox_obs.name = "nvblox_mesh_world"
                     world.add_obstacle(nvblox_obs)
+
+                    coll_mesh = mg.world_coll_checker.get_mesh_in_bounding_box(
+                        curobo_Cuboid(name="test", pose=[0, 0, 0, 1, 0, 0, 0], dims=[1.5, 1.5, 1]),
+                        voxel_size=0.005,
+                    )
+
+                    coll_mesh.color = [0.0, 0.8, 0.8, 0.8]
+
+                    coll_mesh.name = "nvblox_voxel_world"
+                    world.add_obstacle(coll_mesh)
+                    # exit()
                 # run planner
                 start_state = JointState.from_position(mg.tensor_args.to_device([q_start]))
                 result = mg.plan_single(
@@ -292,20 +338,12 @@ def benchmark_mb(
                 if result.status == "IK Fail":
                     ik_fail += 1
                 problem["solution"] = None
-                if plan_config.enable_finetune_trajopt:
-                    problem_name = key + "_" + str(i)
-                else:
-                    problem_name = "noft_" + key + "_" + str(i)
-                problem_name = "nvblox_" + problem_name
                 if write_usd or save_log:
                     # CuRobo
                     from curobo.util.usd_helper import UsdHelper
 
-                    world.randomize_color(r=[0.5, 0.9], g=[0.2, 0.5], b=[0.0, 0.2])
-                    if len(world.mesh) > 1:
-                        world.mesh[1].color = [125 / 255, 255 / 255, 70.0 / 255, 1.0]
                     gripper_mesh = Mesh(
-                        name="target_gripper",
+                        name="target_gripper_1",
                         file_path=join_path(
                             get_assets_path(),
                             "robot/franka_description/meshes/visual/hand.dae",
@@ -330,7 +368,7 @@ def benchmark_mb(
                         plot_cost_iteration(
                             traj_cost,
                             title=problem_name + "_" + str(success) + "_" + str(dt),
-                            save_path=join_path("log/plot/", problem_name + "_cost"),
+                            save_path=join_path("benchmark/log/plot/", problem_name + "_cost"),
                             log_scale=False,
                         )
                         if "finetune_trajopt_result" in result.debug_info:
@@ -341,7 +379,9 @@ def benchmark_mb(
                             plot_cost_iteration(
                                 traj_cost,
                                 title=problem_name + "_" + str(success) + "_" + str(dt),
-                                save_path=join_path("log/plot/", problem_name + "_ft_cost"),
+                                save_path=join_path(
+                                    "benchmark/log/plot/", problem_name + "_ft_cost"
+                                ),
                                 log_scale=False,
                             )
                 if result.success.item():
@@ -397,9 +437,24 @@ def benchmark_mb(
                         "valid_query": result.valid_query,
                     }
                     problem["solution_debug"] = debug
+                    # check if path is collision free w.r.t. ground truth mesh:
+                    robot_world.world_model.clear_cache()
+                    robot_world.update_world(world)
+                    q_int_traj = result.get_interpolated_plan().position.unsqueeze(0)
+                    d_int_mask = (
+                        torch.count_nonzero(~robot_world.validate_trajectory(q_int_traj)) == 0
+                    ).item()
+
+                    q_traj = result.optimized_plan.position.unsqueeze(0)
+                    d_mask = (
+                        torch.count_nonzero(~robot_world.validate_trajectory(q_traj)) == 0
+                    ).item()
+
                     current_metrics = CuroboMetrics(
                         skip=False,
                         success=True,
+                        perception_success=d_mask,
+                        perception_interpolated_success=d_int_mask,
                         time=result.total_time,
                         collision=False,
                         joint_limit_violation=False,
@@ -423,14 +478,15 @@ def benchmark_mb(
                             start_state,
                             q_traj,
                             dt=result.interpolation_dt,
-                            save_path=join_path("log/usd/", problem_name) + ".usd",
+                            save_path=join_path("benchmark/log/usd/", problem_name) + ".usd",
                             interpolation_steps=1,
-                            write_robot_usd_path="log/usd/assets/",
+                            write_robot_usd_path="benchmark/log/usd/assets/",
                             robot_usd_local_reference="assets/",
                             base_frame="/world_" + problem_name,
                             visualize_robot_spheres=True,
+                            # flatten_usd=True,
                         )
-
+                        exit()
                     if write_plot:
                         problem_name = problem_name
                         plot_traj(
@@ -439,7 +495,7 @@ def benchmark_mb(
                             # result.get_interpolated_plan(),
                             # result.interpolation_dt,
                             title=problem_name,
-                            save_path=join_path("log/plot/", problem_name + ".pdf"),
+                            save_path=join_path("benchmark/log/plot/", problem_name + ".pdf"),
                         )
                         plot_traj(
                             # result.optimized_plan,
@@ -447,7 +503,7 @@ def benchmark_mb(
                             result.get_interpolated_plan(),
                             result.interpolation_dt,
                             title=problem_name,
-                            save_path=join_path("log/plot/", problem_name + "_int.pdf"),
+                            save_path=join_path("benchmark/log/plot/", problem_name + "_int.pdf"),
                         )
 
                     m_list.append(current_metrics)
@@ -470,7 +526,7 @@ def benchmark_mb(
                     m_list.append(current_metrics)
                     all_groups.append(current_metrics)
                 else:
-                    print("invalid: " + problem_name)
+                    # print("invalid: " + problem_name)
                     debug = {
                         "used_graph": result.used_graph,
                         "attempts": result.attempts,
@@ -483,28 +539,37 @@ def benchmark_mb(
                         "valid_query": result.valid_query,
                     }
                     problem["solution_debug"] = debug
-                if save_log:
+                if save_log and not result.success.item():
                     UsdHelper.write_motion_gen_log(
                         result,
                         robot_cfg,
                         world,
                         start_state,
                         Pose.from_list(pose),
-                        join_path("log/usd/", problem_name) + "_debug",
-                        write_ik=not result.success.item(),
+                        join_path("benchmark/log/usd/", problem_name) + "_debug",
+                        write_ik=True,
                         write_trajopt=True,
                         visualize_robot_spheres=True,
                         grid_space=2,
+                        # flatten_usd=True,
                     )
+                    # exit()
             g_m = CuroboGroupMetrics.from_list(m_list)
             print(
                 key,
                 f"{g_m.success:2.2f}",
                 g_m.time.mean,
+                # g_m.time.percent_75,
                 g_m.time.percent_98,
                 g_m.position_error.percent_98,
+                # g_m.position_error.median,
                 g_m.orientation_error.percent_98,
+                g_m.cspace_path_length.percent_98,
+                g_m.motion_time.percent_98,
+                g_m.perception_success,
+                # g_m.orientation_error.median,
             )
+            print(g_m.attempts)
         g_m = CuroboGroupMetrics.from_list(all_groups)
         print(
             "All: ",
@@ -514,6 +579,7 @@ def benchmark_mb(
             g_m.time.percent_75,
             g_m.position_error.percent_75,
             g_m.orientation_error.percent_75,
+            g_m.perception_success,
         )
         print(g_m.attempts)
         if write_benchmark:
@@ -525,6 +591,11 @@ def benchmark_mb(
     g_m = CuroboGroupMetrics.from_list(all_files)
     print("######## FULL SET ############")
     print("All: ", f"{g_m.success:2.2f}")
+    print(
+        "Perception Success (coarse, interpolated):",
+        g_m.perception_success,
+        g_m.perception_interpolated_success,
+    )
     print("MT: ", g_m.motion_time)
     print("PT:", g_m.time)
     print("ST: ", g_m.solve_time)

benchmark/curobo_nvblox_profile.py

@@ -17,6 +17,7 @@ from typing import Any, Dict, List
 import numpy as np
 import torch
 import torch.autograd.profiler as profiler
+from nvblox_torch.datasets.sun3d_dataset import Sun3dDataset
 from robometrics.datasets import demo_raw
 from torch.profiler import ProfilerActivity, profile, record_function
 from tqdm import tqdm
@@ -55,7 +56,7 @@ from curobo.types.camera import CameraObservation
 
 def load_curobo(n_cubes: int, enable_log: bool = False):
     robot_cfg = load_yaml(join_path(get_robot_configs_path(), "franka.yml"))["robot_cfg"]
-    robot_cfg["kinematics"]["collision_sphere_buffer"] = -0.015
+    robot_cfg["kinematics"]["collision_sphere_buffer"] = -0.0
     motion_gen_config = MotionGenConfig.load_from_robot_config(
         robot_cfg,
         "collision_nvblox_online.yml",
@@ -67,7 +68,6 @@ def load_curobo(n_cubes: int, enable_log: bool = False):
         num_ik_seeds=30,
         num_trajopt_seeds=12,
         interpolation_dt=0.02,
-        # grad_trajopt_iters=200,
         store_ik_debug=enable_log,
         store_trajopt_debug=enable_log,
     )
@@ -85,7 +85,7 @@ def benchmark_mb(write_usd=False, save_log=False):
         spheres = load_yaml(join_path(get_robot_configs_path(), spheres))["collision_spheres"]
 
     plan_config = MotionGenPlanConfig(
-        max_attempts=2,
+        max_attempts=1,
         enable_graph_attempt=3,
         enable_finetune_trajopt=True,
         partial_ik_opt=False,
@@ -107,7 +107,7 @@ def benchmark_mb(write_usd=False, save_log=False):
             n_cubes = check_problems(scene_problems)
             mg = load_curobo(n_cubes, save_log)
             m_list = []
-            i = 0
+            i = 1
             for problem in tqdm(scene_problems, leave=False):
                 q_start = problem["start"]
                 pose = (
@@ -124,9 +124,13 @@ def benchmark_mb(write_usd=False, save_log=False):
                 mg.clear_world_cache()
                 obs = []
                 # get camera_observations:
-                m_dataset = MeshDataset(
-                    None, n_frames=200, image_size=640, save_data_dir=None, trimesh_mesh=mesh
-                )
+                save_path = "benchmark/log/nvblox/" + key + "_" + str(i)
+
+                m_dataset = Sun3dDataset(save_path)
+
+                # m_dataset = MeshDataset(
+                #    None, n_frames=200, image_size=640, save_data_dir=None, trimesh_mesh=mesh
+                # )
                 obs = []
                 tensor_args = mg.tensor_args
                 for j in range(len(m_dataset)):
@@ -158,7 +162,7 @@ def benchmark_mb(write_usd=False, save_log=False):
                         plan_config,
                     )
                 print("Exporting the trace..")
-                prof.export_chrome_trace("log/trace/trajopt_global_nvblox.json")
+                prof.export_chrome_trace("benchmark/log/trace/motion_gen_nvblox.json")
                 print(result.success, result.status)
                 exit()
 

benchmark/curobo_profile.py

@@ -50,7 +50,7 @@ def load_curobo(
     n_cubes: int, enable_log: bool = False, mesh_mode: bool = False, cuda_graph: bool = False
 ):
     robot_cfg = load_yaml(join_path(get_robot_configs_path(), "franka.yml"))["robot_cfg"]
-    robot_cfg["kinematics"]["collision_sphere_buffer"] = -0.015
+    robot_cfg["kinematics"]["collision_sphere_buffer"] = -0.0
     world_cfg = WorldConfig.from_dict(
         load_yaml(join_path(get_world_configs_path(), "collision_table.yml"))
     ).get_obb_world()
@@ -160,7 +160,7 @@ if __name__ == "__main__":
     parser.add_argument(
         "--save_path",
         type=str,
-        default="log/trace",
+        default="benchmark/log/trace",
         help="path to save file",
     )
     parser.add_argument(

benchmark/curobo_python_profile.py

@@ -39,24 +39,22 @@ def demo_motion_gen():
         robot_file,
         world_file,
         tensor_args,
-        trajopt_tsteps=32,
+        trajopt_tsteps=44,
         collision_checker_type=CollisionCheckerType.PRIMITIVE,
         use_cuda_graph=False,
-        num_trajopt_seeds=4,
-        num_graph_seeds=1,
+        num_trajopt_seeds=24,
+        num_graph_seeds=24,
         evaluate_interpolated_trajectory=True,
-        interpolation_dt=0.01,
+        interpolation_dt=0.02,
     )
     motion_gen = MotionGen(motion_gen_config)
 
     # st_time = time.time()
-    motion_gen.warmup(batch=50, enable_graph=False, warmup_js_trajopt=False)
+    motion_gen.warmup(enable_graph=True, warmup_js_trajopt=False)
     print("motion gen time:", time.time() - st_time)
 
     # print(time.time() - st_time)
-    return
-    robot_cfg = load_yaml(join_path(get_robot_configs_path(), robot_file))["robot_cfg"]
-    robot_cfg = RobotConfig.from_dict(robot_cfg, tensor_args)
+    # return
     retract_cfg = motion_gen.get_retract_config()
     print(retract_cfg)
     state = motion_gen.rollout_fn.compute_kinematics(
@@ -66,8 +64,14 @@ def demo_motion_gen():
     retract_pose = Pose(state.ee_pos_seq.squeeze(), quaternion=state.ee_quat_seq.squeeze())
     start_state = JointState.from_position(retract_cfg.view(1, -1) + 0.3)
     result = motion_gen.plan(
-        start_state, retract_pose, enable_graph=True, enable_opt=False, max_attempts=1
+        start_state,
+        retract_pose,
+        enable_graph=True,
+        enable_opt=True,
+        max_attempts=1,
+        need_graph_success=True,
     )
+    print(result.status)
     print(result.optimized_plan.position.shape)
     traj = result.get_interpolated_plan()  # $.position.view(-1, 7)  # optimized plan
     print("Trajectory Generated: ", result.success, result.optimized_dt.item())
@@ -137,9 +141,14 @@ if __name__ == "__main__":
         "--kinematics",
         action="store_true",
         help="When True, runs startup for kinematics",
-        default=True,
+        default=False,
+    )
+    parser.add_argument(
+        "--motion_gen_once",
+        action="store_true",
+        help="When True, runs startup for kinematics",
+        default=False,
     )
-
     args = parser.parse_args()
 
     # cProfile.run('demo_motion_gen()')
@@ -163,6 +172,9 @@ if __name__ == "__main__":
         filename = join_path(args.save_path, args.file_name) + "_kinematics_trace.json"
         prof.export_chrome_trace(filename)
 
+    if args.motion_gen_once:
+        demo_motion_gen()
+
     if args.motion_gen:
         for _ in range(5):
             demo_motion_gen()

benchmark/generate_nvblox_images.py

@@ -0,0 +1,66 @@
+#
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+#
+
+# Standard Library
+from copy import deepcopy
+
+# Third Party
+import numpy as np
+import torch
+from nvblox_torch.datasets.mesh_dataset import MeshDataset
+from robometrics.datasets import demo_raw, motion_benchmaker_raw, mpinets_raw
+from tqdm import tqdm
+
+# CuRobo
+from curobo.geom.sdf.world import WorldConfig
+from curobo.util.logger import setup_curobo_logger
+
+torch.manual_seed(0)
+
+torch.backends.cudnn.benchmark = True
+
+torch.backends.cuda.matmul.allow_tf32 = True
+torch.backends.cudnn.allow_tf32 = True
+
+np.random.seed(0)
+
+
+def generate_images():
+    # load dataset:
+
+    file_paths = [demo_raw, motion_benchmaker_raw, mpinets_raw][1:]
+
+    for file_path in file_paths:
+        problems = file_path()
+
+        for key, v in tqdm(problems.items()):
+            scene_problems = problems[key]
+            i = 0
+            for problem in tqdm(scene_problems, leave=False):
+                i += 1
+
+                world = WorldConfig.from_dict(deepcopy(problem["obstacles"])).get_mesh_world(
+                    merge_meshes=True
+                )
+                mesh = world.mesh[0].get_trimesh_mesh()
+
+                # world.save_world_as_mesh(problem_name + ".stl")
+                save_path = "benchmark/log/nvblox/" + key + "_" + str(i)
+
+                # generate images and write to disk:
+                MeshDataset(
+                    None, n_frames=50, image_size=640, save_data_dir=save_path, trimesh_mesh=mesh
+                )
+
+
+if __name__ == "__main__":
+    setup_curobo_logger("error")
+    generate_images()

benchmark/ik_benchmark.py

@@ -23,8 +23,10 @@ from curobo.rollout.arm_base import ArmBase, ArmBaseConfig
 from curobo.types.base import TensorDeviceType
 from curobo.types.math import Pose
 from curobo.types.robot import RobotConfig
+from curobo.util.logger import setup_curobo_logger
 from curobo.util_file import (
     get_motion_gen_robot_list,
+    get_multi_arm_robot_list,
     get_robot_configs_path,
     get_robot_list,
     get_task_configs_path,
@@ -53,6 +55,7 @@ def run_full_config_collision_free_ik(
     if not collision_free:
         robot_data["kinematics"]["collision_link_names"] = None
         robot_data["kinematics"]["lock_joints"] = {}
+    robot_data["kinematics"]["collision_sphere_buffer"] = 0.0
     robot_cfg = RobotConfig.from_dict(robot_data)
     world_cfg = WorldConfig.from_dict(load_yaml(join_path(get_world_configs_path(), world_file)))
     position_threshold = 0.005
@@ -63,7 +66,7 @@ def run_full_config_collision_free_ik(
         world_cfg,
         rotation_threshold=0.05,
         position_threshold=position_threshold,
-        num_seeds=30,
+        num_seeds=24,
         self_collision_check=collision_free,
         self_collision_opt=collision_free,
         tensor_args=tensor_args,
@@ -89,12 +92,14 @@ def run_full_config_collision_free_ik(
     return (
         total_time,
         100.0 * torch.count_nonzero(result.success).item() / len(q_sample),
+        # np.mean(result.position_error[result.success].cpu().numpy()).item(),
         np.percentile(result.position_error[result.success].cpu().numpy(), 98).item(),
         np.percentile(result.rotation_error[result.success].cpu().numpy(), 98).item(),
     )
 
 
 if __name__ == "__main__":
+    setup_curobo_logger("error")
     parser = argparse.ArgumentParser()
     parser.add_argument(
         "--save_path",
@@ -119,22 +124,21 @@ if __name__ == "__main__":
 
     b_list = [1, 10, 100, 1000][-1:]
 
-    robot_list = get_motion_gen_robot_list()
-
+    robot_list = get_motion_gen_robot_list() + get_multi_arm_robot_list()[:2]
     world_file = "collision_test.yml"
 
     print("running...")
     data = {
         "robot": [],
-        "IK time(ms)": [],
-        "Collision-Free IK time(ms)": [],
-        "Batch Size": [],
-        "Success IK": [],
-        "Success Collision-Free IK": [],
-        "Position Error(mm)": [],
-        "Orientation Error": [],
-        "Position Error Collision-Free IK(mm)": [],
-        "Orientation Error Collision-Free IK": [],
+        "IK-time(ms)": [],
+        "Collision-Free-IK-time(ms)": [],
+        "Batch-Size": [],
+        "Success-IK": [],
+        "Success-Collision-Free-IK": [],
+        "Position-Error(mm)": [],
+        "Orientation-Error": [],
+        "Position-Error-Collision-Free-IK(mm)": [],
+        "Orientation-Error-Collision-Free-IK": [],
     }
     for robot_file in robot_list:
         # create a sampler with dof:
@@ -155,27 +159,28 @@ if __name__ == "__main__":
                 batch_size=b_size,
                 use_cuda_graph=True,
                 collision_free=True,
+                # high_precision=args.high_precision,
             )
-
             # print(dt_cu/b_size, dt_cu_cg/b_size)
             data["robot"].append(robot_file)
-            data["IK time(ms)"].append(dt_cu_ik * 1000.0)
-            data["Batch Size"].append(b_size)
-            data["Success Collision-Free IK"].append(success)
-            data["Success IK"].append(succ)
+            data["IK-time(ms)"].append(dt_cu_ik * 1000.0)
+            data["Batch-Size"].append(b_size)
+            data["Success-Collision-Free-IK"].append(success)
+            data["Success-IK"].append(succ)
 
-            data["Position Error(mm)"].append(p_err * 1000.0)
-            data["Orientation Error"].append(q_err)
-            data["Position Error Collision-Free IK(mm)"].append(p_err_c * 1000.0)
-            data["Orientation Error Collision-Free IK"].append(q_err_c)
+            data["Position-Error(mm)"].append(p_err * 1000.0)
+            data["Orientation-Error"].append(q_err)
+            data["Position-Error-Collision-Free-IK(mm)"].append(p_err_c * 1000.0)
+            data["Orientation-Error-Collision-Free-IK"].append(q_err_c)
 
-            data["Collision-Free IK time(ms)"].append(dt_cu_ik_cfree * 1000.0)
+            data["Collision-Free-IK-time(ms)"].append(dt_cu_ik_cfree * 1000.0)
     write_yaml(data, join_path(args.save_path, args.file_name + ".yml"))
     try:
         # Third Party
         import pandas as pd
 
         df = pd.DataFrame(data)
+        print("Reported errors are 98th percentile")
         print(df)
         df.to_csv(join_path(args.save_path, args.file_name + ".csv"))
     except ImportError:

benchmark/metrics.py

@@ -11,21 +11,32 @@
 
 # Standard Library
 from dataclasses import dataclass
-from typing import List
+from typing import List, Optional
 
 # Third Party
 import numpy as np
-from robometrics.statistics import Statistic, TrajectoryGroupMetrics, TrajectoryMetrics
+from robometrics.statistics import (
+    Statistic,
+    TrajectoryGroupMetrics,
+    TrajectoryMetrics,
+    percent_true,
+)
 
 
 @dataclass
 class CuroboMetrics(TrajectoryMetrics):
     time: float = np.inf
+    cspace_path_length: float = 0.0
+    perception_success: bool = False
+    perception_interpolated_success: bool = False
 
 
 @dataclass
 class CuroboGroupMetrics(TrajectoryGroupMetrics):
     time: float = np.inf
+    cspace_path_length: Optional[Statistic] = None
+    perception_success: float = 0.0
+    perception_interpolated_success: float = 0.0
 
     @classmethod
     def from_list(cls, group: List[CuroboMetrics]):
@@ -33,4 +44,10 @@ class CuroboGroupMetrics(TrajectoryGroupMetrics):
         successes = [m for m in unskipped if m.success]
         data = super().from_list(group)
         data.time = Statistic.from_list([m.time for m in successes])
+        data.cspace_path_length = Statistic.from_list([m.cspace_path_length for m in successes])
+        data.perception_success = percent_true([m.perception_success for m in group])
+        data.perception_interpolated_success = percent_true(
+            [m.perception_interpolated_success for m in group]
+        )
+
         return data