Improved precision, quality and js planner.

kelvin34501 · Apr 11, 2024 · d6e600c · d6e600c
1 parent 774dcfd
commit d6e600c
Show file tree

Hide file tree

Showing 51 changed files with 1,907 additions and 833 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -10,10 +10,23 @@ its affiliates is strictly prohibited.
 -->
 # Changelog
 
-## Latest Commit
+## Version 0.7.1
 
 ### New Features
 - Add mimic joint parsing and optimization support. Check `ur5e_robotiq_2f_140.yml`.
+- Add `finetune_dt_scale` as a parameter to `MotionGenPlanConfig` to dynamically change the 
+time-optimal scaling on a per problem instance.
+- `MotionGen.plan_single()` will now try finetuning in a for-loop, with larger and larger dt 
+until convergence. This also warm starts from previous failure.
+- Add `high_precision` mode to `MotionGenConfig` to support `<1mm` convergence.
+
+### Changes in default behavior
+- collision_sphere_buffer now supports having offset per link. Also, collision_sphere_buffer only 
+applies to world collision while self_collision_buffer applies for self collision. Previously, 
+self_collision_buffer was added on top of collision_sphere_buffer.
+- `TrajEvaluatorConfig` cannot be initialized without dof as now per-joint jerk and acceleration 
+limits are used. Use `TrajEvaluatorConfig.from_basic()` to initialize similar to previous behavior.
+- `finetune_dt_scale` default value is 0.9 from 0.95.
 
 ### BugFixes & Misc.
 - Fix bug in `WorldVoxelCollision` where `env_query_idx` was being overwritten.
@@ -26,6 +39,21 @@ its affiliates is strictly prohibited.
 - Added flag to sample from ik seeder instead of `rollout_fn` sampler.
 - Added ik startup profiler to `benchmark/curobo_python_profile.py`.
 - Reduced branching in Kinematics kernels and added mimic joint computations.
+- Add init_cache to WorldVoxelCollision to create cache for Mesh and Cuboid obstacles.
+- `TrajEvaluator` now uses per-joint acceleration and jerk limits.
+- Fixed regression in `batch_motion_gen_reacher.py` example where robot's position was not being 
+set correctly.
+- Switched from smooth l2 to l2 for BoundCost as that gives better convergence.
+- `requires_grad` is explicitly stored in a varaible before `tensor.detach()` in warp kernel calls
+as this can get set to False in some instances.
+- Fix dt update in `MotionGen.plan_single_js()` where dt was not reset after finetunestep, causing
+joint space planner to fail often.
+- Improve joint space planner success by changing smooth l2 distance cost to l2 distance. Also, 
+added fallback to graph planner when linear path is not possible.
+- Retuned weigths for IKSolver, now 98th percentile accuracy is 10 micrometers wtih 16 seeds 
+(vs 24 seeds previously).
+- Switch float8 precision check from `const` to macro to avoid compile errors in older nvcc, this
+fixes docker build issues for isaac sim 2023.1.0.
 
 ## Version 0.7.0
 ### Changes in default behavior

diff --git a/benchmark/curobo_benchmark.py b/benchmark/curobo_benchmark.py
@@ -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
+            scene_problems = problems[key]
+            n_cubes = check_problems(scene_problems)
 
-            if "table_under_pick_panda" in key:
+            if "cubby_task_oriented" in key and "merged" not 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][:]
-            n_cubes = check_problems(scene_problems)
             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,32 +570,64 @@ 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")
             else:
                 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 @@ def check_problems(all_problems):
         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()
 

diff --git a/benchmark/curobo_nvblox_benchmark.py b/benchmark/curobo_nvblox_benchmark.py
@@ -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