Generating and Executing Robot Grasps#
This tutorial demonstrates how to generate antipodal grasp poses for a target object and execute a full grasp trajectory with a robot arm. It covers scene initialization, robot and object creation, interactive grasp region annotation, grasp pose computation, and trajectory execution in the simulation loop.
The Code#
The tutorial corresponds to the grasp_generator.py script in the scripts/tutorials/grasp directory.
Code for grasp_generator.py
1# ----------------------------------------------------------------------------
2# Copyright (c) 2021-2026 DexForce Technology Co., Ltd.
3#
4# Licensed under the Apache License, Version 2.0 (the "License");
5# you may not use this file except in compliance with the License.
6# You may obtain a copy of the License at
7#
8# http://www.apache.org/licenses/LICENSE-2.0
9#
10# Unless required by applicable law or agreed to in writing, software
11# distributed under the License is distributed on an "AS IS" BASIS,
12# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13# See the License for the specific language governing permissions and
14# limitations under the License.
15# ----------------------------------------------------------------------------
16
17"""
18This script demonstrates the creation and simulation of a robot that grasps a rigid mug
19in a simulated environment using the SimulationManager and grasp planning utilities.
20"""
21
22import argparse
23import numpy as np
24import time
25import torch
26
27from embodichain.lab.sim import SimulationManager, SimulationManagerCfg
28from embodichain.lab.sim.objects import Robot, RigidObject
29from embodichain.lab.sim.utility.action_utils import interpolate_with_distance
30from embodichain.lab.sim.shapes import MeshCfg
31from embodichain.lab.sim.solvers import PytorchSolverCfg
32from embodichain.data import get_data_path
33from embodichain.utils import logger
34from embodichain.lab.sim.cfg import (
35 JointDrivePropertiesCfg,
36 RobotCfg,
37 LightCfg,
38 RigidBodyAttributesCfg,
39 RigidObjectCfg,
40 URDFCfg,
41)
42from embodichain.toolkits.graspkit.pg_grasp.antipodal_generator import (
43 GraspGenerator,
44 GraspGeneratorCfg,
45 AntipodalSamplerCfg,
46)
47from embodichain.toolkits.graspkit.pg_grasp.gripper_collision_checker import (
48 GripperCollisionCfg,
49)
50
51
52def parse_arguments():
53 """
54 Parse command-line arguments to configure the simulation.
55
56 Returns:
57 argparse.Namespace: Parsed arguments including number of environments and rendering options.
58 """
59 parser = argparse.ArgumentParser(
60 description="Create and simulate a robot in SimulationManager"
61 )
62 parser.add_argument(
63 "--num_envs", type=int, default=1, help="Number of parallel environments"
64 )
65 parser.add_argument(
66 "--enable_rt", action="store_true", help="Enable ray tracing rendering"
67 )
68 parser.add_argument("--headless", action="store_true", help="Enable headless mode")
69 parser.add_argument(
70 "--device",
71 type=str,
72 default="cpu",
73 help="device to run the environment on, e.g., 'cpu' or 'cuda'",
74 )
75 return parser.parse_args()
76
77
78def initialize_simulation(args) -> SimulationManager:
79 """
80 Initialize the simulation environment based on the provided arguments.
81
82 Args:
83 args (argparse.Namespace): Parsed command-line arguments.
84
85 Returns:
86 SimulationManager: Configured simulation manager instance.
87 """
88 config = SimulationManagerCfg(
89 headless=True,
90 sim_device=args.device,
91 enable_rt=args.enable_rt,
92 physics_dt=1.0 / 100.0,
93 arena_space=2.5,
94 )
95 sim = SimulationManager(config)
96
97 if args.enable_rt:
98 light = sim.add_light(
99 cfg=LightCfg(
100 uid="main_light",
101 color=(0.6, 0.6, 0.6),
102 intensity=30.0,
103 init_pos=(1.0, 0, 3.0),
104 )
105 )
106
107 return sim
108
109
110def create_robot(sim: SimulationManager, position=[0.0, 0.0, 0.0]) -> Robot:
111 """
112 Create and configure a robot with an arm and a dexterous hand in the simulation.
113
114 Args:
115 sim (SimulationManager): The simulation manager instance.
116
117 Returns:
118 Robot: The configured robot instance added to the simulation.
119 """
120 # Retrieve URDF paths for the robot arm and hand
121 ur10_urdf_path = get_data_path("UniversalRobots/UR10/UR10.urdf")
122 gripper_urdf_path = get_data_path("DH_PGC_140_50_M/DH_PGC_140_50_M.urdf")
123 # Configure the robot with its components and control properties
124 cfg = RobotCfg(
125 uid="UR10",
126 urdf_cfg=URDFCfg(
127 components=[
128 {"component_type": "arm", "urdf_path": ur10_urdf_path},
129 {"component_type": "hand", "urdf_path": gripper_urdf_path},
130 ]
131 ),
132 drive_pros=JointDrivePropertiesCfg(
133 stiffness={"JOINT[0-9]": 1e4, "FINGER[1-2]": 1e3},
134 damping={"JOINT[0-9]": 1e3, "FINGER[1-2]": 1e2},
135 max_effort={"JOINT[0-9]": 1e5, "FINGER[1-2]": 1e4},
136 drive_type="force",
137 ),
138 control_parts={
139 "arm": ["JOINT[0-9]"],
140 "hand": ["FINGER[1-2]"],
141 },
142 solver_cfg={
143 "arm": PytorchSolverCfg(
144 end_link_name="ee_link",
145 root_link_name="base_link",
146 tcp=[
147 [0.0, 1.0, 0.0, 0.0],
148 [-1.0, 0.0, 0.0, 0.0],
149 [0.0, 0.0, 1.0, 0.12],
150 [0.0, 0.0, 0.0, 1.0],
151 ],
152 )
153 },
154 init_qpos=[0.0, -np.pi / 2, -np.pi / 2, np.pi / 2, -np.pi / 2, 0.0, 0.0, 0.0],
155 init_pos=position,
156 )
157 return sim.add_robot(cfg=cfg)
158
159
160def create_mug(sim: SimulationManager):
161 mug_cfg = RigidObjectCfg(
162 uid="table",
163 shape=MeshCfg(
164 fpath=get_data_path("CoffeeCup/cup.ply"),
165 ),
166 attrs=RigidBodyAttributesCfg(
167 mass=0.01,
168 dynamic_friction=0.97,
169 static_friction=0.99,
170 ),
171 max_convex_hull_num=16,
172 init_pos=[0.55, 0.0, 0.01],
173 init_rot=[0.0, 0.0, -90],
174 body_scale=(4, 4, 4),
175 )
176 mug = sim.add_rigid_object(cfg=mug_cfg)
177 return mug
178
179
180def get_grasp_traj(sim: SimulationManager, robot: Robot, grasp_xpos: torch.Tensor):
181 n_envs = sim.num_envs
182 rest_arm_qpos = robot.get_qpos("arm")
183
184 approach_xpos = grasp_xpos.clone()
185 approach_xpos[:, 2, 3] += 0.1
186
187 _, qpos_approach = robot.compute_ik(
188 pose=approach_xpos, joint_seed=rest_arm_qpos, name="arm"
189 )
190 _, qpos_grasp = robot.compute_ik(
191 pose=grasp_xpos, joint_seed=qpos_approach, name="arm"
192 )
193 hand_open_qpos = torch.tensor([0.00, 0.00], dtype=torch.float32, device=sim.device)
194 hand_close_qpos = torch.tensor(
195 [0.025, 0.025], dtype=torch.float32, device=sim.device
196 )
197
198 arm_trajectory = torch.cat(
199 [
200 rest_arm_qpos[:, None, :],
201 qpos_approach[:, None, :],
202 qpos_grasp[:, None, :],
203 qpos_grasp[:, None, :],
204 qpos_approach[:, None, :],
205 rest_arm_qpos[:, None, :],
206 ],
207 dim=1,
208 )
209 hand_trajectory = torch.cat(
210 [
211 hand_open_qpos[None, None, :].repeat(n_envs, 1, 1),
212 hand_open_qpos[None, None, :].repeat(n_envs, 1, 1),
213 hand_open_qpos[None, None, :].repeat(n_envs, 1, 1),
214 hand_close_qpos[None, None, :].repeat(n_envs, 1, 1),
215 hand_close_qpos[None, None, :].repeat(n_envs, 1, 1),
216 hand_close_qpos[None, None, :].repeat(n_envs, 1, 1),
217 ],
218 dim=1,
219 )
220 all_trajectory = torch.cat([arm_trajectory, hand_trajectory], dim=-1)
221 interp_trajectory = interpolate_with_distance(
222 trajectory=all_trajectory, interp_num=200, device=sim.device
223 )
224 return interp_trajectory
225
226
227if __name__ == "__main__":
228 import time
229
230 args = parse_arguments()
231 sim = initialize_simulation(args)
232 robot = create_robot(sim, position=[0.0, 0.0, 0.0])
233 mug = create_mug(sim)
234
235 # get mug grasp pose
236 grasp_cfg = GraspGeneratorCfg(
237 viser_port=11801,
238 antipodal_sampler_cfg=AntipodalSamplerCfg(
239 n_sample=20000, max_length=0.088, min_length=0.003
240 ),
241 )
242 sim.open_window()
243
244 # Annotate part of the mug to be grasped by following the instructions in the visualization window:
245 # 1. View grasp object in browser (e.g http://localhost:11801)
246 # 2. press 'Rect Select Region', select grasp region
247 # 3. press 'Confirm Selection' to finish grasp region selection.
248
249 start_time = time.time()
250
251 gripper_collision_cfg = GripperCollisionCfg(
252 max_open_length=0.088, finger_length=0.078, point_sample_dense=0.012
253 )
254
255 # Extract mesh data from the mug and create grasp generator
256 vertices = mug.get_vertices(env_ids=[0], scale=True)[0]
257 triangles = mug.get_triangles(env_ids=[0])[0]
258 grasp_generator = GraspGenerator(
259 vertices=vertices,
260 triangles=triangles,
261 cfg=grasp_cfg,
262 gripper_collision_cfg=gripper_collision_cfg,
263 )
264
265 # Annotate grasp region (populates internal antipodal point pairs)
266 grasp_generator.annotate()
267
268 # Compute grasp poses per environment
269 approach_direction = torch.tensor(
270 [0, 0, -1], dtype=torch.float32, device=sim.device
271 )
272 obj_poses = mug.get_local_pose(to_matrix=True)
273 grasp_xpos_list = []
274
275 rest_xpos = robot.compute_fk(
276 qpos=robot.get_qpos("arm"), name="arm", to_matrix=True
277 )[0]
278 for i, obj_pose in enumerate(obj_poses):
279 is_success, grasp_pose, open_length = grasp_generator.get_grasp_poses(
280 obj_pose, approach_direction, visualize_pose=False
281 )
282 if is_success:
283 grasp_xpos_list.append(grasp_pose.unsqueeze(0))
284 else:
285 logger.log_warning(f"No valid grasp pose found for {i}-th object.")
286 grasp_xpos_list.append(rest_xpos.unsqueeze(0))
287
288 grasp_xpos = torch.cat(grasp_xpos_list, dim=0)
289 cost_time = time.time() - start_time
290 logger.log_info(f"Get grasp pose cost time: {cost_time:.2f} seconds")
291
292 grab_traj = get_grasp_traj(sim, robot, grasp_xpos)
293 input("Press Enter to start the grab mug demo...")
294 n_waypoint = grab_traj.shape[1]
295 for i in range(n_waypoint):
296 robot.set_qpos(grab_traj[:, i, :])
297 sim.update(step=4)
298 time.sleep(1e-2)
299 input("Press Enter to exit the simulation...")
The Code Explained#
Configuring the simulation#
Command-line arguments are parsed with argparse to select the number of parallel environments, the compute device, and optional rendering features such as ray tracing and headless mode.
def parse_arguments():
"""
Parse command-line arguments to configure the simulation.
Returns:
argparse.Namespace: Parsed arguments including number of environments and rendering options.
"""
parser = argparse.ArgumentParser(
description="Create and simulate a robot in SimulationManager"
)
parser.add_argument(
"--num_envs", type=int, default=1, help="Number of parallel environments"
)
parser.add_argument(
"--enable_rt", action="store_true", help="Enable ray tracing rendering"
)
parser.add_argument("--headless", action="store_true", help="Enable headless mode")
parser.add_argument(
"--device",
type=str,
default="cpu",
help="device to run the environment on, e.g., 'cpu' or 'cuda'",
)
return parser.parse_args()
The parsed arguments are passed to initialize_simulation, which builds a SimulationManagerCfg and creates the SimulationManager instance. When ray tracing is enabled a directional cfg.LightCfg is also added to the scene.
def initialize_simulation(args) -> SimulationManager:
"""
Initialize the simulation environment based on the provided arguments.
Args:
args (argparse.Namespace): Parsed command-line arguments.
Returns:
SimulationManager: Configured simulation manager instance.
"""
config = SimulationManagerCfg(
headless=True,
sim_device=args.device,
enable_rt=args.enable_rt,
physics_dt=1.0 / 100.0,
arena_space=2.5,
)
sim = SimulationManager(config)
if args.enable_rt:
light = sim.add_light(
cfg=LightCfg(
uid="main_light",
color=(0.6, 0.6, 0.6),
intensity=30.0,
init_pos=(1.0, 0, 3.0),
)
)
return sim
Creating a robot and a target object#
A UR10 arm with a parallel-jaw gripper is created via SimulationManager.add_robot(). The gripper URDF and drive properties are configured so that the arm joints and finger joints can be controlled independently.
def create_robot(sim: SimulationManager, position=[0.0, 0.0, 0.0]) -> Robot:
"""
Create and configure a robot with an arm and a dexterous hand in the simulation.
Args:
sim (SimulationManager): The simulation manager instance.
Returns:
Robot: The configured robot instance added to the simulation.
"""
# Retrieve URDF paths for the robot arm and hand
ur10_urdf_path = get_data_path("UniversalRobots/UR10/UR10.urdf")
gripper_urdf_path = get_data_path("DH_PGC_140_50_M/DH_PGC_140_50_M.urdf")
# Configure the robot with its components and control properties
cfg = RobotCfg(
uid="UR10",
urdf_cfg=URDFCfg(
components=[
{"component_type": "arm", "urdf_path": ur10_urdf_path},
{"component_type": "hand", "urdf_path": gripper_urdf_path},
]
),
drive_pros=JointDrivePropertiesCfg(
stiffness={"JOINT[0-9]": 1e4, "FINGER[1-2]": 1e3},
damping={"JOINT[0-9]": 1e3, "FINGER[1-2]": 1e2},
max_effort={"JOINT[0-9]": 1e5, "FINGER[1-2]": 1e4},
drive_type="force",
),
control_parts={
"arm": ["JOINT[0-9]"],
"hand": ["FINGER[1-2]"],
},
solver_cfg={
"arm": PytorchSolverCfg(
end_link_name="ee_link",
root_link_name="base_link",
tcp=[
[0.0, 1.0, 0.0, 0.0],
[-1.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 1.0, 0.12],
[0.0, 0.0, 0.0, 1.0],
],
)
},
init_qpos=[0.0, -np.pi / 2, -np.pi / 2, np.pi / 2, -np.pi / 2, 0.0, 0.0, 0.0],
init_pos=position,
)
return sim.add_robot(cfg=cfg)
The target object (a mug) is loaded as a objects.RigidObject from a PLY mesh file:
def create_mug(sim: SimulationManager):
mug_cfg = RigidObjectCfg(
uid="table",
shape=MeshCfg(
fpath=get_data_path("CoffeeCup/cup.ply"),
),
attrs=RigidBodyAttributesCfg(
mass=0.01,
dynamic_friction=0.97,
static_friction=0.99,
),
max_convex_hull_num=16,
init_pos=[0.55, 0.0, 0.01],
init_rot=[0.0, 0.0, -90],
body_scale=(4, 4, 4),
)
mug = sim.add_rigid_object(cfg=mug_cfg)
return mug
Annotating and computing grasp poses#
Grasp generation is performed by GraspGenerator, which runs an antipodal sampler on the object mesh. The mesh data (vertices and triangles) is extracted from the objects.RigidObject via its accessor methods. A GraspGeneratorCfg controls sampler parameters (sample count, gripper jaw limits) and the interactive annotation workflow:
Open the visualization in a browser at the reported port (e.g.
http://localhost:11801).Use Rect Select Region to highlight the area of the object that should be grasped.
Click Confirm Selection to finalize the region.
After annotation, antipodal point pairs are cached to disk and automatically reused unless user call GraspGenerator.annotate().
For each environment, a grasp pose is computed by calling get_grasp_poses() with the object pose and desired approach direction. The result is a (4, 4) homogeneous transformation matrix representing the grasp frame in world coordinates. Set visualize=True to open an Open3D window showing the selected grasp on the object.
The approach direction is the unit vector along which the gripper approaches the object. In this tutorial, we use a fixed approach direction (straight down in world frame) for simplicity, but it can be customized based on the task or object geometry.
gripper_collision_cfg = GripperCollisionCfg(
max_open_length=0.088, finger_length=0.078, point_sample_dense=0.012
)
# Extract mesh data from the mug and create grasp generator
vertices = mug.get_vertices(env_ids=[0], scale=True)[0]
triangles = mug.get_triangles(env_ids=[0])[0]
grasp_generator = GraspGenerator(
vertices=vertices,
triangles=triangles,
cfg=grasp_cfg,
gripper_collision_cfg=gripper_collision_cfg,
)
# Annotate grasp region (populates internal antipodal point pairs)
grasp_generator.annotate()
# Compute grasp poses per environment
approach_direction = torch.tensor(
[0, 0, -1], dtype=torch.float32, device=sim.device
)
obj_poses = mug.get_local_pose(to_matrix=True)
grasp_xpos_list = []
rest_xpos = robot.compute_fk(
qpos=robot.get_qpos("arm"), name="arm", to_matrix=True
)[0]
for i, obj_pose in enumerate(obj_poses):
is_success, grasp_pose, open_length = grasp_generator.get_grasp_poses(
obj_pose, approach_direction, visualize_pose=False
)
if is_success:
grasp_xpos_list.append(grasp_pose.unsqueeze(0))
else:
logger.log_warning(f"No valid grasp pose found for {i}-th object.")
grasp_xpos_list.append(rest_xpos.unsqueeze(0))
grasp_xpos = torch.cat(grasp_xpos_list, dim=0)
cost_time = time.time() - start_time
logger.log_info(f"Get grasp pose cost time: {cost_time:.2f} seconds")
Building and executing the grasp trajectory#
Once a grasp pose is obtained, a waypoint trajectory is built that moves the arm from its rest configuration to an approach pose (offset above the grasp), down to the grasp pose, closes the fingers, lifts, and returns. The trajectory is interpolated for smooth motion and executed step-by-step in the simulation loop.
def get_grasp_traj(sim: SimulationManager, robot: Robot, grasp_xpos: torch.Tensor):
n_envs = sim.num_envs
rest_arm_qpos = robot.get_qpos("arm")
approach_xpos = grasp_xpos.clone()
approach_xpos[:, 2, 3] += 0.1
_, qpos_approach = robot.compute_ik(
pose=approach_xpos, joint_seed=rest_arm_qpos, name="arm"
)
_, qpos_grasp = robot.compute_ik(
pose=grasp_xpos, joint_seed=qpos_approach, name="arm"
)
hand_open_qpos = torch.tensor([0.00, 0.00], dtype=torch.float32, device=sim.device)
hand_close_qpos = torch.tensor(
[0.025, 0.025], dtype=torch.float32, device=sim.device
)
arm_trajectory = torch.cat(
[
rest_arm_qpos[:, None, :],
qpos_approach[:, None, :],
qpos_grasp[:, None, :],
qpos_grasp[:, None, :],
qpos_approach[:, None, :],
rest_arm_qpos[:, None, :],
],
dim=1,
)
hand_trajectory = torch.cat(
[
hand_open_qpos[None, None, :].repeat(n_envs, 1, 1),
hand_open_qpos[None, None, :].repeat(n_envs, 1, 1),
hand_open_qpos[None, None, :].repeat(n_envs, 1, 1),
hand_close_qpos[None, None, :].repeat(n_envs, 1, 1),
hand_close_qpos[None, None, :].repeat(n_envs, 1, 1),
hand_close_qpos[None, None, :].repeat(n_envs, 1, 1),
],
dim=1,
)
all_trajectory = torch.cat([arm_trajectory, hand_trajectory], dim=-1)
interp_trajectory = interpolate_with_distance(
trajectory=all_trajectory, interp_num=200, device=sim.device
)
return interp_trajectory
Configuring GraspGeneratorCfg#
GraspGeneratorCfg controls the overall grasp annotation workflow. The key parameters are listed below.
Parameter |
Default |
Description |
|---|---|---|
|
|
Port used by the Viser browser-based visualizer for interactive grasp region annotation. |
|
|
When |
|
|
Nested configuration for the antipodal point sampler. See the table below for its parameters. |
|
|
Maximum allowed angle (in radians) between the specified approach direction and the axis connecting an antipodal point pair. Pairs that deviate more than this threshold are discarded. |
The antipodal_sampler_cfg field accepts an AntipodalSamplerCfg instance, which controls how antipodal point pairs are sampled on the mesh surface.
Parameter |
Default |
Description |
|---|---|---|
|
|
Number of surface points uniformly sampled from the mesh before ray casting. Higher values yield denser coverage but increase computation time. |
|
|
Maximum angle (in radians) used to randomly perturb the ray direction away from the inward normal. Larger values increase diversity of sampled antipodal pairs. Setting this to |
|
|
Maximum allowed distance (in metres) between an antipodal pair. Pairs farther apart than this value are discarded; set this to match the maximum gripper jaw opening width. |
|
|
Minimum allowed distance (in metres) between an antipodal pair. Pairs closer together than this value are discarded to avoid degenerate or self-intersecting grasps. |
Configuring GripperCollisionCfg#
GripperCollisionCfg models the geometry of a parallel-jaw gripper as a point cloud and is used to filter out grasp candidates that would collide with the object. All length parameters are in metres.
Parameter |
Default |
Description |
|---|---|---|
|
|
Maximum finger separation of the gripper when fully open. Should match the physical gripper specification. |
|
|
Length of each finger along the Z-axis (depth direction from the root). Should match the physical gripper specification. |
|
|
Thickness of the gripper body and fingers along the Y-axis (perpendicular to the opening direction). |
|
|
Thickness of each finger along the X-axis (parallel to the opening direction). |
|
|
Extent of the gripper root block along the Z-axis. |
|
|
Approximate number of sample points per unit length along each edge of the gripper point cloud. Higher values produce denser point clouds and improve collision-check accuracy at the cost of additional computation. |
|
|
Maximum number of convex hulls used when decomposing the object mesh for collision checking. More hulls give a tighter shape approximation but increase cost. |
|
|
Extra clearance added to the gripper open length during collision checking to account for pose uncertainty or mesh inaccuracies. |
The Code Execution#
To run the script, execute the following command from the project root:
python scripts/tutorials/grasp/grasp_generator.py
A simulation window will open showing the robot and the mug. A browser-based visualizer will also launch (default port 11801) for interactive grasp region annotation.
You can customize the run with additional arguments:
python scripts/tutorials/grasp/grasp_generator.py --num_envs <n> --device <cuda/cpu> --enable_rt --headless
After confirming the grasp region in the browser, the script will compute a grasp pose, print the elapsed time, and then wait for you to press Enter before executing the full grasp trajectory in the simulation. Press Enter again to exit once the motion is complete.
Grasp Annotation CLI#
EmbodiChain provides a dedicated CLI for interactively annotating grasp regions on a mesh and caching the resulting antipodal point pairs, without requiring a full simulation environment.
Basic usage:
python -m embodichain annotate-grasp --mesh_path /path/to/object.ply
This will:
Load the mesh file via
trimesh.Launch a browser-based annotator (default port
15531).Open http://localhost:15531 in your browser, use Rect Select Region to highlight the graspable area, then click Confirm Selection.
Compute antipodal point pairs on the selected region and cache them to disk.
Common options:
python -m embodichain annotate-grasp \
--mesh_path /path/to/object.ply \
--viser_port 15531 \
--n_sample 20000 \
--max_length 0.1 \
--min_length 0.001 \
Option |
Default |
Description |
|---|---|---|
|
(required) |
Path to the mesh file ( |
|
|
Port for the browser-based annotation UI. |
|
|
Number of surface points to sample for antipodal pair detection. |
|
|
Maximum distance (metres) between antipodal pairs; should match the gripper’s maximum opening width. |
|
|
Minimum distance (metres) between antipodal pairs; filters out degenerate pairs. |
|
|
Compute device ( |