improved joint space planning

tests/conftest.py

@@ -13,3 +13,4 @@
 import os
 
 os.environ["CUROBO_TORCH_COMPILE_DISABLE"] = str(1)
+os.environ["CUROBO_USE_LRU_CACHE"] = str(1)

tests/motion_gen_js_test.py

@@ -11,6 +11,7 @@
 
 
 # Third Party
+import pytest
 import torch
 
 # CuRobo
@@ -18,21 +19,20 @@ from curobo.types.robot import JointState
 from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig
 
 
-def test_motion_gen_plan_js():
+@pytest.fixture(scope="module")
+def motion_gen():
     world_file = "collision_table.yml"
     robot_file = "ur5e.yml"
     motion_gen_config = MotionGenConfig.load_from_robot_config(
         robot_file,
         world_file,
-        trajopt_tsteps=32,
-        use_cuda_graph=False,
-        num_trajopt_seeds=4,
-        fixed_iters_trajopt=True,
-        evaluate_interpolated_trajectory=True,
     )
-    motion_gen = MotionGen(motion_gen_config)
-    motion_gen.warmup(warmup_js_trajopt=True)
+    motion_gen_instance = MotionGen(motion_gen_config)
+    motion_gen_instance.warmup(warmup_js_trajopt=True)
+    return motion_gen_instance
 
+
+def test_motion_gen_plan_js(motion_gen):
     retract_cfg = motion_gen.get_retract_config()
 
     start_state = JointState.from_position(retract_cfg.view(1, -1).clone())
@@ -42,3 +42,27 @@ def test_motion_gen_plan_js():
     )
     result = motion_gen.plan_single_js(start_state, goal_state, MotionGenPlanConfig(max_attempts=1))
     assert result.success.item()
+
+
+@pytest.mark.parametrize(
+    "motion_gen_str, delta",
+    [
+        ("motion_gen", 0.1),
+        ("motion_gen", 0.2),
+        ("motion_gen", 0.3),
+        ("motion_gen", 0.4),
+        ("motion_gen", 0.5),
+    ],
+)
+def test_motion_gen_plan_js_delta(motion_gen_str, delta, request):
+    motion_gen = request.getfixturevalue(motion_gen_str)
+    start_state = JointState.from_position(
+        motion_gen.get_retract_config().view(1, -1).clone(),
+        joint_names=motion_gen.joint_names,
+    )
+    goal_state = JointState.from_position(
+        motion_gen.get_retract_config().view(1, -1).clone() + delta,
+        joint_names=motion_gen.joint_names,
+    )
+    result = motion_gen.plan_single_js(start_state, goal_state, MotionGenPlanConfig(max_attempts=1))
+    assert result.success.item()

tests/voxel_collision_test.py

@@ -40,8 +40,8 @@ def get_world_model(single_object: bool = False):
         world_model = WorldConfig.from_dict(
             {
                 "cuboid": {
-                    "block": {"dims": [0.5, 0.5, 0.5], "pose": [-0.25, 0, 0, 1, 0, 0, 0]},
-                    "block1": {"dims": [0.1, 0.2, 0.5], "pose": [0.25, 0.1, 0, 1, 0, 0, 0]},
+                    "block2": {"dims": [0.5, 0.5, 0.5], "pose": [-0.25, 0, 0, 1, 0, 0, 0]},
+                    "block3": {"dims": [0.1, 0.2, 0.5], "pose": [0.25, 0.1, 0, 1, 0, 0, 0]},
                 }
             }
         )
@@ -62,7 +62,7 @@ def world_collision(request):
                 if not request.param[0]
                 else CollisionCheckerType.MESH
             ),
-            "max_distance": 5.0,
+            "max_distance": 1.0,
             "n_envs": 1,
         },
         world_model,