improved joint space planning

Author: balakumar-s

CHANGELOG.md

@@ -10,10 +10,19 @@ its affiliates is strictly prohibited.
 -->
 # Changelog
 
-## Latest Commit
+## Version 0.7.3
 
 ### New Features
 - Add start state checks for world collision, self-collision, and joint limits.
+- Add finetune with dt scaling for `motion_gen.plan_single_js` to get more time optimal
+trajectories in joint space planning.
+- Improve joint space planning convergence, now succeeds in more planning problems with higher
+accuracy.
+
+### Changes in default behavior
+- Some warp kernels are now compiled based on runtime parameters (dof), causing a slowdown in load
+time for motion_gen. To avoid this slowdown, add an environment variable `CUROBO_USE_LRU_CACHE=1`
+which will cache the runtime generated kernels.
 
 ### BugFixes & Misc.
 - Fix bug in evaluator to account for dof maximum acceleration and jerk.
@@ -23,6 +32,17 @@ its affiliates is strictly prohibited.
 - Add `g_dim` check for `int` in batched planning.
 - Add `link_poses` for motion_gen.warmup() in batch planning mode.
 - Add `link_poses` as input to `batch_goalset`.
+- Add finetune js trajopt solver.
+- Pass raw velocity, acceleration, and jerk values to dt computation function to prevent
+interpolation errors from causing out of joint limit failures
+- Add `finetune_js_dt_scale` with a default value > 1.0 as joint space trajectories are
+time optimal in sparse obstacle environments.
+- Add note on deterministic behavior to website. Use lbfgs history < 12 for deterministic
+optimization results.
+- Add warning when adding a mesh with the same name as in existing cache.
+- Remove warmup for batch motion gen reacher isaac sim example.
+- Fix python examples in getting started webpage.
+- Refactor warp mesh query kernels to use a `wp.func` for signed distance queries.
 
 ## Version 0.7.2
 

benchmark/ik_benchmark.py

@@ -37,6 +37,9 @@
 )
 from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig
 
+# set seeds
+torch.manual_seed(2)
+
 torch.backends.cudnn.benchmark = True
 torch.backends.cuda.matmul.allow_tf32 = True
 torch.backends.cudnn.allow_tf32 = True
@@ -49,6 +52,7 @@ def run_full_config_collision_free_ik(
     use_cuda_graph=False,
     collision_free=True,
     high_precision=False,
+    num_seeds=12,
 ):
     tensor_args = TensorDeviceType()
     robot_data = load_yaml(join_path(get_robot_configs_path(), robot_file))["robot_cfg"]
