hofee/gen_data_curobo
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

check start state validity and minor fixes

Browse Source
This commit is contained in:
Balakumar Sundaralingam
2024-05-14 22:16:12 -07:00
parent 7196be75f5
commit 911da8cb24
8 changed files with 175 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
CHANGELOG.md
View File

@@ -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

2
src/curobo/content/configs/robot/ur5e_robotiq_2f_140.yml
View File

@@ -23,7 +23,7 @@ robot_cfg:
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" ,
#"link_name": "attached_object", "fixed_transform": [0,0,0,1,0,0,0], "joint_type":"FIXED",

9
src/curobo/cuda_robot_model/cuda_robot_model.py
View File

@@ -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:

13
src/curobo/curobolib/cpp/self_collision_kernel.cu
View File

@@ -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] =

5
src/curobo/rollout/dynamics_model/kinematic_model.py
View File

@@ -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()

20
src/curobo/wrap/reacher/ik_solver.py
View File

@@ -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(

42
src/curobo/wrap/reacher/motion_gen.py
View File

@@ -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]

54
tests/motion_gen_module_test.py
View File

@@ -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