diff --git a/CHANGELOG.md b/CHANGELOG.md index 9ac53c7..685a6fc 100644 --- a/CHANGELOG.md +++ b/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 diff --git a/src/curobo/content/configs/robot/ur5e_robotiq_2f_140.yml b/src/curobo/content/configs/robot/ur5e_robotiq_2f_140.yml index 1a377ec..139ff60 100644 --- a/src/curobo/content/configs/robot/ur5e_robotiq_2f_140.yml +++ b/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_outer_finger', - 'left_inner_finger', - 'left_inner_finger_pad', + 'left_inner_knuckle', + 'left_outer_finger', + 'left_inner_finger', + '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, - 'forearm_link': 0, - 'wrist_1_link': 0, + 'upper_arm_link': 0, + 'forearm_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] diff --git a/src/curobo/cuda_robot_model/cuda_robot_model.py b/src/curobo/cuda_robot_model/cuda_robot_model.py index c7c99e3..f52282c 100644 --- a/src/curobo/cuda_robot_model/cuda_robot_model.py +++ b/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: diff --git a/src/curobo/curobolib/cpp/self_collision_kernel.cu b/src/curobo/curobolib/cpp/self_collision_kernel.cu index 7b6d19f..894cfcf 100644 --- a/src/curobo/curobolib/cpp/self_collision_kernel.cu +++ b/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] = diff --git a/src/curobo/rollout/dynamics_model/kinematic_model.py b/src/curobo/rollout/dynamics_model/kinematic_model.py index 94f0d41..9c9b6c3 100644 --- a/src/curobo/rollout/dynamics_model/kinematic_model.py +++ b/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() diff --git a/src/curobo/wrap/reacher/ik_solver.py b/src/curobo/wrap/reacher/ik_solver.py index 23ffd8e..89dc8ff 100644 --- a/src/curobo/wrap/reacher/ik_solver.py +++ b/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( diff --git a/src/curobo/wrap/reacher/motion_gen.py b/src/curobo/wrap/reacher/motion_gen.py index 48b5c51..082b199 100644 --- a/src/curobo/wrap/reacher/motion_gen.py +++ b/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] diff --git a/tests/motion_gen_module_test.py b/tests/motion_gen_module_test.py index 7d2a97e..67b60aa 100644 --- a/tests/motion_gen_module_test.py +++ b/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