hofee/gen_data_curobo
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

constrained planning, robot segmentation

Browse Source
This commit is contained in:
Balakumar Sundaralingam
2024-02-22 21:45:47 -08:00
parent 88eac64edc
commit bafdf80c05
102 changed files with 12440 additions and 8112 deletions
Download Patch File Download Diff File Expand all files Collapse all files

45
benchmark/README.md
View File

@@ -12,10 +12,47 @@ This folder contains scripts to run the motion planning benchmarks.
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.
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 15ms (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`.
To get results similar to in the technical report, pass `--report_edition` to `curobo_benchmark.py`.
# Latest Results (Feb 2024)
Motion Generation on 2600 problems from motion benchmaker and motion policy networks, gives the
following values on a RTX 4090:
| Metric | Value |
|-------------------|--------------------------------------------------------------|
|Success % | 99.84 |
|Plan Time (s) | mean: 0.068 ± 0.158 median:0.042 75%: 0.055 98%: 0.246 |
|Motion Time (s) | mean: 1.169 ± 0.360 median:1.140 75%: 1.381 98%: 2.163 |
|Path Length (rad.) | mean: 3.177 ± 1.072 median:3.261 75%: 3.804 98%: 5.376 |
|Jerk | mean: 97.700 ± 48.630 median:88.993 75%: 126.092 98%: 199.540|
|Position Error (mm)| mean: 0.119 ± 0.341 median:0.027 75%: 0.091 98%: 1.039 |
## Motion Benchmaker (800 problems):
| Metric | Value |
|-------------------|--------------------------------------------------------------|
|Success % | 100 |
|Plan Time (s) | mean: 0.063 ± 0.137 median:0.042 75%: 0.044 98%: 0.206 |
|Motion Time (s) | mean: 1.429 ± 0.330 median:1.392 75%: 1.501 98%: 2.473 |
|Path Length (rad.) | mean: 3.956 ± 0.783 median:3.755 75%: 4.352 98%: 6.041 |
|Jerk | mean: 67.284 ± 27.788 median:61.853 75%: 83.337 98%: 143.118 |
|Position Error (mm)| mean: 0.079 ± 0.139 median:0.032 75%: 0.077 98%: 0.472 |
## Motion Policy Networks (1800 problems):
| Metric | Value |
|-------------------|-----------------------------------------------------------------|
|Success % | 99.77 |
|Plan Time (s) | mean: 0.068 ± 0.117 median:0.042 75%: 0.059 98%: 0.243 |
|Motion Time (s) | mean: 1.051 ± 0.306 median:1.016 75%: 1.226 98%: 1.760 |
|Path Length (rad.) | mean: 2.829 ± 1.000 median:2.837 75%: 3.482 98%: 4.905 |
|Jerk | mean: 110.610 ± 47.586 median:105.271 75%: 141.517 98%: 217.158 |
|Position Error (mm)| mean: 0.122 ± 0.343 median:0.024 75%: 0.095 98%: 1.114 |

152
benchmark/curobo_benchmark.py
View File

@@ -10,12 +10,15 @@
#
# Standard Library
import argparse
import random
from copy import deepcopy
from typing import Optional
# Third Party
import numpy as np
import torch
from robometrics.datasets import demo_raw, motion_benchmaker_raw, mpinets_raw
from tqdm import tqdm
# CuRobo
@@ -26,6 +29,7 @@ from curobo.types.math import Pose
from curobo.types.robot import RobotConfig
from curobo.types.state import JointState
from curobo.util.logger import setup_curobo_logger
from curobo.util.metrics import CuroboGroupMetrics, CuroboMetrics
from curobo.util_file import (
get_assets_path,
get_robot_configs_path,
@@ -36,28 +40,16 @@ from curobo.util_file import (
)
from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig
# torch.set_num_threads(8)
# torch.use_deterministic_algorithms(True)
torch.manual_seed(0)
# set seeds
torch.manual_seed(2)
torch.backends.cudnn.benchmark = True
torch.backends.cuda.matmul.allow_tf32 = True
torch.backends.cudnn.allow_tf32 = True
torch.backends.cuda.matmul.allow_tf32 = False
torch.backends.cudnn.allow_tf32 = False
# torch.backends.cuda.matmul.allow_tf32 = False
# torch.backends.cudnn.allow_tf32 = False
# torch.backends.cuda.matmul.allow_fp16_reduced_precision_reduction = True
np.random.seed(0)
# Standard Library
import argparse
import warnings
from typing import List, Optional
# Third Party
from metrics import CuroboGroupMetrics, CuroboMetrics
from robometrics.datasets import demo_raw, motion_benchmaker_raw, mpinets_raw
np.random.seed(2)
random.seed(2)
def plot_cost_iteration(cost: torch.Tensor, save_path="cost", title="", log_scale=False):
@@ -174,7 +166,7 @@ def load_curobo(
world_cfg = WorldConfig.from_dict(
load_yaml(join_path(get_world_configs_path(), "collision_table.yml"))
).get_obb_world()
interpolation_steps = 2000
interpolation_steps = 1000
c_checker = CollisionCheckerType.PRIMITIVE
c_cache = {"obb": n_cubes}
if mesh_mode:
@@ -189,7 +181,7 @@ def load_curobo(
K = robot_cfg_instance.kinematics.kinematics_config.joint_limits
K.position[0, :] -= 0.2
K.position[1, :] += 0.2
finetune_iters = None
finetune_iters = 300
grad_iters = None
if args.report_edition:
finetune_iters = 200
@@ -215,7 +207,7 @@ def load_curobo(
collision_activation_distance=collision_activation_distance,
trajopt_dt=0.25,
finetune_dt_scale=finetune_dt_scale,
maximum_trajectory_dt=0.1,
maximum_trajectory_dt=0.16,
)
mg = MotionGen(motion_gen_config)
mg.warmup(enable_graph=True, warmup_js_trajopt=False, parallel_finetune=parallel_finetune)
@@ -238,7 +230,7 @@ def benchmark_mb(
interpolation_dt = 0.02
# mpinets_data = True
# if mpinets_data:
file_paths = [motion_benchmaker_raw, mpinets_raw][:] # [1:]
file_paths = [motion_benchmaker_raw, mpinets_raw][:]
if args.demo:
file_paths = [demo_raw]
@@ -269,13 +261,13 @@ def benchmark_mb(
if "cage_panda" in key:
trajopt_seeds = 16
finetune_dt_scale = 0.95
collision_activation_distance = 0.01
parallel_finetune = True
if "table_under_pick_panda" in key:
tsteps = 44
tsteps = 40
trajopt_seeds = 24
finetune_dt_scale = 0.98
finetune_dt_scale = 0.95
parallel_finetune = True
if "table_pick_panda" in key:
collision_activation_distance = 0.005
@@ -297,15 +289,19 @@ def benchmark_mb(
"cubby_neutral_start",
"cubby_neutral_goal",
"dresser_neutral_start",
"dresser_neutral_goal",
"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]
# tsteps = 36
collision_activation_distance = 0.01
# trajopt_seeds = 12
scene_problems = problems[key] # [:10]
n_cubes = check_problems(scene_problems)
# print(n_cubes)
# continue
mg, robot_cfg = load_curobo(
n_cubes,
enable_debug,
@@ -327,11 +323,10 @@ 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"])
continue
plan_config = MotionGenPlanConfig(
max_attempts=100, # 100, # 00, # 00, # 100, # 00, # 000,#,00,#00, # 5000,
max_attempts=100,
enable_graph_attempt=1,
disable_graph_attempt=20,
enable_finetune_trajopt=True,
@@ -349,24 +344,13 @@ def benchmark_mb(
problem_name = "d_" + key + "_" + str(i)
# reset planner
mg.reset(reset_seed=False)
mg.reset(reset_seed=True)
if args.mesh:
world = WorldConfig.from_dict(deepcopy(problem["obstacles"])).get_mesh_world()
else:
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
# run planner
start_state = JointState.from_position(mg.tensor_args.to_device([q_start]))
@@ -379,7 +363,6 @@ def benchmark_mb(
)
if result.status == "IK Fail":
ik_fail += 1
# rint(plan_config.enable_graph, plan_config.enable_graph_attempt)
problem["solution"] = None
problem_name = key + "_" + str(i)
@@ -432,7 +415,6 @@ def benchmark_mb(
log_scale=False,
)
if result.success.item():
# print("GT: ", result.graph_time)
q_traj = result.get_interpolated_plan()
problem["goal_ik"] = q_traj.position.cpu().squeeze().numpy()[-1, :].tolist()
problem["solution"] = {
@@ -458,8 +440,7 @@ def benchmark_mb(
.tolist(),
"dt": interpolation_dt,
}
# print(problem["solution"]["position"])
# exit()
debug = {
"used_graph": result.used_graph,
"attempts": result.attempts,
@@ -487,14 +468,7 @@ def benchmark_mb(
"valid_query": result.valid_query,
}
problem["solution_debug"] = debug
# print(
# "T: ",
# result.motion_time.item(),
# result.optimized_dt.item(),
# (len(problem["solution"]["position"]) - 1 ) * result.interpolation_dt,
# result.interpolation_dt,
# )
# exit()
reached_pose = mg.compute_kinematics(result.optimized_plan[-1]).ee_pose
rot_error = goal_pose.angular_distance(reached_pose) * 100.0
if args.graph:
@@ -526,6 +500,7 @@ def benchmark_mb(
motion_time=result.motion_time.item(),
solve_time=solve_time,
cspace_path_length=path_length,
jerk=torch.max(torch.abs(result.optimized_plan.jerk)).item(),
)
if write_usd:
@@ -552,20 +527,9 @@ def benchmark_mb(
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 + ".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)
@@ -587,7 +551,6 @@ def benchmark_mb(
m_list.append(current_metrics)
all_groups.append(current_metrics)
else:
# print("invalid: " + problem_name)
debug = {
"used_graph": result.used_graph,
"attempts": result.attempts,
@@ -601,28 +564,7 @@ def benchmark_mb(
}
problem["solution_debug"] = debug
if False:
world.save_world_as_mesh(problem_name + ".obj")
q_traj = start_state # .unsqueeze(0)
# CuRobo
from curobo.util.usd_helper import UsdHelper
UsdHelper.write_trajectory_animation_with_robot_usd(
robot_cfg,
world,
start_state,
q_traj,
dt=result.interpolation_dt,
save_path=join_path("benchmark/log/usd/", problem_name) + ".usd",
interpolation_steps=1,
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():
# print("save log")
UsdHelper.write_motion_gen_log(
result,
robot_cfg,
@@ -637,8 +579,7 @@ def benchmark_mb(
write_robot_usd_path="benchmark/log/usd/assets/",
# flatten_usd=True,
)
# print(result.status)
# exit()
print(result.status)
g_m = CuroboGroupMetrics.from_list(m_list)
if not args.kpi:
@@ -653,10 +594,6 @@ def benchmark_mb(
g_m.motion_time.percent_98,
)
print(g_m.attempts)
# print("MT: ", g_m.motion_time)
# $print(ik_fail)
# exit()
# print(g_m.position_error, g_m.orientation_error)
g_m = CuroboGroupMetrics.from_list(all_groups)
if not args.kpi:
@@ -668,12 +605,8 @@ def benchmark_mb(
g_m.time.percent_75,
g_m.position_error.percent_75,
g_m.orientation_error.percent_75,
) # g_m.time, g_m.attempts)
# print("MT: ", g_m.motion_time)
)
# print(g_m.position_error, g_m.orientation_error)
# exit()
if write_benchmark:
if not mpinets_data:
write_yaml(problems, args.file_name + "_mb_solution.yaml")
@@ -681,7 +614,6 @@ def benchmark_mb(
write_yaml(problems, args.file_name + "_mpinets_solution.yaml")
all_files += all_groups
g_m = CuroboGroupMetrics.from_list(all_files)
# print(g_m.success, g_m.time, g_m.attempts, g_m.position_error, g_m.orientation_error)
print("######## FULL SET ############")
print("All: ", f"{g_m.success:2.2f}")
print("MT: ", g_m.motion_time)
@@ -690,6 +622,7 @@ def benchmark_mb(
print("ST: ", g_m.solve_time)
print("position error (mm): ", g_m.position_error)
print("orientation error(%): ", g_m.orientation_error)
print("jerk: ", g_m.jerk)
if args.kpi:
kpi_data = {
@@ -702,8 +635,6 @@ def benchmark_mb(
}
write_yaml(kpi_data, join_path(args.save_path, args.file_name + ".yml"))
# run on mb dataset:
def check_problems(all_problems):
n_cube = 0
@@ -801,12 +732,13 @@ if __name__ == "__main__":
args = parser.parse_args()
setup_curobo_logger("error")
benchmark_mb(
save_log=False,
write_usd=args.save_usd,
write_plot=args.save_plot,
write_benchmark=args.write_benchmark,
plot_cost=False,
graph_mode=args.graph,
args=args,
)
for _ in range(1):
benchmark_mb(
save_log=False,
write_usd=args.save_usd,
write_plot=args.save_plot,
write_benchmark=args.write_benchmark,
plot_cost=False,
graph_mode=args.graph,
args=args,
)

198
benchmark/curobo_nvblox_benchmark.py
View File

@@ -18,7 +18,6 @@ from typing import Optional
import matplotlib.pyplot as plt
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
@@ -34,6 +33,7 @@ from curobo.types.math import Pose
from curobo.types.robot import RobotConfig
from curobo.types.state import JointState
from curobo.util.logger import setup_curobo_logger
from curobo.util.metrics import CuroboGroupMetrics, CuroboMetrics
from curobo.util_file import (
get_assets_path,
get_robot_configs_path,
@@ -126,11 +126,12 @@ def load_curobo(
mpinets: bool = False,
graph_mode: bool = False,
cuda_graph: bool = True,
collision_activation_distance: float = 0.025,
):
robot_cfg = load_yaml(join_path(get_robot_configs_path(), "franka.yml"))["robot_cfg"]
robot_cfg["kinematics"]["collision_sphere_buffer"] = -0.0
robot_cfg["kinematics"]["collision_sphere_buffer"] = 0.0
ik_seeds = 30 # 500
ik_seeds = 30
if graph_mode:
trajopt_seeds = 4
if trajopt_seeds >= 14:
@@ -146,7 +147,7 @@ def load_curobo(
"world": {
"pose": [0, 0, 0, 1, 0, 0, 0],
"integrator_type": "tsdf",
"voxel_size": 0.014,
"voxel_size": 0.01,
}
}
}
@@ -174,16 +175,17 @@ def load_curobo(
store_ik_debug=enable_debug,
store_trajopt_debug=enable_debug,
interpolation_steps=interpolation_steps,
collision_activation_distance=0.01,
collision_activation_distance=collision_activation_distance,
trajopt_dt=0.25,
finetune_dt_scale=1.0,
maximum_trajectory_dt=0.1,
finetune_trajopt_iters=300,
)
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=True)
# create a ground truth collision checker:
config = RobotWorldConfig.load_from_config(
robot_cfg,
robot_cfg_instance,
"collision_table.yml",
collision_activation_distance=0.0,
collision_checker_type=CollisionCheckerType.PRIMITIVE,
@@ -206,7 +208,7 @@ def benchmark_mb(
# load dataset:
graph_mode = args.graph
interpolation_dt = 0.02
file_paths = [demo_raw, motion_benchmaker_raw, mpinets_raw][1:]
file_paths = [demo_raw, motion_benchmaker_raw, mpinets_raw][2:]
enable_debug = save_log or plot_cost
all_files = []
@@ -215,43 +217,73 @@ def benchmark_mb(
og_tsteps = override_tsteps
og_trajopt_seeds = 12
og_collision_activation_distance = 0.03 # 0.03
if args.graph:
og_trajopt_seeds = 4
for file_path in file_paths:
all_groups = []
mpinets_data = False
problems = file_path()
if "dresser_task_oriented" in list(problems.keys()):
mpinets_data = True
mg, robot_cfg, robot_world = load_curobo(
1,
enable_debug,
og_tsteps,
og_trajopt_seeds,
mpinets_data,
graph_mode,
not args.disable_cuda_graph,
)
for key, v in tqdm(problems.items()):
scene_problems = problems[key]
m_list = []
i = 0
i = -1
ik_fail = 0
trajopt_seeds = og_trajopt_seeds
tsteps = og_tsteps
collision_activation_distance = og_collision_activation_distance
if "dresser_task_oriented" in list(problems.keys()):
mpinets_data = True
if "cage_panda" in key:
trajopt_seeds = 16
finetune_dt_scale = 0.95
if "table_under_pick_panda" in key:
tsteps = 44
trajopt_seeds = 16
finetune_dt_scale = 0.98
if "cubby_task_oriented" in key and "merged" not in key:
trajopt_seeds = 24
finetune_dt_scale = 0.95
collision_activation_distance = 0.035
if "dresser_task_oriented" in key:
trajopt_seeds = 24
finetune_dt_scale = 0.95
collision_activation_distance = 0.035 # 0.035
if "merged_cubby_task_oriented" in key:
collision_activation_distance = 0.025 # 0.035
if "tabletop_task_oriented" in key:
collision_activation_distance = 0.025 # 0.035
if key in ["dresser_neutral_goal"]:
trajopt_seeds = 24
collision_activation_distance = og_collision_activation_distance
mg, robot_cfg, robot_world = load_curobo(
1,
enable_debug,
tsteps,
trajopt_seeds,
mpinets_data,
graph_mode,
not args.disable_cuda_graph,
collision_activation_distance=collision_activation_distance,
)
for problem in tqdm(scene_problems, leave=False):
i += 1
if problem["collision_buffer_ik"] < 0.0:
continue
plan_config = MotionGenPlanConfig(
max_attempts=10, # 00, # 00, # 100, # 00, # 000,#,00,#00, # 5000,
enable_graph_attempt=3,
disable_graph_attempt=20,
max_attempts=10,
enable_graph_attempt=1,
enable_finetune_trajopt=True,
partial_ik_opt=False,
enable_graph=graph_mode,
timeout=60,
enable_opt=not graph_mode,
parallel_finetune=True,
)
q_start = problem["start"]
@@ -265,23 +297,15 @@ def benchmark_mb(
mg.reset(reset_seed=False)
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.clear_cache()
mg.world_coll_checker.update_blox_hashes()
mg.world_coll_checker.clear_cache()
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 tqdm(range(len(m_dataset)), leave=False):
for j in tqdm(range(min(1, len(m_dataset))), leave=False):
data = m_dataset[j]
cam_obs = CameraObservation(
rgb_image=tensor_args.to_device(data["rgba"])
@@ -300,35 +324,12 @@ def benchmark_mb(
torch.cuda.synchronize()
mg.world_coll_checker.update_blox_hashes()
torch.cuda.synchronize()
# if i > 2:
# mg.world_coll_checker.clear_cache()
# mg.world_coll_checker.update_blox_hashes()
# 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 = mg.world_coll_checker.get_mesh_from_blox_layer(
"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(
start_state,
@@ -338,20 +339,6 @@ def benchmark_mb(
if result.status == "IK Fail":
ik_fail += 1
problem["solution"] = None
if write_usd or save_log:
# CuRobo
from curobo.util.usd_helper import UsdHelper
gripper_mesh = Mesh(
name="target_gripper_1",
file_path=join_path(
get_assets_path(),
"robot/franka_description/meshes/visual/hand.dae",
),
color=[0.0, 0.8, 0.1, 1.0],
pose=pose,
)
world.add_obstacle(gripper_mesh)
# get costs:
if plot_cost:
@@ -437,9 +424,12 @@ def benchmark_mb(
"valid_query": result.valid_query,
}
problem["solution_debug"] = debug
world_coll = WorldConfig.from_dict(problem["obstacles"]).get_obb_world()
# check if path is collision free w.r.t. ground truth mesh:
robot_world.world_model.clear_cache()
robot_world.update_world(world)
# robot_world.world_model.clear_cache()
robot_world.update_world(world_coll)
q_int_traj = result.get_interpolated_plan().position.unsqueeze(0)
d_int_mask = (
torch.count_nonzero(~robot_world.validate_trajectory(q_int_traj)) == 0
@@ -449,7 +439,14 @@ def benchmark_mb(
d_mask = (
torch.count_nonzero(~robot_world.validate_trajectory(q_traj)) == 0
).item()
# d_world, _ = robot_world.get_world_self_collision_distance_from_joint_trajectory(
# q_traj)
# thres_dist = robot_world.contact_distance
# in_collision = d_world.squeeze(0) > thres_dist
# d_mask = not torch.any(in_collision, dim=-1).item()
# if not d_mask:
# write_usd = True
# #print(torch.max(d_world).item(), problem_name)
current_metrics = CuroboMetrics(
skip=False,
success=True,
@@ -466,10 +463,40 @@ def benchmark_mb(
attempts=result.attempts,
motion_time=result.motion_time.item(),
solve_time=result.solve_time,
jerk=torch.max(torch.abs(result.optimized_plan.jerk)).item(),
)
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 = mg.world_coll_checker.get_mesh_from_blox_layer(
"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)
# run planner
if write_usd:
# CuRobo
from curobo.util.usd_helper import UsdHelper
q_traj = result.get_interpolated_plan()
UsdHelper.write_trajectory_animation_with_robot_usd(
@@ -486,7 +513,8 @@ def benchmark_mb(
visualize_robot_spheres=True,
# flatten_usd=True,
)
exit()
# write_usd = False
# exit()
if write_plot:
problem_name = problem_name
plot_traj(
@@ -540,6 +568,9 @@ def benchmark_mb(
}
problem["solution_debug"] = debug
if save_log and not result.success.item():
# CuRobo
from curobo.util.usd_helper import UsdHelper
UsdHelper.write_motion_gen_log(
result,
robot_cfg,
@@ -553,7 +584,7 @@ def benchmark_mb(
grid_space=2,
# flatten_usd=True,
)
# exit()
exit()
g_m = CuroboGroupMetrics.from_list(m_list)
print(
key,
@@ -566,7 +597,7 @@ def benchmark_mb(
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.perception_interpolated_success,
# g_m.orientation_error.median,
)
print(g_m.attempts)
@@ -600,6 +631,7 @@ def benchmark_mb(
print("PT:", g_m.time)
print("ST: ", g_m.solve_time)
print("accuracy: ", g_m.position_error, g_m.orientation_error)
print("Jerk: ", g_m.jerk)
if __name__ == "__main__":

15
benchmark/curobo_profile.py
View File

@@ -73,8 +73,8 @@ def load_curobo(
position_threshold=0.005,
rotation_threshold=0.05,
num_ik_seeds=30,
num_trajopt_seeds=10,
interpolation_dt=0.02,
num_trajopt_seeds=12,
interpolation_dt=0.025,
# grad_trajopt_iters=200,
store_ik_debug=enable_log,
store_trajopt_debug=enable_log,
@@ -91,7 +91,7 @@ def benchmark_mb(args):
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,
@@ -130,11 +130,18 @@ def benchmark_mb(args):
world = WorldConfig.from_dict(problem["obstacles"]).get_obb_world()
mg.update_world(world)
start_state = JointState.from_position(mg.tensor_args.to_device([q_start]))
result = mg.plan_single(
start_state,
Pose.from_list(pose),
plan_config,
)
print(result.total_time, result.solve_time)
# continue
# load obstacles
# run planner
start_state = JointState.from_position(mg.tensor_args.to_device([q_start]))
with profile(activities=[ProfilerActivity.CPU, ProfilerActivity.CUDA]) as prof:
result = mg.plan_single(
start_state,

2
benchmark/generate_nvblox_images.py
View File

@@ -43,7 +43,7 @@ def generate_images():
for key, v in tqdm(problems.items()):
scene_problems = problems[key]
i = 0
i = -1
for problem in tqdm(scene_problems, leave=False):
i += 1

3
benchmark/metrics.py
View File

@@ -29,6 +29,7 @@ class CuroboMetrics(TrajectoryMetrics):
cspace_path_length: float = 0.0
perception_success: bool = False
perception_interpolated_success: bool = False
jerk: float = np.inf
@dataclass
@@ -37,6 +38,7 @@ class CuroboGroupMetrics(TrajectoryGroupMetrics):
cspace_path_length: Optional[Statistic] = None
perception_success: float = 0.0
perception_interpolated_success: float = 0.0
jerk: float = np.inf
@classmethod
def from_list(cls, group: List[CuroboMetrics]):
@@ -49,5 +51,6 @@ class CuroboGroupMetrics(TrajectoryGroupMetrics):
data.perception_interpolated_success = percent_true(
[m.perception_interpolated_success for m in group]
)
data.jerk = Statistic.from_list([m.jerk for m in successes])
return data