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

fix terminal action, 10x more accurate pose reaching

Browse Source
This commit is contained in:
Balakumar Sundaralingam
2024-11-22 14:32:43 -08:00
parent 36ea382dab
commit 2fbffc3522
15 changed files with 198 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files

29
CHANGELOG.md
View File

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

2
benchmark/curobo_benchmark.py
View File

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

2
src/curobo/content/configs/task/finetune_trajopt.yml
View File

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

16
src/curobo/content/configs/task/finetune_trajopt_slow.yml
View File

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

6
src/curobo/content/configs/task/gradient_ik.yml
View File

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

114
src/curobo/content/configs/task/gradient_ik_autotune.yml Normal file
View File

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

2
src/curobo/content/configs/task/gradient_trajopt.yml
View File

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

2
src/curobo/curobolib/cpp/tensor_step_kernel.cu
View File

@@ -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)
{

12
src/curobo/opt/newton/newton_base.py
View File

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

4
src/curobo/rollout/arm_reacher.py
View File

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

4
src/curobo/util/sample_lib.py
View File

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

2
src/curobo/wrap/reacher/ik_solver.py
View File

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

18
src/curobo/wrap/reacher/motion_gen.py
View File

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

12
src/curobo/wrap/reacher/trajopt.py
View File

@@ -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")

1
tests/motion_gen_js_test.py
View File

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