@@ -67,7 +71,7 @@ def run_full_config_collision_free_ik(
         robot_cfg,
         world_cfg,
         position_threshold=position_threshold,
-        num_seeds=16,
+        num_seeds=num_seeds,
         self_collision_check=collision_free,
         self_collision_opt=collision_free,
         tensor_args=tensor_args,
@@ -120,10 +124,15 @@ def run_full_config_collision_free_ik(
         default="ik",
         help="File name prefix to use to save benchmark results",
     )
-
+    parser.add_argument(
+        "--num_seeds",
+        type=int,
+        default=16,
+        help="Number of seeds to use for IK",
+    )
     args = parser.parse_args()
 
-    b_list = [1, 10, 100, 2000][-1:]
+    b_list = [1, 10, 100, 500, 2000][:]
 
     robot_list = get_motion_gen_robot_list() + get_multi_arm_robot_list()[:2]
     world_file = "collision_test.yml"
@@ -142,6 +151,7 @@ def run_full_config_collision_free_ik(
         "Orientation-Error-Collision-Free-IK": [],
     }
     for robot_file in robot_list[:-1]:
+        print("running for robot: ", robot_file)
         # create a sampler with dof:
         for b_size in b_list:
             # sample test configs:
@@ -153,13 +163,15 @@ def run_full_config_collision_free_ik(
                 use_cuda_graph=True,
                 collision_free=False,
                 high_precision=args.high_precision,
+                num_seeds=args.num_seeds,
             )
             dt_cu_ik_cfree, success, p_err_c, q_err_c = run_full_config_collision_free_ik(
                 robot_file,
                 world_file,
                 batch_size=b_size,
                 use_cuda_graph=True,
                 collision_free=True,
+                num_seeds=args.num_seeds,
                 # high_precision=args.high_precision,
             )
             # print(dt_cu/b_size, dt_cu_cg/b_size)

examples/isaac_sim/batch_motion_gen_reacher.py

@@ -156,11 +156,11 @@ def main():
     default_config = robot_cfg["kinematics"]["cspace"]["retract_config"]
 
     print("warming up...")
-    motion_gen.warmup(
-        batch=n_envs,
-        batch_env_mode=True,
-        warmup_js_trajopt=False,
-    )
+    # motion_gen.warmup(
+    #     batch=n_envs,
+    #     batch_env_mode=True,
+    #     warmup_js_trajopt=False,
+    # )
 
     add_extensions(simulation_app, args.headless_mode)
     config = RobotWorldConfig.load_from_config(

examples/motion_gen_example.py

@@ -134,7 +134,7 @@ def demo_motion_gen_simple():
     )
 
     result = motion_gen.plan_single(start_state, goal_pose, MotionGenPlanConfig(max_attempts=1))
-    traj = result.get_interpolated_plan()  # result.optimized_dt has the dt between timesteps
+    traj = result.get_interpolated_plan()  # result.interpolation_dt has the dt between timesteps
     print("Trajectory Generated: ", result.success)
 
 

setup.cfg

@@ -50,12 +50,13 @@ install_requires =
   torch>=1.10
   trimesh
   yourdfpy>=0.0.53
-  warp-lang>=0.9.0
+  warp-lang>=0.11.0
   scipy>=1.7.0
   tqdm
   wheel
   importlib_resources
   scikit-image
+
 packages = find_namespace:
 package_dir =
   = src
@@ -85,10 +86,10 @@ ci =
 
 
 # this is only available in 3.8+
-smooth = 
+smooth =
   trajectory_smoothing @ https://github.com/balakumar-s/trajectory_smoothing/raw/main/dist/trajectory_smoothing-0.3-cp38-cp38-linux_x86_64.whl
 
-usd = 
+usd =
   usd-core
 
 dev =
@@ -101,12 +102,12 @@ dev =
   pytest>6.2.5
   pytest-cov
 
-isaacsim = 
+isaacsim =
   tomli
   wheel
   ninja
 
-doc = 
+doc =
   sphinx
   sphinx_rtd_theme
   graphviz>=0.20.1

src/curobo/content/configs/robot/jaco7.yml

@@ -10,7 +10,7 @@
 ##
 
 robot_cfg:
-  
+
   kinematics:
 
     urdf_path: "robot/jaco/jaco_7s.urdf"
@@ -37,13 +37,13 @@ robot_cfg:
     base_link: "root"
     ee_link: "j2s7s300_end_effector"
     link_names: null
-    
+
     collision_link_names: [
       "j2s7s300_link_base", "j2s7s300_link_1", "j2s7s300_link_2",
       "j2s7s300_link_3", "j2s7s300_link_4", "j2s7s300_link_5", "j2s7s300_link_6", "j2s7s300_link_7",
       "j2s7s300_link_finger_tip_1", "j2s7s300_link_finger_tip_2",
-      "j2s7s300_link_finger_tip_3", 
-      "j2s7s300_link_finger_3", 
+      "j2s7s300_link_finger_tip_3",
+      "j2s7s300_link_finger_3",
       "j2s7s300_link_finger_2",
       "j2s7s300_link_finger_1",
       ]
@@ -151,37 +151,60 @@ robot_cfg:
     collision_sphere_buffer: 0.005
     self_collision_ignore: {
       "j2s7s300_link_base":["j2s7s300_link_1"],
-      "j2s7s300_link_1":["j2s7s300_link_2"], 
+      "j2s7s300_link_1":["j2s7s300_link_2"],
       "j2s7s300_link_2":["j2s7s300_link_3"],
-      "j2s7s300_link_3":["j2s7s300_link_4"], 
-      "j2s7s300_link_4":["j2s7s300_link_5"], 
-      "j2s7s300_link_5":["j2s7s300_link_6"], 
-      "j2s7s300_link_6":["j2s7s300_link_7"], 
+      "j2s7s300_link_3":["j2s7s300_link_4"],
+      "j2s7s300_link_4":["j2s7s300_link_5"],
+      "j2s7s300_link_5":["j2s7s300_link_6",
+      "j2s7s300_link_finger_tip_1",
+      "j2s7s300_link_finger_tip_2",
+      "j2s7s300_link_finger_tip_3",
+      "j2s7s300_link_finger_1",
+      "j2s7s300_link_finger_2",
+      "j2s7s300_link_finger_3"],
+      "j2s7s300_link_6":["j2s7s300_link_7",
+      "j2s7s300_link_finger_tip_1",
+      "j2s7s300_link_finger_tip_2",
+      "j2s7s300_link_finger_tip_3",
+      "j2s7s300_link_finger_1",
+      "j2s7s300_link_finger_2",
+      "j2s7s300_link_finger_3"],
       "j2s7s300_link_7":[
-      "j2s7s300_link_finger_tip_1", 
+      "j2s7s300_link_finger_tip_1",
       "j2s7s300_link_finger_tip_2",
       "j2s7s300_link_finger_tip_3",
       "j2s7s300_link_finger_1",
       "j2s7s300_link_finger_2",
       "j2s7s300_link_finger_3",
       ],
-    
-      "j2s7s300_link_finger_3":["j2s7s300_link_finger_tip_3","j2s7s300_link_finger_2",
-       "j2s7s300_link_finger_1"], 
+
+      "j2s7s300_link_finger_3":
+      ["j2s7s300_link_finger_tip_3","j2s7s300_link_finger_2",
+       "j2s7s300_link_finger_1", "j2s7s300_link_finger_tip_1", "j2s7s300_link_finger_tip_2"],
+
       "j2s7s300_link_finger_2":["j2s7s300_link_finger_tip_2", "j2s7s300_link_finger_1",
-      "j2s7s300_link_finger_3"],
+      "j2s7s300_link_finger_3", "j2s7s300_link_finger_tip_3", "j2s7s300_link_finger_tip_1"],
+
       "j2s7s300_link_finger_1":["j2s7s300_link_finger_tip_1", "j2s7s300_link_finger_2",
-      "j2s7s300_link_finger_3"],
+      "j2s7s300_link_finger_3", "j2s7s300_link_finger_tip_3", "j2s7s300_link_finger_tip_2"],
+
+      "j2s7s300_link_finger_tip_1":["j2s7s300_link_finger_tip_2", "j2s7s300_link_finger_tip_3"],
+
+      "j2s7s300_link_finger_tip_2":["j2s7s300_link_finger_tip_3"],
 
       } # Dict[str, List[str]]
