add isaac sim 2023.1.1 partial support

CHANGELOG.md

@@ -58,6 +58,8 @@ of signed distances (SDF) to compute collision avoidance metrics. Documentation
 ``benchmark/curobo_voxel_benchmark.py`` for an example.
 - Add API for ESDF computation from world representations, see 
 ``WorldCollision.get_esdf_in_bounding_box()``.
+- Add partial support for isaac sim 2023.1.1. Most examples run for UR robots. `Franka Panda` is 
+unstable.
 
 ### BugFixes & Misc.
 - refactored wp.index() instances to `[]` to avoid errors in future releases of warp.
@@ -95,10 +97,12 @@ trajectory optimization fails.
 - Added unit tests for collision checking functions.
 - Fix bug in linear interpolation which was not reading the new ``optimized_dt`` to interpolate
 velocity, acceleration, and jerk.
+- Remove torch.jit.script wrapper for lbfgs as it causes TorchScript error if history is different
+between trajopt and finetune_trajopt.
 
 
 ### Known Bugs (WIP)
-- Examples don't run in Isaac Sim 2023.1.1 due to behavior change in urdf importer.
+- `Franka Panda` robot loading from urdf in isaac sim 2023.1.1 is unstable. 
 
 ## Version 0.6.2
 ### New Features

examples/isaac_sim/helper.py

@@ -19,6 +19,7 @@ from omni.isaac.core import World
 from omni.isaac.core.materials import OmniPBR
 from omni.isaac.core.objects import cuboid
 from omni.isaac.core.robots import Robot
+from pxr import UsdPhysics
 
 # CuRobo
 from curobo.util.logger import log_warn
@@ -100,24 +101,28 @@ def add_robot_to_scene(
         import_config,
         dest_path,
     )
-    # prim_path = omni.usd.get_stage_next_free_path(
+
-    # my_world.scene.stage, str(my_world.scene.stage.GetDefaultPrim().GetPath()) + robot_path, False)
+    base_link_name = robot_config["kinematics"]["base_link"]
-    # print(prim_path)
+
-    # robot_prim = my_world.scene.stage.OverridePrim(prim_path)
-    # robot_prim.GetReferences().AddReference(dest_path)
     robot_p = Robot(
-        prim_path=robot_path,
+        prim_path=robot_path + "/" + base_link_name,
         name=robot_name,
         position=position,
     )
-    if ISAAC_SIM_23:
+    if False and ISAAC_SIM_23:  # this doesn't work in isaac sim 2023.1.1
         robot_p.set_solver_velocity_iteration_count(0)
         robot_p.set_solver_position_iteration_count(44)
 
         my_world._physics_context.set_solver_type("PGS")
 
+    if ISAAC_SIM_23:  # fix to load robot correctly in isaac sim 2023.1.1
+        robot_prim = robot_p.prim
+        stage = robot_prim.GetStage()
+        linkp = stage.GetPrimAtPath(robot_path + "/" + base_link_name)
+        mass = UsdPhysics.MassAPI(linkp)
+        mass.GetMassAttr().Set(0)
     robot = my_world.scene.add(robot_p)
-    robot_path = robot.prim_path
+    # robot_path = robot.prim_path
     return robot, robot_path
 
 

src/curobo/cuda_robot_model/types.py

@@ -437,7 +437,7 @@ class SelfCollisionKinematicsConfig:
     checks_per_thread: int = 32
 
 
-@dataclass(frozen=True)
+@dataclass
 class CudaRobotModelState:
     """Dataclass that stores kinematics information."""
 

src/curobo/opt/newton/lbfgs.py

@@ -25,13 +25,13 @@ from curobo.util.torch_utils import get_torch_jit_decorator
 
 
 # kernel for l-bfgs:
-@get_torch_jit_decorator()
+@get_torch_jit_decorator(only_valid_for_compile=True)
 def jit_lbfgs_compute_step_direction(
-    alpha_buffer,
+    alpha_buffer: torch.Tensor,
-    rho_buffer,
+    rho_buffer: torch.Tensor,
-    y_buffer,
+    y_buffer: torch.Tensor,
-    s_buffer,
+    s_buffer: torch.Tensor,
-    grad_q,
+    grad_q: torch.Tensor,
     m: int,
     epsilon: float,
     stable_mode: bool = True,

src/curobo/rollout/cost/pose_cost.py

@@ -224,15 +224,12 @@ class PoseCost(CostBase, PoseCostConfig):
             run_weight = self.run_weight
 
         active_steps = math.floor(self._horizon * run_tstep_fraction)
+        self.initialize_run_weight_vec(self._horizon)
         self._run_weight_vec[:, :active_steps] = 0
         self._run_weight_vec[:, active_steps:-1] = run_weight
 
     def update_batch_size(self, batch_size, horizon):
         if batch_size != self._batch_size or horizon != self._horizon:
-            # print(self.weight)
-            # print(batch_size, horizon, self._batch_size, self._horizon)
-
-            # batch_size = b*h
             self.out_distance = torch.zeros(
                 (batch_size, horizon), device=self.tensor_args.device, dtype=self.tensor_args.dtype
             )
@@ -265,16 +262,21 @@ class PoseCost(CostBase, PoseCostConfig):
                 device=self.tensor_args.device,
                 dtype=self.tensor_args.dtype,
             )
-            if self._run_weight_vec is None or self._run_weight_vec.shape[1] != horizon:
+            self.initialize_run_weight_vec(horizon)
-                self._run_weight_vec = torch.ones(
-                    (1, horizon), device=self.tensor_args.device, dtype=self.tensor_args.dtype
-                )
             if self.terminal and self.run_weight is not None and horizon > 1:
                 self._run_weight_vec[:, :-1] = self.run_weight
 
             self._batch_size = batch_size
             self._horizon = horizon
 
+    def initialize_run_weight_vec(self, horizon: Optional[int] = None):
+        if horizon is None:
+            horizon = self._horizon
+        if self._run_weight_vec is None or self._run_weight_vec.shape[1] != horizon:
+            self._run_weight_vec = torch.ones(
+                (1, horizon), device=self.tensor_args.device, dtype=self.tensor_args.dtype
+            )
+
     @property
     def goalset_index_buffer(self):
         return self.out_idx