fix terminal action, 10x more accurate pose reaching

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 
+  n_iters: 400
-  inner_iters: 25 
+  inner_iters: 25
-  cold_start_n_iters: null 
+  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_convergence: 1e-11
-  cost_delta_threshold: 1.0 #0.0001
+  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 @@ class NewtonOptBase(Optimizer, NewtonOptConfig):
     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 @@ class ArmReacher(ArmBase, ArmReacherConfig):
                     current_fn = self._link_pose_convergence[k]
                     if current_fn.enabled:
                         # get link pose
-                        current_pos = link_poses[k].position
+                        current_pos = link_poses[k].position.contiguous()
-                        current_quat = link_poses[k].quaternion
+                        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 @@ class SampleLib(BaseSampleLib):
                 if self.sample_ratio[k] == 0.0:
                     continue
                 n_samples = round(sample_shape[0] * self.sample_ratio[k])
-                s_shape = torch.Size(
+                s_shape = torch.Size([n_samples])
-                    [n_samples], device=self.tensor_args.device, dtype=self.tensor_args.dtype
-                )
                 # 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 @@ class IKSolverConfig:
         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,

src/curobo/wrap/reacher/motion_gen.py

@@ -180,7 +180,7 @@ class MotionGenConfig:
         world_coll_checker=None,
         base_cfg_file: str = "base_cfg.yml",
         particle_ik_file: str = "particle_ik.yml",
-        gradient_ik_file: str = "gradient_ik.yml",
+        gradient_ik_file: str = "gradient_ik_autotune.yml",
         graph_file: str = "graph.yml",
         particle_trajopt_file: str = "particle_trajopt.yml",
         gradient_trajopt_file: str = "gradient_trajopt.yml",
@@ -202,7 +202,7 @@ class MotionGenConfig:
         traj_evaluator_config: Optional[TrajEvaluatorConfig] = None,
         traj_evaluator: Optional[TrajEvaluator] = None,
         minimize_jerk: bool = True,
-        filter_robot_command: bool = True,
+        filter_robot_command: bool = False,
         use_gradient_descent: bool = False,
         collision_cache: Optional[Dict[str, int]] = None,
         n_collision_envs: Optional[int] = None,
@@ -243,6 +243,8 @@ class MotionGenConfig:
         graph_seed: int = 1531,
         high_precision: bool = False,
         use_cuda_graph_trajopt_metrics: bool = False,
