fix terminal action, 10x more accurate pose reaching

Author: balakumar-s

CHANGELOG.md

@@ -10,6 +10,34 @@ its affiliates is strictly prohibited.
 -->
 # Changelog
 
+## Version 0.7.6
+
+### Changes in Default Behavior
+- Acceleration and Jerk in Output trajectory from motion_gen is not filtered. Previously, this was
+filtered with a sliding window to remove aliasing artifacts. To get previous behavior, set
+`filter_robot_command=True` in `MotionGenConfig.load_from_robot_config()`.
+- Terminal action for motion planning is now fixed from initial seed. This improves accuracy (10x).
+To get previous behavior, set `trajopt_fix_terminal_action=True` and also
+`trajopt_js_fix_terminal_action=True` in `MotionGenConfig.load_from_robot_config()`.
+- Introduce higher accuracy weights for IK in `gradient_ik_autotune.yml`. To use old file,
+pass `gradient_ik_file='gradient_ik.yml'` in `MotionGenConfig.load_from_robot_config()`. Similarly
+for IKSolver, pass `gradient_file='gradient_ik.yml'` in `IKSolverConfig.load_from_robot_config()`.
+
+### New Features
+- Add fix terminal action in quasi-netwon solvers. This keeps the final action constant (from
+initial seed) and only optimizing for the remaining states. Improved accuracy in
+reaching targets (10x improvement for Cartesian pose targets and exact reaching for joint position
+targets).
+
+
+### BugFixes & Misc.
+
+- Fix bug (opposite sign) in gradient calculation for jerk. Trajectory optimizaiton generates
+shorter motion time trajectories.
+- Fix numerical precision issues when calculating linear interpolated seeds by copying terminal
+state to final action of trajectory after interpolation.
+
+
 ## Version 0.7.5
 
 ### Changes in Default Behavior
@@ -44,6 +72,7 @@ numpy array was created instead of torch tensor.
 - Improve sphere position to voxel location calculation to match nvblox's implementation.
 - Add self collision checking support for spheres > 1024 and number of checks > 512 * 1024.
 - Fix gradient passthrough in warp batch transform kernels.
+- Remove torch.Size() initialization with device kwarg.
 
 ## Version 0.7.4
 

benchmark/curobo_benchmark.py

@@ -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.85
+    og_finetune_dt_scale = 0.8
     og_trajopt_seeds = 4
     og_parallel_finetune = True
     og_collision_activation_distance = 0.01

src/curobo/content/configs/task/finetune_trajopt.yml

@@ -54,7 +54,7 @@ cost:
     run_weight: 0.00 #1
 
   bound_cfg:
-    weight: [10000.0, 500000.0, 500.0, 500.0]
+    weight: [50000.0, 50000.0, 5000.0, 5.0]
     smooth_weight: [0.0,10000.0, 5.0, 0.0]
     run_weight_velocity: 0.0
     run_weight_acceleration: 1.0

src/curobo/content/configs/task/finetune_trajopt_slow.yml

@@ -24,7 +24,7 @@ model:
     max_dt: 0.15
   vel_scale: 1.0
   control_space: 'POSITION'
-  state_finite_difference_mode: "CENTRAL" 
+  state_finite_difference_mode: "CENTRAL"
 
   teleport_mode: False
   return_full_act_buffer: True
@@ -35,7 +35,7 @@ cost:
     weight: [2000,500000.0,30,60]
     vec_convergence: [0.0,0.0] # orientation, position, orientation metric activation, position metric activation
     terminal: True
-    run_weight: 1.0 
+    run_weight: 1.0
     use_metric: True
 
   link_pose_cfg:
@@ -46,19 +46,19 @@ cost:
     terminal: True
     run_weight: 1.0
     use_metric: True
-  
+
   cspace_cfg:
     weight: 10000.0
     terminal: True
     run_weight: 0.00 #1
 
   bound_cfg:
-    weight: [10000.0, 500000.0, 500.0, 500.0] 
+    weight: [10000.0, 500000.0, 5000.0, 5.0]
     smooth_weight: [0.0,10000.0, 5.0, 0.0]
     run_weight_velocity: 0.0
     run_weight_acceleration: 1.0
     run_weight_jerk: 1.0
-    activation_distance: [0.05,0.05,0.05,0.05] # for position, velocity, acceleration and jerk
+    activation_distance: [0.05,0.001,0.001,0.001] # for position, velocity, acceleration and jerk
     null_space_weight: [0.0]
 
   primitive_collision_cfg:
