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

Improved precision, quality and js planner.

Browse Source
This commit is contained in:
Balakumar Sundaralingam
2024-04-11 13:19:01 -07:00
parent 774dcfd609
commit d6e600c88c
51 changed files with 2128 additions and 1054 deletions
Download Patch File Download Diff File Expand all files Collapse all files

103
benchmark/curobo_benchmark.py
View File

@@ -152,7 +152,6 @@ def load_curobo(
robot_cfg["kinematics"]["collision_link_names"].remove("attached_object")
robot_cfg["kinematics"]["ee_link"] = "panda_hand"
# del robot_cfg["kinematics"]
if ik_seeds is None:
ik_seeds = 32
@@ -211,6 +210,7 @@ def load_curobo(
trajopt_dt=0.25,
finetune_dt_scale=finetune_dt_scale,
maximum_trajectory_dt=0.15,
high_precision=args.high_precision,
)
mg = MotionGen(motion_gen_config)
mg.warmup(enable_graph=True, warmup_js_trajopt=False, parallel_finetune=parallel_finetune)
@@ -240,7 +240,7 @@ def benchmark_mb(
og_tsteps = 32
if override_tsteps is not None:
og_tsteps = override_tsteps
og_finetune_dt_scale = 0.9
og_finetune_dt_scale = 0.85
og_trajopt_seeds = 4
og_parallel_finetune = True
og_collision_activation_distance = 0.01
@@ -252,6 +252,7 @@ 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
@@ -260,23 +261,12 @@ def benchmark_mb(
parallel_finetune = og_parallel_finetune
ik_seeds = og_ik_seeds
if "cage_panda" in key:
trajopt_seeds = 8
if "table_under_pick_panda" in key:
trajopt_seeds = 8
finetune_dt_scale = 0.95
if key == "cubby_task_oriented": # or key == "merged_cubby_task_oriented":
trajopt_seeds = 16
finetune_dt_scale = 0.95
if "dresser_task_oriented" in key:
trajopt_seeds = 16
finetune_dt_scale = 0.95
scene_problems = problems[key][:]
scene_problems = problems[key]
n_cubes = check_problems(scene_problems)
if "cubby_task_oriented" in key and "merged" not in key:
trajopt_seeds = 8
mg, robot_cfg = load_curobo(
n_cubes,
enable_debug,
@@ -302,7 +292,7 @@ def benchmark_mb(
continue
plan_config = MotionGenPlanConfig(
max_attempts=20,
max_attempts=20, # 20,
enable_graph_attempt=1,
disable_graph_attempt=10,
enable_finetune_trajopt=not args.no_finetune,
@@ -312,6 +302,7 @@ def benchmark_mb(
enable_opt=not graph_mode,
need_graph_success=force_graph,
parallel_finetune=parallel_finetune,
finetune_dt_scale=finetune_dt_scale,
)
q_start = problem["start"]
pose = (
@@ -579,16 +570,31 @@ def benchmark_mb(
g_m = CuroboGroupMetrics.from_list(all_groups)
if not args.kpi:
print(
"All: ",
f"{g_m.success:2.2f}",
g_m.motion_time.percent_98,
g_m.time.mean,
g_m.time.percent_75,
g_m.position_error.percent_75,
g_m.orientation_error.percent_75,
)
try:
from tabulate import tabulate
headers = ["Metric", "Value"]
table = [
["Success %", f"{g_m.success:2.2f}"],
["Plan Time (s)", g_m.time],
["Motion Time(s)", g_m.motion_time],
["Path Length (rad.)", g_m.cspace_path_length],
["Jerk", g_m.jerk],
["Position Error (mm)", g_m.position_error],
]
print(tabulate(table, headers, tablefmt="grid"))
except ImportError:
print(
"All: ",
f"{g_m.success:2.2f}",
g_m.motion_time.percent_98,
g_m.time.mean,
g_m.time.percent_75,
g_m.position_error.percent_75,
g_m.orientation_error.percent_75,
)
if write_benchmark:
if not mpinets_data:
write_yaml(problems, args.file_name + "_mb_solution.yaml")
@@ -596,15 +602,32 @@ def benchmark_mb(
write_yaml(problems, args.file_name + "_mpinets_solution.yaml")
all_files += all_groups
g_m = CuroboGroupMetrics.from_list(all_files)
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)
print("orientation error(%): ", g_m.orientation_error)
print("jerk: ", g_m.jerk)
try:
from tabulate import tabulate
headers = ["Metric", "Value"]
table = [
["Success %", f"{g_m.success:2.2f}"],
["Plan Time (s)", g_m.time],
["Motion Time(s)", g_m.motion_time],
["Path Length (rad.)", g_m.cspace_path_length],
["Jerk", g_m.jerk],
["Position Error (mm)", g_m.position_error],
]
print(tabulate(table, headers, tablefmt="grid"))
except ImportError:
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)
print("orientation error(%): ", g_m.orientation_error)
print("jerk: ", g_m.jerk)
if args.kpi:
kpi_data = {
@@ -716,6 +739,12 @@ if __name__ == "__main__":
help="When True, runs benchmark with parameters for jetson",
default=False,
)
parser.add_argument(
"--high_precision",
action="store_true",
help="When True, runs benchmark with parameters for jetson",
default=False,
)
args = parser.parse_args()

15
benchmark/curobo_nvblox_benchmark.py
View File

@@ -147,7 +147,7 @@ def load_curobo(
"world": {
"pose": [0, 0, 0, 1, 0, 0, 0],
"integrator_type": "tsdf",
"voxel_size": 0.01,
"voxel_size": 0.02,
}
}
}
@@ -177,9 +177,9 @@ def load_curobo(
interpolation_steps=interpolation_steps,
collision_activation_distance=collision_activation_distance,
trajopt_dt=0.25,
finetune_dt_scale=1.0,
maximum_trajectory_dt=0.1,
finetune_trajopt_iters=300,
finetune_dt_scale=0.9,
maximum_trajectory_dt=0.15,
finetune_trajopt_iters=200,
)
mg = MotionGen(motion_gen_config)
mg.warmup(enable_graph=True, warmup_js_trajopt=False, parallel_finetune=True)
@@ -208,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][2:]
file_paths = [demo_raw, motion_benchmaker_raw, mpinets_raw][1:2]
enable_debug = save_log or plot_cost
all_files = []
@@ -237,8 +237,9 @@ def benchmark_mb(
mpinets_data = True
if "cage_panda" in key:
trajopt_seeds = 16
finetune_dt_scale = 0.95
trajopt_seeds = 8
else:
continue
if "table_under_pick_panda" in key:
tsteps = 44
trajopt_seeds = 16

98
benchmark/curobo_voxel_benchmark.py
View File

@@ -133,7 +133,7 @@ def load_curobo(
robot_cfg = load_yaml(join_path(get_robot_configs_path(), "franka.yml"))["robot_cfg"]
robot_cfg["kinematics"]["collision_sphere_buffer"] = -0.00
ik_seeds = 24
ik_seeds = 32
if graph_mode:
trajopt_seeds = 4
if trajopt_seeds >= 14:
@@ -253,21 +253,9 @@ def benchmark_mb(
collision_activation_distance = og_collision_activation_distance
finetune_dt_scale = 0.9
parallel_finetune = True
if "cage_panda" in key:
if "cubby_task_oriented" in key and "merged" not in key:
trajopt_seeds = 8
if "table_under_pick_panda" in key:
trajopt_seeds = 8
finetune_dt_scale = 0.98
if key == "cubby_task_oriented":
trajopt_seeds = 16
finetune_dt_scale = 0.98
if "dresser_task_oriented" in key:
trajopt_seeds = 16
finetune_dt_scale = 0.98
mg, robot_cfg, robot_world = load_curobo(
0,
enable_debug,
@@ -285,16 +273,12 @@ def benchmark_mb(
i += 1
if problem["collision_buffer_ik"] < 0.0:
continue
plan_config = MotionGenPlanConfig(
max_attempts=10,
max_attempts=20,
enable_graph_attempt=1,
enable_finetune_trajopt=True,
disable_graph_attempt=10,
partial_ik_opt=False,
enable_graph=graph_mode,
timeout=60,
enable_opt=not graph_mode,
parallel_finetune=True,
)
q_start = problem["start"]
@@ -593,17 +577,31 @@ def benchmark_mb(
)
print(g_m.attempts)
g_m = CuroboGroupMetrics.from_list(all_groups)
print(
"All: ",
f"{g_m.success:2.2f}",
g_m.motion_time.percent_98,
g_m.time.mean,
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)
try:
from tabulate import tabulate
headers = ["Metric", "Value"]
table = [
["Success %", f"{g_m.success:2.2f}"],
["Plan Time (s)", g_m.time],
["Motion Time(s)", g_m.motion_time],
["Path Length (rad.)", g_m.cspace_path_length],
["Jerk", g_m.jerk],
["Position Error (mm)", g_m.position_error],
]
print(tabulate(table, headers, tablefmt="grid"))
except ImportError:
print(
"All: ",
f"{g_m.success:2.2f}",
g_m.motion_time.percent_98,
g_m.time.mean,
g_m.time.percent_75,
g_m.position_error.percent_75,
g_m.orientation_error.percent_75,
)
if write_benchmark:
if not mpinets_data:
write_yaml(problems, "mb_curobo_solution_voxel.yaml")
@@ -612,17 +610,33 @@ def benchmark_mb(
all_files += all_groups
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)
print("accuracy: ", g_m.position_error, g_m.orientation_error)
print("Jerk: ", g_m.jerk)
try:
from tabulate import tabulate
headers = ["Metric", "Value"]
table = [
["Success %", f"{g_m.success:2.2f}"],
["Plan Time (s)", g_m.time],
["Motion Time(s)", g_m.motion_time],
["Path Length (rad.)", g_m.cspace_path_length],
["Jerk", g_m.jerk],
["Position Error (mm)", g_m.position_error],
]
print(tabulate(table, headers, tablefmt="grid"))
except ImportError:
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)
print("accuracy: ", g_m.position_error, g_m.orientation_error)
print("Jerk: ", g_m.jerk)
if __name__ == "__main__":

16
benchmark/ik_benchmark.py
View File

@@ -67,7 +67,7 @@ def run_full_config_collision_free_ik(
robot_cfg,
world_cfg,
position_threshold=position_threshold,
num_seeds=30,
num_seeds=16,
self_collision_check=collision_free,
self_collision_opt=collision_free,
tensor_args=tensor_args,
@@ -123,7 +123,7 @@ if __name__ == "__main__":
args = parser.parse_args()
b_list = [1, 10, 100, 1000][-1:]
b_list = [1, 10, 100, 2000][-1:]
robot_list = get_motion_gen_robot_list() + get_multi_arm_robot_list()[:2]
world_file = "collision_test.yml"
@@ -141,7 +141,7 @@ if __name__ == "__main__":
"Position-Error-Collision-Free-IK(mm)": [],
"Orientation-Error-Collision-Free-IK": [],
}
for robot_file in robot_list[:1]:
for robot_file in robot_list[:-1]:
# create a sampler with dof:
for b_size in b_list:
# sample test configs:
@@ -176,13 +176,21 @@ if __name__ == "__main__":
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"))
try:
from tabulate import tabulate
print(tabulate(df, headers="keys", tablefmt="grid"))
except ImportError:
print(df)
pass
except ImportError:
pass