+        trajopt_fix_terminal_action: bool = True,
+        trajopt_js_fix_terminal_action: bool = True,
     ):
         """Create a motion generation configuration from robot and world configuration.
 
@@ -478,6 +480,14 @@ class MotionGenConfig:
                 trajectories after trajectory optimization. If interpolation_buffer is smaller
                 than interpolated trajectory, then the buffers will be re-created. This can cause
                 existing cuda graph to be invalid.
+            trajopt_fix_terminal_action: Flag to disable optimizing for final state. When true,
+                the final state is unchanged from initial seed. When false, terminal state can
+                change based on cost. Setting to False will lead to worse accuracy at target
+                pose (>0.1mm). Setting to True can achieve < 0.01mm accuracy.
+            trajopt_js_fix_terminal_action: Flag to disable optimizing for final state for joint
+                space target planning. When true, the final state is unchanged from initial seed.
+                When false, terminal state can change based on cost. Setting to False will lead to
+                worse accuracy at target joint configuration.
 
         Returns:
             MotionGenConfig: Instance of motion generation configuration.
@@ -728,6 +738,7 @@ class MotionGenConfig:
             optimize_dt=optimize_dt,
             project_pose_to_goal_frame=project_pose_to_goal_frame,
             use_cuda_graph_metrics=use_cuda_graph_trajopt_metrics,
+            fix_terminal_action=trajopt_fix_terminal_action,
         )
         trajopt_solver = TrajOptSolver(trajopt_cfg)
 
@@ -770,6 +781,7 @@ class MotionGenConfig:
             optimize_dt=optimize_dt,
             num_seeds=num_trajopt_noisy_seeds,
             use_cuda_graph_metrics=use_cuda_graph_trajopt_metrics,
+            fix_terminal_action=trajopt_js_fix_terminal_action,
         )
         js_trajopt_solver = TrajOptSolver(js_trajopt_cfg)
 
@@ -813,6 +825,7 @@ class MotionGenConfig:
             optimize_dt=optimize_dt,
             project_pose_to_goal_frame=project_pose_to_goal_frame,
             use_cuda_graph_metrics=use_cuda_graph_trajopt_metrics,
+            fix_terminal_action=trajopt_fix_terminal_action,
         )
 
         finetune_trajopt_solver = TrajOptSolver(finetune_trajopt_cfg)
@@ -856,6 +869,7 @@ class MotionGenConfig:
             optimize_dt=optimize_dt,
             num_seeds=num_trajopt_noisy_seeds,
             use_cuda_graph_metrics=use_cuda_graph_trajopt_metrics,
+            fix_terminal_action=trajopt_js_fix_terminal_action,
         )
         finetune_js_trajopt_solver = TrajOptSolver(finetune_js_trajopt_cfg)
 

src/curobo/wrap/reacher/trajopt.py

@@ -149,6 +149,7 @@ class TrajOptSolverConfig:
         optimize_dt: bool = True,
         project_pose_to_goal_frame: bool = True,
         use_cuda_graph_metrics: bool = False,
+        fix_terminal_action: bool = False,
     ):
         """Load TrajOptSolver configuration from robot configuration.
 
@@ -318,8 +319,6 @@ class TrajOptSolverConfig:
         if not self_collision_check:
             base_config_data["constraint"]["self_collision_cfg"]["weight"] = 0.0
             self_collision_opt = False
-        if not minimize_jerk:
-            filter_robot_command = False
 
         if collision_checker_type is not None:
             base_config_data["world_collision_checker_cfg"]["checker_type"] = collision_checker_type
@@ -340,6 +339,7 @@ class TrajOptSolverConfig:
         base_config_data["convergence"]["pose_cfg"]["project_distance"] = project_pose_to_goal_frame
         config_data["cost"]["pose_cfg"]["project_distance"] = project_pose_to_goal_frame
         grad_config_data["cost"]["pose_cfg"]["project_distance"] = project_pose_to_goal_frame
+        grad_config_data["lbfgs"]["fix_terminal_action"] = fix_terminal_action
 
         config_data["model"]["horizon"] = traj_tsteps
         grad_config_data["model"]["horizon"] = traj_tsteps
@@ -1543,6 +1543,10 @@ class TrajOptSolver(TrajOptSolverConfig):
         edges = torch.cat((start_q, end_q), dim=1)
 
         seed = self.delta_vec @ edges
+
+        # Setting final state to end_q explicitly to avoid matmul numerical precision issues.
+        seed[..., -1:, :] = end_q
+
         return seed
 
     def get_start_seed(self, start_state) -> torch.Tensor:
@@ -1725,6 +1729,10 @@ class TrajOptSolver(TrajOptSolverConfig):
         bias_q = self.bias_node.view(-1, 1, self.dof).repeat(start_q.shape[0], 1, 1)
         edges = torch.cat((start_q, bias_q, end_q), dim=1)
         seed = self.waypoint_delta_vec @ edges
+
+        # Setting final state to end_q explicitly to avoid matmul numerical precision issues.
+        seed[..., -1:, :] = end_q
+
         return seed
 
     @profiler.record_function("trajopt/interpolation")

tests/motion_gen_js_test.py

@@ -66,3 +66,4 @@ def test_motion_gen_plan_js_delta(motion_gen_str, delta, request):
     )
     result = motion_gen.plan_single_js(start_state, goal_state, MotionGenPlanConfig(max_attempts=1))
     assert result.success.item()
+    assert result.cspace_error < 0.0001