check start state validity and minor fixes

CHANGELOG.md

@@ -12,9 +12,17 @@ its affiliates is strictly prohibited.
 
 ## Latest Commit
 
+### New Features
+- Added validity of start state check for motion_gen plan calls for single queries.
+
 ### BugFixes & Misc.
 - Fix bug in evaluator to account for dof maximum acceleration and jerk.
 - Add unit test for different acceleration and jerk limits.
+- Add a check in self-collision kernels to avoid computing over inactive threads.
+- Add `link_poses` as an additional property to kinematics to be more descriptive.
+- 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`.
 
 ## Version 0.7.2
 

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

@@ -19,28 +19,28 @@ robot_cfg:
 
     urdf_path: "robot/ur_description/ur5e_robotiq_2f_140.urdf"
     asset_root_path: "robot/ur_description"
-    
+
     base_link: "base_link"
     ee_link: "grasp_frame"
     link_names: null
-    lock_joints: {'finger_joint': 0.3}
+    lock_joints: {'finger_joint': 0.0}
 
-    extra_links: null #{"attached_object":{"parent_link_name": "grasp_frame" , 
+    extra_links: null #{"attached_object":{"parent_link_name": "grasp_frame" ,
     #"link_name": "attached_object", "fixed_transform": [0,0,0,1,0,0,0], "joint_type":"FIXED",
     #"joint_name": "attach_joint" }}
     extra_collision_spheres: null #{"attached_object": 4}
 
-    collision_link_names: ['shoulder_link','upper_arm_link', 'forearm_link', 'wrist_1_link', 
+    collision_link_names: ['shoulder_link','upper_arm_link', 'forearm_link', 'wrist_1_link',
     'wrist_2_link' ,'wrist_3_link', 'tool0', 'robotiq_arg2f_base_link',
     'left_outer_knuckle',
-    'left_inner_knuckle', 
+    'left_inner_knuckle',
-    'left_outer_finger', 
+    'left_outer_finger',
-    'left_inner_finger', 
+    'left_inner_finger',
-    'left_inner_finger_pad', 
+    'left_inner_finger_pad',
     'right_outer_knuckle',
-    'right_inner_knuckle', 
+    'right_inner_knuckle',
     'right_outer_finger' ,
-    'right_inner_finger', 
+    'right_inner_finger',
     'right_inner_finger_pad',
     ] # List[str]
     collision_spheres:
@@ -117,7 +117,7 @@ robot_cfg:
             "radius": 0.01
           - "center": [0.0, 0.015, -0.01]
             "radius": 0.01
-        
+
       left_inner_finger_pad:
           - "center": [0.0, -0.0, 0.005]
             "radius": 0.01
@@ -135,7 +135,7 @@ robot_cfg:
       left_inner_finger:
           - "center": [0.0, -0.0, -0.0]
             "radius": 0.01
-         
+
       left_outer_knuckle:
           - "center": [0.0, 0.055, 0.01]
             "radius": 0.01
@@ -148,7 +148,7 @@ robot_cfg:
             "radius": 0.01
           - "center": [0.0, 0.015, -0.01]
             "radius": 0.01
-      
+
       right_inner_finger_pad:
           - "center": [0.0, -0.0, 0.005]
             "radius": 0.01
@@ -166,7 +166,7 @@ robot_cfg:
       right_inner_finger:
           - "center": [0.0, -0.0, -0.0]
             "radius": 0.01
-         
+
       right_outer_knuckle:
           - "center": [0.0, 0.055, 0.01]
             "radius": 0.01
@@ -175,25 +175,25 @@ robot_cfg:
 
 
     collision_sphere_buffer: 0.005
-   
+
     self_collision_ignore: {
       "upper_arm_link": ["forearm_link", "shoulder_link"],
       "forearm_link": ["wrist_1_link", "wrist_2_link", "wrist_3_link"],
       "wrist_1_link": ["wrist_2_link","wrist_3_link","tool0", "robotiq_arg2f_base_link"],
       "wrist_2_link": ["wrist_3_link", "tool0",  "robotiq_arg2f_base_link"],
       "wrist_3_link": ["tool0", "robotiq_arg2f_base_link"],
-      
+
-      "tool0": ['robotiq_arg2f_base_link', 'left_outer_finger', 'left_inner_finger_pad', 
+      "tool0": ['robotiq_arg2f_base_link', 'left_outer_finger', 'left_inner_finger_pad',
       'left_inner_knuckle', 'left_inner_finger', 'left_outer_knuckle',
       'right_outer_finger', 'right_inner_finger_pad',
       'right_inner_knuckle', 'right_inner_finger', 'right_outer_knuckle'],
 
-      "robotiq_arg2f_base_link": ['left_outer_finger', 'left_inner_finger_pad', 
+      "robotiq_arg2f_base_link": ['left_outer_finger', 'left_inner_finger_pad',
       'left_inner_knuckle', 'left_inner_finger', 'left_outer_knuckle',
       'right_outer_finger', 'right_inner_finger_pad',
       'right_inner_knuckle', 'right_inner_finger', 'right_outer_knuckle'],
-      
+
-      "left_outer_finger": ['left_inner_finger_pad', 
+      "left_outer_finger": ['left_inner_finger_pad',
       'left_inner_knuckle', 'left_inner_finger', 'left_outer_knuckle',
       'right_outer_finger', 'right_inner_finger_pad',
       'right_inner_knuckle', 'right_inner_finger', 'right_outer_knuckle'],
@@ -216,7 +216,7 @@ robot_cfg:
       'right_inner_knuckle', 'right_inner_finger', 'right_outer_knuckle'],
 
 
-      
+
       "right_outer_finger": ['right_inner_finger_pad',
       'right_inner_knuckle', 'right_inner_finger', 'right_outer_knuckle'],
 
@@ -227,36 +227,36 @@ robot_cfg:
 
       "right_inner_finger": [ 'right_outer_knuckle'],
 
-      
+
 
       }
     self_collision_buffer: {
       'shoulder_link': 0.01,
-      'upper_arm_link': 0, 
+      'upper_arm_link': 0,
-      'forearm_link': 0, 
+      'forearm_link': 0,
-      'wrist_1_link': 0, 
+      'wrist_1_link': 0,
       'wrist_2_link': 0,
       'wrist_3_link' : 0,
       'tool0': 0.02,
-      } 
+      }
-      
+
     use_global_cumul: True
-    mesh_link_names:  ['shoulder_link','upper_arm_link', 'forearm_link', 'wrist_1_link', 
+    mesh_link_names:  ['shoulder_link','upper_arm_link', 'forearm_link', 'wrist_1_link',
     'wrist_2_link' ,'wrist_3_link', 'robotiq_arg2f_base_link',
     'left_outer_knuckle',
-    'left_inner_knuckle', 
+    'left_inner_knuckle',
     'left_outer_finger',
-    'left_inner_finger', 
+    'left_inner_finger',
     'right_outer_knuckle',
-    'right_inner_knuckle', 
+    'right_inner_knuckle',
     'right_outer_finger' ,
-    'right_inner_finger', 
+    'right_inner_finger',
     ] # List[str]
 
     cspace:
-      joint_names: ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 
+      joint_names: ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint',
                     'wrist_2_joint', 'wrist_3_joint',
-                    'finger_joint'] 
+                    'finger_joint']
       retract_config:  [0.0, -2.2, 1.0, -1.383, -1.57, 0.00, 0.6]
       null_space_weight:  [1.0, 1.0, 1.0, 1.0, 1.0, 1.0,1.0]
       cspace_distance_weight: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]

src/curobo/cuda_robot_model/cuda_robot_model.py

@@ -12,7 +12,7 @@ from __future__ import annotations
 
 # Standard Library
 from dataclasses import dataclass
-from typing import Any, Dict, List, Optional
+from typing import Any, Dict, List, Optional, Union
 
 # Third Party
 import torch
@@ -186,7 +186,12 @@ class CudaRobotModelState:
         return self.link_spheres_tensor
 
     @property
-    def link_pose(self) -> Dict[str, Pose]:
+    def link_pose(self) -> Union[None, Dict[str, Pose]]:
+        """Deprecated, use link_poses."""
+        return self.link_poses
+
+    @property
+    def link_poses(self) -> Union[None, Dict[str, Pose]]:
         """Get link poses as a dictionary of link name to Pose object."""
         link_poses = None
         if self.link_names is not None:

src/curobo/curobolib/cpp/self_collision_kernel.cu

@@ -82,7 +82,7 @@ namespace Curobo
 
             if (write_grad)
             {
-              dist_vec = (sph1 - sph2) / distance;
+              dist_vec = normalize(sph1 - sph2);// / distance;
             }
           }
         }
@@ -235,7 +235,7 @@ namespace Curobo
             uint64_t nd     = __shfl_down_sync(mask, *(uint64_t *)&max_d, offset);
             dist_t   d_temp = *(dist_t *)&nd;
 
-            if (d_temp.d > max_d.d)
+            if (((threadIdx.x + offset) < blockDim.x)  && d_temp.d > max_d.d)
             {
               max_d = d_temp;
             }
@@ -285,11 +285,12 @@ namespace Curobo
 
           if (write_grad)
           {
+            // NOTE: spheres can be read from rs_shared
             float3 sph1 =
               *(float3 *)&robot_spheres[4 * (batch_idx * nspheres + max_d.i)];
             float3 sph2 =
               *(float3 *)&robot_spheres[4 * (batch_idx * nspheres + max_d.j)];
-            float3 dist_vec = (sph1 - sph2) / max_d.d;
+            float3 dist_vec = normalize(sph1 - sph2);
             *(float3 *)&out_vec[batch_idx * nspheres * 4 + max_d.i * 4] =
               weight[0] * -1 * dist_vec;
             *(float3 *)&out_vec[batch_idx * nspheres * 4 + max_d.j * 4] =
@@ -434,7 +435,7 @@ namespace Curobo
               uint64_t nd     = __shfl_down_sync(mask, *(uint64_t *)&max_d[l], offset);
               dist_t   d_temp = *(dist_t *)&nd;
 
-              if (d_temp.d > max_d[l].d)
+              if (((threadIdx.x + offset) < blockDim.x)  && d_temp.d > max_d[l].d)
               {
                 max_d[l] = d_temp;
               }
@@ -493,13 +494,15 @@ namespace Curobo
 
             if (write_grad)
             {
+              // NOTE: spheres can also be read from rs_shared
               float3 sph1 =
                 *(float3 *)&robot_spheres[4 *
                                           ((batch_idx + l) * nspheres + max_d.i)];
               float3 sph2 =
                 *(float3 *)&robot_spheres[4 *
                                           ((batch_idx + l) * nspheres + max_d.j)];
-              float3 dist_vec = (sph1 - sph2) / max_d.d;
+              float3 dist_vec = normalize(sph1 - sph2);// / max_d.d;
+
               *(float3 *)&out_vec[(batch_idx + l) * nspheres * 4 + max_d.i * 4] =
                 weight[0] * -1 * dist_vec;
               *(float3 *)&out_vec[(batch_idx + l) * nspheres * 4 + max_d.j * 4] =

src/curobo/rollout/dynamics_model/kinematic_model.py

@@ -109,6 +109,11 @@ class KinematicModelState(Sequence):
 
     @property
     def link_pose(self):
+        """Deprecated: Use link_poses instead."""
+        return self.link_poses
+
+    @property
+    def link_poses(self):
         if self.link_names is not None:
             link_pos_seq = self.link_pos_seq.contiguous()
             link_quat_seq = self.link_quat_seq.contiguous()

src/curobo/wrap/reacher/ik_solver.py

@@ -1560,6 +1560,26 @@ class IKSolver(IKSolverConfig):
         """Get ordered names of all joints used in optimization with IKSolver."""
         return self.rollout_fn.kinematics.joint_names
 
+    def check_valid(self, joint_position: torch.Tensor) -> torch.Tensor:
+        """Check if joint position is valid. Also supports batch of joint positions.
+
+        Args:
+            joint_position: input position tensor of shape (batch, dof).
+
+        Returns:
+            boolean tensor of shape (batch) indicating if the joint position is valid.
+        """
+        if len(joint_position.shape) == 1:
+            joint_position = joint_position.unsqueeze(0)
+        if len(joint_position.shape) > 2:
+            log_error("joint_position should be of shape (batch, dof)")
+        metrics = self.rollout_fn.rollout_constraint(
+            joint_position.unsqueeze(1),
+            use_batch_env=False,
+        )
+        feasible = metrics.feasible.squeeze(1)
+        return feasible
+
 
 @get_torch_jit_decorator()
 def get_success(

src/curobo/wrap/reacher/motion_gen.py

@@ -947,6 +947,10 @@ class MotionGenPlanConfig:
     #: post-process the trajectory.
     time_dilation_factor: Optional[float] = None
 
+    #: Check if the start state is valid before runnning any steps in motion generation. This will
+    #: check for joint limits, self-collision, and collision with the world.
+    check_start_validity: bool = True
+
     def __post_init__(self):
         """Post initialization checks."""
         if not self.enable_opt and not self.enable_graph:
@@ -998,9 +1002,14 @@ class MotionGenStatus(Enum):
     FINETUNE_TRAJOPT_FAIL = "Finetune TrajOpt Fail"
 
     #: Invalid query was given. The start state is either out of joint limits, in collision with
-    #: world, or in self-collision.
+    #: world, or in self-collision. In the future, this will also check for reachability of goal
+    #: pose/ joint target in joint limits.
     INVALID_QUERY = "Invalid Query"
 
+    #: Invalid query was given. The start state is either out of joint limits, in collision with
+    #: world, or in self-collision.
+    INVALID_START = "Invalid Start"
+
     #: Motion generation query was successful.
     SUCCESS = "Success"
 
@@ -1064,7 +1073,7 @@ class MotionGenResult:
     #: Debug information
     debug_info: Any = None
 
-    #: status of motion generation query. returns [IK Fail, Graph Fail, TrajOpt Fail].
+    #: status of motion generation query.
     status: Optional[Union[MotionGenStatus, str]] = None
 
     #: number of attempts used before returning a solution.
@@ -1663,6 +1672,7 @@ class MotionGen(MotionGenConfig):
             start_state,
             goal_pose,
             plan_config,
+            link_poses=link_poses,
         )
         return result
 
@@ -1884,6 +1894,7 @@ class MotionGen(MotionGenConfig):
                                 enable_graph=False,
                                 enable_graph_attempt=None,
                             ),
+                            link_poses=link_poses,
                         )
                     else:
                         self.plan_batch(
@@ -1895,6 +1906,7 @@ class MotionGen(MotionGenConfig):
                                 enable_graph=enable_graph,
                                 enable_graph_attempt=None if not enable_graph else 20,
                             ),
+                            link_poses=link_poses,
                         )
             else:
                 retract_pose = Pose(
@@ -1914,6 +1926,7 @@ class MotionGen(MotionGenConfig):
                                 enable_finetune_trajopt=True,
                                 enable_graph=False,
                             ),
+                            link_poses=link_poses,
                         )
                     else:
                         self.plan_batch_goalset(
@@ -1925,6 +1938,7 @@ class MotionGen(MotionGenConfig):
                                 enable_graph=enable_graph,
                                 enable_graph_attempt=None if not enable_graph else 20,
                             ),
+                            link_poses=link_poses,
                         )
 
         log_info("Warmup complete")
@@ -1973,6 +1987,17 @@ class MotionGen(MotionGenConfig):
         )
         force_graph = plan_config.enable_graph
 
+        if plan_config.check_start_validity:
+            # check if start state is collision-free and within limits:
+            valid_query = self.ik_solver.check_valid(start_state.position).item()
+            if not valid_query:
+                result = MotionGenResult(
+                    success=torch.as_tensor([False], device=self.tensor_args.device),
+                    valid_query=valid_query,
+                    status=MotionGenStatus.INVALID_START,
+                )
+                return result
+
         for n in range(plan_config.max_attempts):
             result = self._plan_js_from_solve_state(
                 solve_state, start_state, goal_state, plan_config=plan_config
@@ -2753,6 +2778,17 @@ class MotionGen(MotionGenConfig):
             MotionGenResult: Result of planning.
         """
         start_time = time.time()
+        valid_query = True
+        if plan_config.check_start_validity:
+            # check if start state is collision-free and within limits:
+            valid_query = self.ik_solver.check_valid(start_state.position).item()
+            if not valid_query:
+                result = MotionGenResult(
+                    success=torch.as_tensor([False], device=self.tensor_args.device),
+                    valid_query=valid_query,
+                    status=MotionGenStatus.INVALID_START,
+                )
+                return result
         if plan_config.pose_cost_metric is not None:
             valid_query = self.update_pose_cost_metric(
                 plan_config.pose_cost_metric, start_state, goal_pose
@@ -3668,6 +3704,8 @@ class MotionGen(MotionGenConfig):
                     # )
                     result.interpolated_plan = self._batch_graph_search_buffer
                     g_dim = g_dim.cpu().squeeze().tolist()
+                    if isinstance(g_dim, int):
+                        g_dim = [g_dim]
                     for x, x_val in enumerate(g_dim):
                         self._batch_path_buffer_last_tstep[x_val] = (
                             graph_result.path_buffer_last_tstep[x]

tests/motion_gen_module_test.py

@@ -20,7 +20,12 @@ from curobo.types.math import Pose
 from curobo.types.robot import JointState, RobotConfig
 from curobo.util.trajectory import InterpolateType
 from curobo.util_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml
-from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig
+from curobo.wrap.reacher.motion_gen import (
+    MotionGen,
+    MotionGenConfig,
+    MotionGenPlanConfig,
+    MotionGenStatus,
+)
 
 
 @pytest.fixture(scope="module")
@@ -328,3 +333,50 @@ def test_motion_gen_single_js(motion_gen_str, enable_graph, request):
     reached_state = result.optimized_plan[-1]
 
     assert torch.norm(goal_state.position - reached_state.position) < 0.05
+
+
+def test_motion_gen_single_js_invalid_start(motion_gen):
+
+    motion_gen.reset()
+
+    retract_cfg = motion_gen.get_retract_config()
+
+    start_state = JointState.from_position(retract_cfg.view(1, -1) + 0.3)
+
+    m_config = MotionGenPlanConfig(max_attempts=2)
+
+    goal_state = start_state.clone()
+    goal_state.position -= 0.3
+    start_state.position[0] += 10.0
+
+    result = motion_gen.plan_single_js(start_state, goal_state, m_config)
+
+    assert torch.count_nonzero(result.success) == 0
+
+    assert result.valid_query == False
+
+    assert result.status == MotionGenStatus.INVALID_START
+
+
+def test_motion_gen_single_invalid(motion_gen):
+    motion_gen.reset()
+
+    retract_cfg = motion_gen.get_retract_config()
+
+    state = motion_gen.compute_kinematics(JointState.from_position(retract_cfg.view(1, -1)))
+
+    goal_pose = Pose(state.ee_pos_seq, quaternion=state.ee_quat_seq)
+
+    start_state = JointState.from_position(retract_cfg.view(1, -1) + 0.3)
+    start_state.position[..., 1] = 1.7
+
+    m_config = MotionGenPlanConfig(False, True, max_attempts=1)
+
+    result = motion_gen.plan_single(start_state, goal_pose, m_config)
+
+    # get final solutions:
+    assert torch.count_nonzero(result.success) == 0
+
+    assert result.valid_query == False
+
+    assert result.status == MotionGenStatus.INVALID_START