From b481ee201a2754c072ba9bcd80bd5d327fddc599 Mon Sep 17 00:00:00 2001 From: Balakumar Sundaralingam Date: Fri, 22 Mar 2024 19:41:00 -0700 Subject: [PATCH] add isaac sim 2023.1.1 partial support --- CHANGELOG.md | 6 +++++- examples/isaac_sim/helper.py | 21 +++++++++++++-------- src/curobo/cuda_robot_model/types.py | 2 +- src/curobo/opt/newton/lbfgs.py | 12 ++++++------ src/curobo/rollout/cost/pose_cost.py | 18 ++++++++++-------- 5 files changed, 35 insertions(+), 24 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index efa27e14..99d86264 100644 --- a/CHANGELOG.md +++ b/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 diff --git a/examples/isaac_sim/helper.py b/examples/isaac_sim/helper.py index 9586ed04..10b185a3 100644 --- a/examples/isaac_sim/helper.py +++ b/examples/isaac_sim/helper.py @@ -19,6 +19,7 @@ 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) - # print(prim_path) - # robot_prim = my_world.scene.stage.OverridePrim(prim_path) - # robot_prim.GetReferences().AddReference(dest_path) + + base_link_name = robot_config["kinematics"]["base_link"] + 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 diff --git a/src/curobo/cuda_robot_model/types.py b/src/curobo/cuda_robot_model/types.py index 776c240f..f9ba8599 100644 --- a/src/curobo/cuda_robot_model/types.py +++ b/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.""" diff --git a/src/curobo/opt/newton/lbfgs.py b/src/curobo/opt/newton/lbfgs.py index e5e59d72..41cfc700 100644 --- a/src/curobo/opt/newton/lbfgs.py +++ b/src/curobo/opt/newton/lbfgs.py @@ -25,13 +25,13 @@ # 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, - rho_buffer, - y_buffer, - s_buffer, - grad_q, + alpha_buffer: torch.Tensor, + rho_buffer: torch.Tensor, + y_buffer: torch.Tensor, + s_buffer: torch.Tensor, + grad_q: torch.Tensor, m: int, epsilon: float, stable_mode: bool = True, diff --git a/src/curobo/rollout/cost/pose_cost.py b/src/curobo/rollout/cost/pose_cost.py index 7e1a6e0f..e232b281 100644 --- a/src/curobo/rollout/cost/pose_cost.py +++ b/src/curobo/rollout/cost/pose_cost.py @@ -224,15 +224,12 @@ def update_run_weight( 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 @@ def update_batch_size(self, batch_size, horizon): 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._run_weight_vec = torch.ones( - (1, horizon), device=self.tensor_args.device, dtype=self.tensor_args.dtype - ) + self.initialize_run_weight_vec(horizon) 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