@@ -77,9 +77,9 @@ cost:
 
 
 lbfgs:
-  n_iters: 400 
-  inner_iters: 25 
-  cold_start_n_iters: null 
+  n_iters: 400
+  inner_iters: 25
+  cold_start_n_iters: null
   min_iters: 25
   line_search_scale: [0.1, 0.3, 0.7,1.0]   #   #
   fixed_iters: True

src/curobo/content/configs/task/gradient_ik.yml

@@ -68,8 +68,8 @@ lbfgs:
   min_iters: null
   line_search_scale:  [0.1, 0.3, 0.7, 1.0]
   fixed_iters: True
-  cost_convergence: 0.001
-  cost_delta_threshold: 1.0 #0.0001
+  cost_convergence: 1e-11
+  cost_delta_threshold: 1e-11 #0.0001
   cost_relative_threshold: 0.999
   epsilon: 0.01 #0.01 # used only in stable_mode
   history: 6
@@ -84,7 +84,7 @@ lbfgs:
   use_cuda_line_search_kernel: True
   use_cuda_update_best_kernel: True
   sync_cuda_time: True
-  step_scale: 1.0
+  step_scale: 0.98
   use_coo_sparse: True
   last_best: 10
   debug_info:

src/curobo/content/configs/task/gradient_ik_autotune.yml

@@ -0,0 +1,114 @@
+##
+## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+##
+## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+## property and proprietary rights in and to this material, related
+## documentation and any modifications thereto. Any use, reproduction,
+## disclosure or distribution of this material and related documentation
+## without an express license agreement from NVIDIA CORPORATION or
+## its affiliates is strictly prohibited.
+##
+
+cost:
+  bound_cfg:
+    activation_distance:
+    - 0.001
+    null_space_weight: 0.0
+    use_l2_kernel: true
+    weight: 6617.916155103846
+  cspace_cfg:
+    weight: 0.0
+  link_pose_cfg:
+    run_weight: 1.0
+    terminal: false
+    use_metric: true
+    vec_convergence:
+    - 0.0
+    - 0.0
+    vec_weight:
+    - 1.0
+    - 1.0
+    - 1.0
+    - 1.0
+    - 1.0
+    - 1.0
+    weight:
+    - 51057.936375400066
+    - 843196.9018921662
+    - 57.65743420107279
+    - 27.3008552454367
+  pose_cfg:
+    project_distance: false
+    run_weight: 1.0
+    terminal: false
+    use_metric: true
+    vec_convergence:
+    - 0.0
+    - 0.0
+    vec_weight:
+    - 1.0
+    - 1.0
+    - 1.0
+    - 1.0
+    - 1.0
+    - 1.0
+    weight:
+    - 51057.936375400066
+    - 843196.9018921662
+    - 57.65743420107279
+    - 27.3008552454367
+  primitive_collision_cfg:
+    activation_distance: 0.01
+    classify: false
+    use_sweep: false
+    weight: 6955.6913192212
+  self_collision_cfg:
+    classify: false
+    weight: 2140.4341585026104
+lbfgs:
+  cold_start_n_iters: null
+  cost_convergence: 1.0e-11
+  cost_delta_threshold: 1.0e-11
+  cost_relative_threshold: 0.999
+  debug_info:
+    visual_traj: null
+  epsilon: 0.01
+  fixed_iters: true
+  history: 6
+  horizon: 1
+  inner_iters: 25
+  last_best: 10
+  line_search_scale:
+  - 0.11626529237230242
+  - 0.5544193619969185
+  - 0.7460490833893989
+  - 1.0502486637627748
+  line_search_type: approx_wolfe
+  min_iters: null
+  n_iters: 100
+  n_problems: 1
+  stable_mode: true
+  step_scale: 0.98
+  store_debug: false
+  sync_cuda_time: true
+  use_coo_sparse: true
+  use_cuda_graph: true
+  use_cuda_kernel: true
+  use_cuda_line_search_kernel: true
+  use_cuda_update_best_kernel: true
+  use_shared_buffers_kernel: true
+model:
+  control_space: POSITION
+  dt_traj_params:
+    base_dt: 0.02
+    base_ratio: 1.0
+    max_dt: 0.25
+  horizon: 1
+  state_filter_cfg:
+    enable: false
+    filter_coeff:
+      acceleration: 0.0
+      position: 1.0
+      velocity: 1.0
+  teleport_mode: true
+  vel_scale: 1.0

src/curobo/content/configs/task/gradient_trajopt.yml