-    self_collision_buffer: {} # Dict[str, float]
-    
-    mesh_link_names: [ 
+    self_collision_buffer: {
+      #"j2s7s300_link_base": 0.02,
+      #"j2s7s300_link_1": 0.01,
+
+    } # Dict[str, float]
+
+    mesh_link_names: [
       "j2s7s300_link_base", "j2s7s300_link_1", "j2s7s300_link_2",
       "j2s7s300_link_3", "j2s7s300_link_4", "j2s7s300_link_5", "j2s7s300_link_6", "j2s7s300_link_7",
       "j2s7s300_link_finger_tip_1", "j2s7s300_link_finger_tip_2",
-      "j2s7s300_link_finger_tip_3", 
-      "j2s7s300_link_finger_3", 
+      "j2s7s300_link_finger_tip_3",
+      "j2s7s300_link_finger_3",
       "j2s7s300_link_finger_2",
       "j2s7s300_link_finger_1",
     ] # List[str]
@@ -191,18 +214,18 @@ robot_cfg:
     "j2s7s300_joint_finger_tip_1": 0,
     "j2s7s300_joint_finger_tip_2": 0,
     "j2s7s300_joint_finger_tip_3": 0,
-    
+
     }
 
     cspace:
-      joint_names: 
+      joint_names:
         - j2s7s300_joint_1
         - j2s7s300_joint_2
         - j2s7s300_joint_3
         - j2s7s300_joint_4
         - j2s7s300_joint_5
         - j2s7s300_joint_6
-        - j2s7s300_joint_7 
+        - j2s7s300_joint_7
         - j2s7s300_joint_finger_1
         - j2s7s300_joint_finger_2
         - j2s7s300_joint_finger_3

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

@@ -11,10 +11,10 @@
 
 
 world_collision_checker_cfg:
-  cache: null 
+  cache: null
   checker_type: "PRIMITIVE"
   max_distance: 0.1
- 
+
 
 cost:
   pose_cfg:
@@ -27,8 +27,8 @@ cost:
     weight: [0.0, 0.0]
     vec_convergence: [0.0, 0.00] # orientation, position
     terminal: False
-    
- 
+
+
   bound_cfg:
     weight: 000.0
     activation_distance: [0.0,0.0,0.0,0.0]
@@ -58,14 +58,14 @@ convergence:
     weight: [0.1, 10.0]
     vec_convergence: [0.0, 0.0] # orientation, position
     terminal: False
-    
   cspace_cfg:
     weight: 1.0
     terminal: True
     run_weight: 0.0
+    use_l2_kernel: True
   null_space_cfg:
-    weight: 1.0
+    weight: 0.001
     terminal: True
     run_weight: 1.0
-    
- 
+    use_l2_kernel: True
+

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

@@ -49,7 +49,7 @@ cost:
 
 
   cspace_cfg:
-    weight: 10000.0
+    weight: 20000.0
     terminal: True
     run_weight: 0.00 #1
 
@@ -59,7 +59,7 @@ cost:
     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:

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

@@ -40,17 +40,22 @@ cost:
     terminal: False
     use_metric: True
     run_weight: 1.0
+
   cspace_cfg:
     weight: 0.000
+
   bound_cfg:
     weight: 5000.0
     activation_distance: [0.001]
-    null_space_weight: [0.1]
+    null_space_weight: [0.001]
+    use_l2_kernel: True
+
   primitive_collision_cfg:
     weight: 5000.0
     use_sweep: False
     classify: False
     activation_distance: 0.01
+
   self_collision_cfg:
     weight: 5000.0
     classify: False