Minor fixes and geom module documentation

CHANGELOG.md

@@ -10,6 +10,18 @@ its affiliates is strictly prohibited.
 -->
 # Changelog
 
+## Latest Commit
+
+### New Features
+- Add pose cost metric to MPC to allow for partial pose reaching.
+- Update obstacle poses in cpu reference with an optional flag.
+
+### BugFixes & Misc.
+- Fixed optimize_dt not being correctly set when motion gen is called in reactive mode.
+- Add documentation for geom module.
+- Add descriptive api for computing kinematics.
+- Fix cv2 import order in isaac sim realsense examples.
+
 ## Version 0.7.4
 
 ### Changes in Default Behavior

examples/isaac_sim/motion_gen_reacher.py

@@ -209,13 +209,15 @@ def main():
     trim_steps = None
     max_attempts = 4
     interpolation_dt = 0.05
+    enable_finetune_trajopt = True
     if args.reactive:
         trajopt_tsteps = 40
         trajopt_dt = 0.04
         optimize_dt = False
         max_attempts = 1
         trim_steps = [1, None]
         interpolation_dt = trajopt_dt
+        enable_finetune_trajopt = False
     motion_gen_config = MotionGenConfig.load_from_robot_config(
         robot_cfg,
         world_cfg,
@@ -231,8 +233,9 @@ def main():
         trim_steps=trim_steps,
     )
     motion_gen = MotionGen(motion_gen_config)
-    print("warming up...")
-    motion_gen.warmup(enable_graph=True, warmup_js_trajopt=False, parallel_finetune=True)
+    if not args.reactive:
+        print("warming up...")
+        motion_gen.warmup(enable_graph=True, warmup_js_trajopt=False)
 
     print("Curobo is Ready")
 
@@ -242,9 +245,8 @@ def main():
         enable_graph=False,
         enable_graph_attempt=2,
         max_attempts=max_attempts,
-        enable_finetune_trajopt=True,
-        parallel_finetune=True,
-        time_dilation_factor=0.5,
+        enable_finetune_trajopt=enable_finetune_trajopt,
+        time_dilation_factor=0.5 if not args.reactive else 1.0,
     )
 
     usd_help.load_stage(my_world.stage)

examples/isaac_sim/realsense_mpc.py

@@ -9,13 +9,15 @@
 # its affiliates is strictly prohibited.
 #
 
+
 try:
     # Third Party
     import isaacsim
 except ImportError:
     pass
 
 # Third Party
+import cv2
 import torch
 
 a = torch.zeros(4, device="cuda:0")
@@ -31,7 +33,6 @@
     }
 )
 # Third Party
-import cv2
 import numpy as np
 import torch
 from matplotlib import cm

examples/isaac_sim/realsense_reacher.py

@@ -15,7 +15,9 @@
 except ImportError:
     pass
 
+
 # Third Party
+import cv2
 import torch
 
 a = torch.zeros(4, device="cuda:0")
@@ -31,7 +33,6 @@
 )
 
 # Third Party
-import cv2
 import numpy as np
 import torch
 from matplotlib import cm

pyproject.toml

@@ -65,3 +65,5 @@ omit = [
     "src/curobo/rollout/cost/manipulability_cost.py",
 ]
 
+[tool.interrogate]
+ignore-regex = ["forward", "backward"]

src/curobo/cuda_robot_model/cuda_robot_model.py