@@ -53,7 +53,7 @@ cost:
     run_weight: 0.0
 
   bound_cfg:
-    weight: [50000.0, 50000.0, 500.0,500.0]
+    weight: [50000.0, 50000.0, 5000.0,5.0]
     smooth_weight: [0.0,10000.0,5.0, 0.0]
     run_weight_velocity: 0.00
     run_weight_acceleration: 1.0

src/curobo/curobolib/cpp/tensor_step_kernel.cu

@@ -850,7 +850,7 @@ namespace Curobo
                      (1.333333333f) * g_acc[3] + (-0.083333333f) * g_acc[4]) * dt * dt +
 
 // ( g_acc[3] + g_acc[1] - (2.0) * g_acc[2]) * dt * dt +
-                    (0.5f * g_jerk[0] - g_jerk[1] + g_jerk[3] - 0.5f * g_jerk[4]) * dt * dt * dt);
+                    (-0.5f * g_jerk[0] + g_jerk[1] - g_jerk[3] + 0.5f * g_jerk[4]) * dt * dt * dt);
       }
       else if (hid == horizon - 3)
       {

src/curobo/opt/newton/newton_base.py

@@ -52,6 +52,7 @@ class NewtonOptConfig(OptimizerConfig):
     last_best: float = 0
     use_temporal_smooth: bool = False
     cost_relative_threshold: float = 0.999
+    fix_terminal_action: bool = False
 
     # use_update_best_kernel: bool
     # c_1: float
@@ -416,16 +417,21 @@ def _armijo_line_search(self, x, step_direction):
     def _approx_line_search(self, x, step_direction):
         if self.step_scale != 0.0 and self.step_scale != 1.0:
             step_direction = self.scale_step_direction(step_direction)
+        if self.fix_terminal_action and self.action_horizon > 1:
+            step_direction[..., (self.action_horizon - 1) * self.d_action :] = 0.0
         if self.line_search_type == LineSearchType.GREEDY:
-            return self._greedy_line_search(x, step_direction)
+            best_x, best_c, best_grad = self._greedy_line_search(x, step_direction)
         elif self.line_search_type == LineSearchType.ARMIJO:
-            return self._armijo_line_search(x, step_direction)
+            best_x, best_c, best_grad = self._armijo_line_search(x, step_direction)
         elif self.line_search_type in [
             LineSearchType.WOLFE,
             LineSearchType.STRONG_WOLFE,
             LineSearchType.APPROX_WOLFE,
         ]:
-            return self._wolfe_line_search(x, step_direction)
+            best_x, best_c, best_grad = self._wolfe_line_search(x, step_direction)
+        if self.fix_terminal_action and self.action_horizon > 1:
+            best_grad[..., (self.action_horizon - 1) * self.d_action :] = 0.0
+        return best_x, best_c, best_grad
 
     def check_convergence(self, cost):
         above_threshold = cost > self.cost_convergence

src/curobo/rollout/arm_reacher.py

@@ -367,8 +367,8 @@ def convergence_fn(
                     current_fn = self._link_pose_convergence[k]
                     if current_fn.enabled:
                         # get link pose
-                        current_pos = link_poses[k].position
-                        current_quat = link_poses[k].quaternion
+                        current_pos = link_poses[k].position.contiguous()
+                        current_quat = link_poses[k].quaternion.contiguous()
 
                         pose_err, pos_err, quat_err = current_fn.forward_out_distance(
                             current_pos, current_quat, self._goal_buffer, k

src/curobo/util/sample_lib.py

@@ -377,9 +377,7 @@ def get_samples(self, sample_shape, base_seed=None, **kwargs):
                 if self.sample_ratio[k] == 0.0:
                     continue
                 n_samples = round(sample_shape[0] * self.sample_ratio[k])
-                s_shape = torch.Size(
-                    [n_samples], device=self.tensor_args.device, dtype=self.tensor_args.dtype
-                )
+                s_shape = torch.Size([n_samples])
                 # if(k == 'halton' or k == 'random'):
                 samples = self.sample_fns[k](sample_shape=s_shape)
                 # else:

src/curobo/wrap/reacher/ik_solver.py

@@ -129,7 +129,7 @@ def load_from_robot_config(
         world_coll_checker=None,
         base_cfg_file: str = "base_cfg.yml",
         particle_file: str = "particle_ik.yml",
-        gradient_file: str = "gradient_ik.yml",
+        gradient_file: str = "gradient_ik_autotune.yml",
         use_cuda_graph: bool = True,
         self_collision_check: bool = True,
         self_collision_opt: bool = True,