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

Add planning to grasp API

Browse Source
This commit is contained in:
Balakumar Sundaralingam
2024-11-22 14:15:18 -08:00
parent 18e9ebd35f
commit 36ea382dab
38 changed files with 939 additions and 535 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
benchmark/curobo_benchmark.py
View File

@@ -210,6 +210,7 @@ def load_curobo(
trajopt_dt=0.25,
finetune_dt_scale=finetune_dt_scale,
high_precision=args.high_precision,
use_cuda_graph_trajopt_metrics=cuda_graph,
)
mg = MotionGen(motion_gen_config)
mg.warmup(enable_graph=True, warmup_js_trajopt=False, parallel_finetune=parallel_finetune)
@@ -484,7 +485,7 @@ def benchmark_mb(
start_state,
q_traj,
dt=result.interpolation_dt,
save_path=join_path("benchmark/log/usd/", problem_name) + ".usd",
save_path=join_path("benchmark/log/usd/", problem_name)[1:] + ".usd",
interpolation_steps=1,
write_robot_usd_path="benchmark/log/usd/assets/",
robot_usd_local_reference="assets/",
@@ -499,7 +500,7 @@ def benchmark_mb(
result.optimized_plan,
result.optimized_dt.item(),
title=problem_name,
save_path=join_path("benchmark/log/plot/", problem_name + ".png"),
save_path=join_path("benchmark/log/plot/", problem_name + ".png")[1:],
)
m_list.append(current_metrics)

25
benchmark/curobo_voxel_benchmark.py
View File

@@ -149,7 +149,7 @@ def load_curobo(
"base": {
"dims": [2.4, 2.4, 2.4],
"pose": [0, 0, 0, 1, 0, 0, 0],
"voxel_size": 0.005,
"voxel_size": 0.02,
"feature_dtype": torch.bfloat16,
},
}
@@ -294,7 +294,7 @@ def benchmark_mb(
world = WorldConfig.from_dict(problem["obstacles"])
# mg.world_coll_checker.clear_cache()
world_coll = WorldConfig.from_dict(problem["obstacles"])
world_coll = WorldConfig.from_dict(problem["obstacles"]).get_collision_check_world()
if args.mesh:
world_coll = world_coll.get_mesh_world(merge_meshes=False)
robot_world.clear_world_cache()
@@ -302,7 +302,7 @@ def benchmark_mb(
esdf = robot_world.world_model.get_esdf_in_bounding_box(
Cuboid(name="base", pose=[0, 0, 0, 1, 0, 0, 0], dims=[2.4, 2.4, 2.4]),
voxel_size=0.005,
voxel_size=0.02,
dtype=torch.float32,
)
# esdf.feature_tensor[esdf.feature_tensor < -1.0] = -1000.0
@@ -336,13 +336,16 @@ def benchmark_mb(
world.randomize_color(r=[0.5, 0.9], g=[0.2, 0.5], b=[0.0, 0.2])
coll_mesh = mg.world_coll_checker.get_mesh_in_bounding_box(
curobo_Cuboid(name="test", pose=[0, 0, 0, 1, 0, 0, 0], dims=[2, 2, 2]),
voxel_size=0.005,
curobo_Cuboid(
name="test", pose=[0, 0, 0, 1, 0, 0, 0], dims=[2.4, 2.4, 2.4]
),
voxel_size=0.02,
)
coll_mesh.color = [0.0, 0.8, 0.8, 0.2]
coll_mesh.name = "voxel_world"
# world = WorldConfig(mesh=[coll_mesh])
world.add_obstacle(coll_mesh)
# get costs:
@@ -360,7 +363,7 @@ def benchmark_mb(
plot_cost_iteration(
traj_cost,
title=problem_name + "_" + str(success) + "_" + str(dt),
save_path=join_path("benchmark/log/plot/", problem_name + "_cost"),
save_path=join_path("benchmark/log/plot/", problem_name + "_cost")[1:],
log_scale=False,
)
if "finetune_trajopt_result" in result.debug_info:
@@ -373,7 +376,7 @@ def benchmark_mb(
title=problem_name + "_" + str(success) + "_" + str(dt),
save_path=join_path(
"benchmark/log/plot/", problem_name + "_ft_cost"
),
)[1:],
log_scale=False,
)
if result.success.item():
@@ -481,7 +484,7 @@ def benchmark_mb(
start_state,
q_traj,
dt=result.interpolation_dt,
save_path=join_path("benchmark/log/usd/", problem_name) + ".usd",
save_path=join_path("benchmark/log/usd/", problem_name)[1:] + ".usd",
interpolation_steps=1,
write_robot_usd_path="benchmark/log/usd/assets/",
robot_usd_local_reference="assets/",
@@ -499,7 +502,7 @@ def benchmark_mb(
# result.get_interpolated_plan(),
# result.interpolation_dt,
title=problem_name,
save_path=join_path("benchmark/log/plot/", problem_name + ".pdf"),
save_path=join_path("benchmark/log/plot/", problem_name + ".pdf")[1:],
)
plot_traj(
# result.optimized_plan,
@@ -507,7 +510,9 @@ def benchmark_mb(
result.get_interpolated_plan(),
result.interpolation_dt,
title=problem_name,
save_path=join_path("benchmark/log/plot/", problem_name + "_int.pdf"),
save_path=join_path("benchmark/log/plot/", problem_name + "_int.pdf")[
1:
],
)
m_list.append(current_metrics)