@@ -505,6 +505,49 @@ def compute_kinematics(
 
         return self.get_state(js.position, link_name, calculate_jacobian)
 
+    def compute_kinematics_from_joint_state(
+        self, js: JointState, link_name: Optional[str] = None, calculate_jacobian: bool = False
+    ) -> CudaRobotModelState:
+        """Compute forward kinematics of the robot.
+
+        Args:
+            js: Joint state of robot.
+            link_name: Name of link to return pose of. If None, returns end-effector pose.
+            calculate_jacobian: Calculate jacobian of the robot. Not supported.
+
+
+        Returns:
+            CudaRobotModelState: Kinematic state of the robot.
+
+        """
+        if js.joint_names is not None:
+            if js.joint_names != self.kinematics_config.joint_names:
+                log_error("Joint names do not match, reoder joints before forward kinematics")
+
+        return self.get_state(js.position, link_name, calculate_jacobian)
+
+    def compute_kinematics_from_joint_position(
+        self,
+        joint_position: torch.Tensor,
+        link_name: Optional[str] = None,
+        calculate_jacobian: bool = False,
+    ) -> CudaRobotModelState:
+        """Compute forward kinematics of the robot.
+
+        Args:
+            joint_position: Joint position of robot. Assumed to only contain active joints in the
+                order specified in :attr:`CudaRobotModel.joint_names`.
+            link_name: Name of link to return pose of. If None, returns end-effector pose.
+            calculate_jacobian: Calculate jacobian of the robot. Not supported.
+
+
+        Returns:
+            CudaRobotModelState: Kinematic state of the robot.
+
+        """
+
+        return self.get_state(joint_position, link_name, calculate_jacobian)
+
     def get_robot_link_meshes(self) -> List[Mesh]:
         """Get meshes of all links of the robot.
 
@@ -578,12 +621,12 @@ def get_robot_as_spheres(self, q: torch.Tensor, filter_valid: bool = True) -> Li
     def get_link_poses(self, q: torch.Tensor, link_names: List[str]) -> Pose:
         """Get Pose of links at given joint configuration q using forward kinematics.
 
-        Note that only the links specified in :var:`~link_names` are returned.
+        Note that only the links specified in :class:`~CudaRobotModelConfig.link_names` are returned.
 
         Args:
             q: Joint configuration of the robot, shape should be [batch_size, dof].
             link_names: Names of links to get pose of. This should be a subset of
-                :var:`~link_names`.
+                :class:`~CudaRobotModelConfig.link_names`.
 
         Returns:
             Pose: Poses of links at given joint configuration.
@@ -839,6 +882,19 @@ def attach_external_objects_to_robot(
 
         return True
 
+    def get_active_js(self, full_js: JointState):
+        """Get joint state of active joints of the robot.
+
+        Args:
+            full_js: Joint state of all joints.
+
+        Returns:
+            JointState: Joint state of active joints.
+        """
+        active_jnames = self.joint_names
+        out_js = full_js.get_ordered_joint_state(active_jnames)
+        return out_js
+
     @property
     def ee_link(self) -> str:
         """End-effector link of the robot. Changing requires reinitializing this class."""

src/curobo/curobolib/__init__.py

@@ -8,4 +8,9 @@
 # without an express license agreement from NVIDIA CORPORATION or
 # its affiliates is strictly prohibited.
 #
-"""CuRoboLib Module."""
+"""
+cuRoboLib module contains CUDA implementations (kernels) of robotics algorithms, wrapped in
+C++, and compiled with PyTorch for use in Python.
+
+All implementations are in ``.cu`` files in ``cpp`` sub-directory.
+"""

src/curobo/curobolib/kinematics.py

@@ -24,7 +24,7 @@
     from torch.utils.cpp_extension import load
 
     # CuRobo
-    from curobo.curobolib.util_file import add_cpp_path
+    from curobo.util_file import add_cpp_path
 
     kinematics_fused_cu = load(
         name="kinematics_fused_cu",

src/curobo/curobolib/ls.py

@@ -24,7 +24,7 @@
     from torch.utils.cpp_extension import load
 
     # CuRobo
-    from curobo.curobolib.util_file import add_cpp_path
+    from curobo.util_file import add_cpp_path
 
     line_search_cu = load(
         name="line_search_cu",

src/curobo/curobolib/opt.py

@@ -24,7 +24,7 @@
     from torch.utils.cpp_extension import load
 
     # CuRobo
-    from curobo.curobolib.util_file import add_cpp_path
+    from curobo.util_file import add_cpp_path
 
     lbfgs_step_cu = load(
         name="lbfgs_step_cu",

src/curobo/curobolib/tensor_step.py

@@ -23,7 +23,7 @@
     from torch.utils.cpp_extension import load
 
     # CuRobo
-    from curobo.curobolib.util_file import add_cpp_path
+    from curobo.util_file import add_cpp_path
 
     log_warn("tensor_step_cu not found, jit compiling...")
     tensor_step_cu = load(

src/curobo/curobolib/util_file.py

@@ -8,27 +8,9 @@
 # without an express license agreement from NVIDIA CORPORATION or
 # its affiliates is strictly prohibited.
 #
-
+"""Deprecated, use functions from :mod:`curobo.util_file` instead."""
 # Standard Library
 import os
 
-
-def get_cpp_path():
-    path = os.path.dirname(__file__)
-    return os.path.join(path, "cpp")
-
-
-def join_path(path1, path2):
-    if isinstance(path2, str):
-        return os.path.join(path1, path2)
-    else:
-        return path2
-
-
-def add_cpp_path(sources):
-    cpp_path = get_cpp_path()
-    new_list = []
-    for s in sources:
-        s = join_path(cpp_path, s)
-        new_list.append(s)
-    return new_list
+# CuRobo
+from curobo.util_file import add_cpp_path, get_cpp_path, join_path

src/curobo/geom/__init__.py

@@ -10,8 +10,8 @@
 #
 
 """
-This module contains code for geometric processing such as pointcloud processing, analytic signed 
-distance computation for the environment, and also signed distance computation between robot and the
-environment. These functions can be used for robot self collision checking and also for robot 
-environment collision checking. 
+This module contains functions for geometric processing such as pointcloud processing, analytic
+signed distance computation for the environment, and also signed distance computation between robot
+and the environment. These functions can be used for robot self collision checking and also for
+robot environment collision checking.
 """

src/curobo/geom/cv.py

@@ -8,6 +8,7 @@
 # without an express license agreement from NVIDIA CORPORATION or
 # its affiliates is strictly prohibited.
 #
+"""Computer Vision functions, including projection between depth and pointclouds."""
 
 # Third Party
 import torch
@@ -17,15 +18,18 @@
 
 
 @get_torch_jit_decorator()
-def project_depth_to_pointcloud(depth_image: torch.Tensor, intrinsics_matrix: torch.Tensor):
-    """Projects numpy depth image to point cloud.
+def project_depth_to_pointcloud(
+    depth_image: torch.Tensor,
+    intrinsics_matrix: torch.Tensor,
+) -> torch.Tensor:
+    """Projects depth image to point cloud.
 
     Args:
-      np_depth_image: numpy array float, shape (h, w).
-        intrinsics array: numpy array float, 3x3 intrinsics matrix.
+      depth_image: torch tensor of shape (b, h, w).
+        intrinsics array: torch tensor for intrinsics matrix of shape (b, 3, 3).
 
     Returns:
-      array of float (h, w, 3)
+      torch tensor of shape (b, h, w, 3)
     """
     fx = intrinsics_matrix[0, 0]
     fy = intrinsics_matrix[1, 1]
@@ -48,16 +52,21 @@ def project_depth_to_pointcloud(depth_image: torch.Tensor, intrinsics_matrix: to
 
 @get_torch_jit_decorator()
 def get_projection_rays(
-    height: int, width: int, intrinsics_matrix: torch.Tensor, depth_to_meter: float = 0.001
-):
-    """Projects numpy depth image to point cloud.
+    height: int,
+    width: int,
+    intrinsics_matrix: torch.Tensor,
+    depth_to_meter: float = 0.001,
+) -> torch.Tensor:
+    """Get projection rays for a image size and batch of intrinsics matrices.
 
     Args:
-      np_depth_image: numpy array float, shape (h, w).
-        intrinsics array: numpy array float, 3x3 intrinsics matrix.
+        height: Height of the images.
+        width: Width of the images.
+        intrinsics_matrix: Batch of intrinsics matrices of shape (b, 3, 3).
+        depth_to_meter: Scaling factor to convert depth to meters.
 
     Returns:
-      array of float (h, w, 3)
+        torch.Tensor: Projection rays of shape (b, height * width, 3).
     """
     fx = intrinsics_matrix[:, 0:1, 0:1]
     fy = intrinsics_matrix[:, 1:2, 1:2]
@@ -92,15 +101,15 @@ def get_projection_rays(
 def project_pointcloud_to_depth(
     pointcloud: torch.Tensor,
     output_image: torch.Tensor,
-):
-    """Projects pointcloud to depth image
+) -> torch.Tensor:
+    """Projects pointcloud to depth image based on indices.
 
     Args:
-      np_depth_image: numpy array float, shape (h, w).
-        intrinsics array: numpy array float, 3x3 intrinsics matrix.
+        pointcloud: PointCloud of shape (b, h, w, 3).
+        output_image: Image of shape (b, h, w).
 
     Returns:
-      array of float (h, w)
+        torch.Tensor: Depth image of shape (b, h, w).
     """
     width, height = output_image.shape
 
@@ -112,10 +121,26 @@ def project_pointcloud_to_depth(
 
 @get_torch_jit_decorator()
 def project_depth_using_rays(
-    depth_image: torch.Tensor, rays: torch.Tensor, filter_origin: bool = False
-):
+    depth_image: torch.Tensor,
+    rays: torch.Tensor,
+    filter_origin: bool = False,
+    depth_threshold: float = 0.01,
+) -> torch.Tensor:
+    """Project depth image to pointcloud using projection rays.
+
+    Projection rays can be calculated using :func:`~curobo.geom.cv.get_projection_rays` function.
+
+    Args:
+        depth_image: Dpepth image of shape (b, h, w).
+        rays: Projection rays of shape (b, h * w, 3).
+        filter_origin: Remove points with depth less than depth_threshold.
+        depth_threshold: Threshold to filter points.
+
+    Returns:
+        Pointcloud of shape (b, h * w, 3).
+    """
     if filter_origin:
-        depth_image = torch.where(depth_image < 0.01, 0, depth_image)
+        depth_image = torch.where(depth_image < depth_threshold, 0, depth_image)
 
     depth_image = depth_image.view(depth_image.shape[0], -1, 1).contiguous()
     points = depth_image * rays