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

update to 0.6.2

Browse Source
This commit is contained in:
Balakumar Sundaralingam
2023-12-15 02:01:33 -08:00
parent d85ae41fba
commit 58958bbcce
105 changed files with 2514 additions and 934 deletions
Download Patch File Download Diff File Expand all files Collapse all files

49
benchmark/ik_benchmark.py
View File

@@ -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: