tangger/issacdataengine
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: add test tube pick task with custom assets and grasp annotations

Browse Source 
- Add pick_test_tube task: USDC asset repackaging, grasp generation, task config
- Add tools: usdc_to_obj.py, repackage_test_tube.py, fix_test_tube_materials.py
- Add custom_task_guide.md: full Chinese documentation for creating custom tasks
- Add crawled InternDataEngine online docs (23 pages)
- Add grasp generation script (gen_tube_grasp.py) and pipeline config
This commit is contained in:
Tangger
2026-04-05 11:01:59 +08:00
parent 6314603676
commit 3d6b73753a
36 changed files with 18013 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

711
docs_crawled/concepts_skills_place.md Normal file
View File

@@ -0,0 +1,711 @@
# Source: https://internrobotics.github.io/InternDataEngine-Docs/concepts/skills/place.html
# Place Skill [​](#place-skill)
The `Place `skill performs placement operations with constrained end-effector orientations. It generates valid place poses through random sampling and filtering, then executes the placement motion.
Code Example:
python
```
# Source workflows/simbox/core/skills/place.py
from copy import deepcopy
import numpy as np
from core.skills.base_skill import BaseSkill, register_skill
from core.utils.box import Box, get_bbox_center_and_corners
from core.utils.constants import CUROBO_BATCH_SIZE
from core.utils.iou import IoU
from core.utils.plan_utils import (
select_index_by_priority_dual,
select_index_by_priority_single,
)
from core.utils.transformation_utils import create_pose_matrices, poses_from_tf_matrices
from core.utils.usd_geom_utils import compute_bbox
from omegaconf import DictConfig
from omni.isaac.core.controllers import BaseController
from omni.isaac.core.robots.robot import Robot
from omni.isaac.core.tasks import BaseTask
from omni.isaac.core.utils.prims import get_prim_at_path
from omni.isaac.core.utils.transformations import (
get_relative_transform,
pose_from_tf_matrix,
tf_matrix_from_pose,
)
from scipy.spatial.transform import Rotation as R
@register_skill
class Place(BaseSkill):
def __init__(self, robot: Robot, controller: BaseController, task: BaseTask, cfg: DictConfig, *args, **kwargs):
super().__init__()
self.robot = robot
self.controller = controller
self.task = task
self.name = cfg["name"]
self.pick_obj = task._task_objects[cfg["objects"][0]]
self.place_obj = task._task_objects[cfg["objects"][1]]
self.place_align_axis = cfg.get("place_align_axis", None)
self.pick_align_axis = cfg.get("pick_align_axis", None)
self.constraint_gripper_x = cfg.get("constraint_gripper_x", False)
self.place_part_prim_path = cfg.get("place_part_prim_path", None)
if self.place_part_prim_path:
self.place_prim_path = f"{self.place_obj.prim_path}/{self.place_part_prim_path}"
else:
self.place_prim_path = self.place_obj.prim_path
self.manip_list = []
self.robot_ee_path = self.controller.robot_ee_path
self.robot_base_path = self.controller.robot_base_path
self.skill_cfg = cfg
self.align_pick_obj_axis = self.skill_cfg.get("align_pick_obj_axis", None)
self.align_place_obj_axis = self.skill_cfg.get("align_place_obj_axis", None)
self.align_plane_x_axis = self.skill_cfg.get("align_plane_x_axis", None)
self.align_plane_y_axis = self.skill_cfg.get("align_plane_y_axis", None)
self.align_obj_tol = self.skill_cfg.get("align_obj_tol", None)
def simple_generate_manip_cmds(self):
manip_list = []
p_base_ee_cur, q_base_ee_cur = self.controller.get_ee_pose()
cmd = (p_base_ee_cur, q_base_ee_cur, "update_pose_cost_metric", {"hold_vec_weight": None})
manip_list.append(cmd)
if self.skill_cfg.get("ignore_substring", []):
ignore_substring = deepcopy(self.controller.ignore_substring + self.skill_cfg.get("ignore_substring", []))
cmd = (
p_base_ee_cur,
q_base_ee_cur,
"update_specific",
{"ignore_substring": ignore_substring, "reference_prim_path": self.controller.reference_prim_path},
)
manip_list.append(cmd)
result = self.sample_gripper_place_traj()
cmd = (result[0][0], result[0][1], "close_gripper", {})
manip_list.append(cmd)
p_base_ee_place, q_base_ee_place = result[1][0], result[1][1]
cmd = (p_base_ee_place, q_base_ee_place, "close_gripper", {})
manip_list.append(cmd)
cmd = (p_base_ee_place, q_base_ee_place, "open_gripper", {})
manip_list.extend([cmd] * self.skill_cfg.get("gripper_change_steps", 10))
cmd = (p_base_ee_place, q_base_ee_place, "detach_obj", {})
manip_list.append(cmd)
self.manip_list = manip_list
self.place_ee_trans = p_base_ee_place
def sample_gripper_place_traj(self):
# ... sampling logic ...
pass
def generate_constrained_rotation_batch(self, batch_size=3000):
filter_conditions = {
"x": {
"forward": (0, 0, 1),
"backward": (0, 0, -1),
"leftward": (1, 0, 1),
"rightward": (1, 0, -1),
"upward": (2, 0, 1),
"downward": (2, 0, -1),
},
"y": {
"forward": (0, 1, 1),
"backward": (0, 1, -1),
"leftward": (1, 1, 1),
"rightward": (1, 1, -1),
"upward": (2, 1, 1),
"downward": (2, 1, -1),
},
"z": {
"forward": (0, 2, 1),
"backward": (0, 2, -1),
"leftward": (1, 2, 1),
"rightward": (1, 2, -1),
"upward": (2, 2, 1),
"downward": (2, 2, -1),
},
}
rot_mats = R.random(batch_size).as_matrix()
valid_mask = np.ones(batch_size, dtype=bool)
for axis in ["x", "y", "z"]:
filter_list = self.skill_cfg.get(f"filter_{axis}_dir", None)
if filter_list is not None:
direction = filter_list[0]
row, col, sign = filter_conditions[axis][direction]
elements = rot_mats[:, row, col]
if len(filter_list) == 2:
value = filter_list[1]
cos_val = np.cos(np.deg2rad(value))
if sign > 0:
valid_mask &= elements >= cos_val
else:
valid_mask &= elements <= cos_val
valid_rot_mats = rot_mats[valid_mask]
if len(valid_rot_mats) == 0:
return rot_mats[:CUROBO_BATCH_SIZE]
else:
indices = np.random.choice(len(valid_rot_mats), CUROBO_BATCH_SIZE)
return valid_rot_mats[indices]
def is_feasible(self, th=5):
return self.controller.num_plan_failed <= th
def is_subtask_done(self, t_eps=1e-3, o_eps=5e-3):
assert len(self.manip_list) != 0
p_base_ee_cur, q_base_ee_cur = self.controller.get_ee_pose()
p_base_ee, q_base_ee, *_ = self.manip_list[0]
diff_trans = np.linalg.norm(p_base_ee_cur - p_base_ee)
diff_ori = 2 * np.arccos(min(abs(np.dot(q_base_ee_cur, q_base_ee)), 1.0))
pose_flag = np.logical_and(diff_trans < t_eps, diff_ori < o_eps)
self.plan_flag = self.controller.num_last_cmd > 10
return np.logical_or(pose_flag, self.plan_flag)
def is_done(self):
if len(self.manip_list) == 0:
return True
if self.is_subtask_done(t_eps=self.skill_cfg.get("t_eps", 1e-3), o_eps=self.skill_cfg.get("o_eps", 5e-3)):
self.manip_list.pop(0)
return len(self.manip_list) == 0
def is_success(self, th=0.0):
if self.skill_cfg.get("success_mode", "3diou") == "3diou":
bbox_pick_obj = compute_bbox(self.pick_obj.prim)
bbox_place_obj = compute_bbox(get_prim_at_path(self.place_prim_path))
iou = IoU(
Box(get_bbox_center_and_corners(bbox_pick_obj)), Box(get_bbox_center_and_corners(bbox_place_obj))
).iou()
return iou > th
elif self.skill_cfg.get("success_mode", "3diou") == "xybbox":
bbox_place_obj = compute_bbox(get_prim_at_path(self.place_prim_path))
pick_x, pick_y = self.pick_obj.get_local_pose()[0][:2]
place_xy_min = bbox_place_obj.min[:2]
place_xy_max = bbox_place_obj.max[:2]
return ((place_xy_min[0] + 0.015) < pick_x < (place_xy_max[0] - 0.015)) and (
(place_xy_min[1] + 0.015) < pick_y < (place_xy_max[1] - 0.015)
)
```
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
__init__(self, robot, controller, task, cfg, *args, **kwargs)
Initialize the place skill with target objects and constraints.
Parameters:
- **robot **( Robot ): Robot instance for state queries and actions.
- **controller **( BaseController ): Controller for motion planning.
- **task **( BaseTask ): Task instance containing scene objects.
- **cfg **( DictConfig ): Skill configuration from task YAML.
Key Operations:
- Extract pick object from `cfg["objects"][0] `
- Extract place container from `cfg["objects"][1] `
- Initialize alignment constraints ( `align_pick_obj_axis `, `align_place_obj_axis `)
- Set up collision filtering paths
simple_generate_manip_cmds(self)
Generate the full placement motion sequence.
Steps:
- **Update planning settings **— Reset cost metrics and collision settings
- **Generate place trajectory **— Call `sample_gripper_place_traj() `to get pre-place and place poses
- **Build manip_list **— Construct command sequence:
- Move to pre-place pose (gripper closed)
- Move to place pose
- Open gripper to release object
- Detach object from gripper (physics)
- Retreat motion (if `post_place_vector `configured)
sample_gripper_place_traj(self)
Generate pre-place and place poses based on placement direction mode.
Returns:
- list : `[T_base_ee_preplace, T_base_ee_place] `or `[T_base_ee_preplace, T_base_ee_place, T_base_ee_postplace] `
Position Constraint Modes:
- **`"gripper" `**: Target pose controls gripper position directly
- **`"object" `**: Target pose controls pick object position (accounts for object offset from EE)
Direction Modes:
See [Placement Direction Modes](#placement-direction-modes)section below.
generate_constrained_rotation_batch(self, batch_size=3000)
Generate valid end-effector orientations through random sampling and filtering.
Parameters:
- **batch_size **( int ): Number of random rotations to sample.
Returns:
- np.ndarray : Filtered rotation matrices `(N, 3, 3) `.
Filtering Logic:
- Sample random rotation matrices using `scipy.spatial.transform.Rotation `
- Apply direction filters ( `filter_x_dir `, `filter_y_dir `, `filter_z_dir `)
- Apply object alignment constraints (if configured)
- Return filtered rotations
For detailed filtering explanation, see [Orientation Filtering](#orientation-filtering).
is_success(self, th=0.0)
Check if the placement succeeded based on configured mode.
Parameters:
- **th **( float ): Threshold for IoU-based success.
Returns:
- bool : `True `if success conditions are satisfied.
Success Modes:
| Mode | Condition |
| `3diou ` | 3D IoU between pick object and container > threshold |
| `xybbox ` | Pick object center within container XY bounds |
| `height ` | Pick object below height threshold |
| `left `/ `right ` | Object positioned left/right of container |
| `flower ` | IoU + object center within container bounds |
| `cup ` | IoU + object above container base |
## Placement Direction Modes [​](#placement-direction-modes)
### Vertical Placement [​](#vertical-placement)
Default mode for placing objects on top of containers (e.g., placing items in boxes, on plates).
yaml
```
place_direction: "vertical"
```
1
**Algorithm **:
- Sample (x, y) position within container top surface using ratio ranges
- Set z-height to `b_max[2] + pre_place_z_offset `(above container)
- Lower to `b_max[2] + place_z_offset `(final placement height)
yaml
```
x_ratio_range: [0.4, 0.6] # X position ratio within container bbox
y_ratio_range: [0.4, 0.6] # Y position ratio within container bbox
pre_place_z_offset: 0.2 # Height above container for approach
place_z_offset: 0.1 # Final placement height offset
```
1
2
3
4
### Horizontal Placement [​](#horizontal-placement)
For inserting objects into slots, shelves, or openings from the side.
yaml
```
place_direction: "horizontal"
align_place_obj_axis: [1, 0, 0] # Insertion direction
offset_place_obj_axis: [0, 0, 1] # Offset direction
```
1
2
3
**Algorithm **:
- Sample position within container bounding box
- Compute approach position offset along alignment axis
- Move forward along alignment axis to place position
yaml
```
# Position constraint: "object" or "gripper"
position_constraint: "object"
# Approach distances
pre_place_align: 0.2 # Pre-place offset along alignment axis
pre_place_offset: 0.2 # Pre-place offset along offset axis
place_align: 0.1 # Place offset along alignment axis
place_offset: 0.1 # Place offset along offset axis
```
1
2
3
4
5
6
7
8
## Orientation Filtering [​](#orientation-filtering)
The place skill uses the same **direction-based filtering strategy **as the pick skill. See [Pick Skill - Grasp Orientation Filtering](/InternDataEngine-Docs/concepts/skills/pick/#grasp-orientation-filtering)for detailed explanation.
### Filter Parameters [​](#filter-parameters)
- **filter_x_dir **( list ): Filter based on EE's X-axis direction in arm base frame.
- **filter_y_dir **( list ): Filter based on EE's Y-axis direction in arm base frame.
- **filter_z_dir **( list ): Filter based on EE's Z-axis direction in arm base frame.
**Format **: `[direction, angle] `or `[direction, angle_min, angle_max] `
### Direction Mapping [​](#direction-mapping)
- **forward **: EE axis dot arm_base_X ≥ cos(angle)
- **backward **: EE axis dot arm_base_X ≤ cos(angle)
- **leftward **: EE axis dot arm_base_Y ≥ cos(angle)
- **rightward **: EE axis dot arm_base_Y ≤ cos(angle)
- **upward **: EE axis dot arm_base_Z ≥ cos(angle)
- **downward **: EE axis dot arm_base_Z ≤ cos(angle)
### Object Alignment Constraint [​](#object-alignment-constraint)
Additional constraint to align pick object axis with place container axis:
yaml
```
align_pick_obj_axis: [0, 0, 1] # Axis on pick object (e.g., height axis)
align_place_obj_axis: [0, 0, 1] # Target axis on place container
align_obj_tol: 15 # Alignment tolerance (degrees)
```
1
2
3
This ensures that a specific axis on the held object (e.g., bottle height) aligns with a target axis on the container (e.g., cup opening direction).
## Design Philosophy [​](#design-philosophy)
Note
**Random Generation + Filter **: The place skill uses a random generation and filtering strategy rather than delicate construction. This approach is chosen for three key reasons:
-
**Intuitive Position Control **: Specifying place position based on target EE activity direction and object bounding box range is intuitive and easy to configure.
-
**Simple Rotation Sampling **: Randomly generating 3x3 rotation matrices and filtering them is computationally simple. With sufficient samples, valid orientations that pass planning constraints are always found.
-
**Container Diversity **: Different place containers have varying volumes, shapes, and opening directions. Delicate construction of place poses for each container type is difficult. The filtering approach is general and adaptable.
## Configuration Reference [​](#configuration-reference)
- **objects **( list , default: required): `[pick_object, place_container] `.
- **place_part_prim_path **( string , default: `None `): Sub-path within place container prim.
- **place_direction **( string , default: `"vertical" `): `"vertical" `or `"horizontal" `.
- **position_constraint **( string , default: `"gripper" `): `"gripper" `or `"object" `.
- **pre_place_z_offset **( float , default: `0.2 `): Approach height above container.
- **place_z_offset **( float , default: `0.1 `): Final placement height.
- **x_ratio_range **( [float, float] , default: `[0.4, 0.6] `): X-position sampling range.
- **y_ratio_range **( [float, float] , default: `[0.4, 0.6] `): Y-position sampling range.
- **z_ratio_range **( [float, float] , default: `[0.4, 0.6] `): Z-position sampling range (horizontal).
- **align_place_obj_axis **( [float, float, float] , default: `None `): Insertion axis on place container (horizontal).
- **offset_place_obj_axis **( [float, float, float] , default: `None `): Offset axis on place container (horizontal).
- **pre_place_align **( float , default: `0.2 `): Pre-place offset along alignment axis.
- **pre_place_offset **( float , default: `0.2 `): Pre-place offset along offset axis.
- **place_align **( float , default: `0.1 `): Place offset along alignment axis.
- **place_offset **( float , default: `0.1 `): Place offset along offset axis.
- **filter_x_dir **( list , default: `None `): EE X-axis direction filter.
- **filter_y_dir **( list , default: `None `): EE Y-axis direction filter.
- **filter_z_dir **( list , default: `None `): EE Z-axis direction filter.
- **align_pick_obj_axis **( [float, float, float] , default: `None `): Axis on pick object to align.
- **align_place_obj_axis **( [float, float, float] , default: `None `): Target axis on place container.
- **align_obj_tol **( float , default: `None `): Alignment tolerance (degrees).
- **align_plane_x_axis **( [float, float, float] , default: `None `): X-axis alignment plane.
- **align_plane_y_axis **( [float, float, float] , default: `None `): Y-axis alignment plane.
- **pre_place_hold_vec_weight **( list , default: `None `): Hold vector weight at pre-place.
- **post_place_hold_vec_weight **( list , default: `None `): Hold vector weight at place.
- **gripper_change_steps **( int , default: `10 `): Steps for gripper open action.
- **hesitate_steps **( int , default: `0 `): Pause steps before release.
- **post_place_vector **( [float, float, float] , default: `None `): Retreat direction after placement.
- **ignore_substring **( list , default: `[] `): Collision filter substrings.
- **test_mode **( string , default: `"forward" `): Motion test mode: `"forward" `or `"ik" `.
- **t_eps **( float , default: `1e-3 `): Translation tolerance (meters).
- **o_eps **( float , default: `5e-3 `): Orientation tolerance (radians).
- **success_mode **( string , default: `"3diou" `): Success evaluation mode.
- **success_th **( float , default: `0.0 `): IoU threshold for success.
- **threshold **( float , default: `0.03 `): Distance threshold for left/right modes.
## Example Configuration [​](#example-configuration)
### Vertical Place on Container [​](#vertical-place-on-container)
yaml
```
skills:
- lift2:
- left:
- name: place
objects: [pick_object, container]
place_direction: "vertical"
x_ratio_range: [0.4, 0.6]
y_ratio_range: [0.4, 0.6]
pre_place_z_offset: 0.15
place_z_offset: 0.05
filter_z_dir: ["downward", 140]
gripper_change_steps: 10
success_mode: "xybbox"
```
1
2
3
4
5
6
7
8
9
10
11
12
13
### Horizontal Insertion [​](#horizontal-insertion)
yaml
```
skills:
- franka:
- left:
- name: place
objects: [peg, hole]
place_direction: "horizontal"
position_constraint: "object"
align_place_obj_axis: [1, 0, 0]
offset_place_obj_axis: [0, 0, 1]
pre_place_align: 0.15
place_align: 0.02
filter_x_dir: ["forward", 30]
success_mode: "3diou"
```
1
2
3
4
5
6
7
8
9
10
11
12
13
### Place with Retreat [​](#place-with-retreat)
yaml
```
skills:
- split_aloha:
- left:
- name: place
objects: [item, shelf]
place_direction: "vertical"
post_place_vector: [-0.05, 0.0, 0.1]
gripper_change_steps: 15
```
1
2
3
4
5
6
7
8