├── .gitignore ├── README.md ├── grasp ├── __init__.py ├── grasp_sim.py ├── grasping_scenarios │ ├── __init__.py │ ├── franka.py │ ├── grasp_object.py │ └── scenario.py └── utils │ ├── __init__.py │ ├── isaac_utils.py │ └── visualize.py └── simulate_grasp.py /.gitignore: -------------------------------------------------------------------------------- 1 | *.txt 2 | *.dmp 3 | **/__pycache__/ 4 | *.pyc 5 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # isaac-sim-python: Python wrapper for NVIDIA Omniverse Isaac-Sim 2 | 3 | ## Overview 4 | This repository contains a collection of python wrappers for NVIDIA Omniverse Isaac-Sim simulations. `grasp` package simulates a planar grasp execution of a Panda arm in a scene with various rigid objects place in a bin. 5 | 6 | ## Installation 7 | This repository requires installation of NVIDIA Omniverse Isaac-Sim. A comprehensive setup tutorial is provided in the official [NVIDIA Omniverse Isaac-Sim](https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/setup.html) documentation. Following installation of Isaac-Sim, a conda environment must also be created that contains all the required packages for the python wrappers. Another comprehensive conda environment setup tutorial is provided in this [link](https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/python_samples.html). 8 | 9 | `ffmpeg-python` must be installed within the `isaac-sim` conda environment and can be aquired via a typical pip install: 10 | 11 | ``` 12 | conda activate isaac-sim 13 | pip install ffmpeg-python 14 | ``` 15 | 16 | Lastly, clone the repository into the `python-samples` sub-directory within the `isaac-sim` directory. 17 | 18 | ``` 19 | git clone https://github.com/erasromani/isaac-sim-python.git 20 | ``` 21 | 22 | ## Quickstart 23 | 24 | Navigate to the `python-samples` sub-directory within the `isaac-sim` directory, source environment variables, activate conda environment, and run `simulate_grasp.py`. 25 | 26 | ``` 27 | source setenv.sh 28 | conda activate isaac-sim 29 | cd isaac-sim-python 30 | python simulate_grasp.py -P Isaac/Props/Flip_Stack/large_corner_bracket_physics.usd Isaac/Props/Flip_Stack/screw_95_physics.usd Isaac/Props/Flip_Stack/t_connector_physics.usd -l nucleus_server -p 40 0 5 -a 45 -n 5 -v sim.mp4 31 | ``` 32 | 33 | The code above will simulate grasp execution of Panda arm in a scene with a bin and objects 5 randomly selected objects selected from the collection of usd files given. The specified grasp pose is a planar grasp with grasp position `(40, 0, 5)` and angle `5` degrees. A video of the simulation will be generated and saved as `sim.mp4`. 34 | 35 | ## Additional Resources 36 | - https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/overview.html 37 | - https://docs.omniverse.nvidia.com/py/isaacsim/index.html 38 | -------------------------------------------------------------------------------- /grasp/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/erasromani/isaac-sim-python/f306d6dce278d30d6d75ba8e2cb5d89f8751bab9/grasp/__init__.py -------------------------------------------------------------------------------- /grasp/grasp_sim.py: -------------------------------------------------------------------------------- 1 | import os 2 | import numpy as np 3 | import tempfile 4 | import omni.kit 5 | 6 | from omni.isaac.synthetic_utils import SyntheticDataHelper 7 | 8 | from grasp.utils.isaac_utils import RigidBody 9 | from grasp.grasping_scenarios.grasp_object import GraspObject 10 | from grasp.utils.visualize import screenshot, img2vid 11 | 12 | 13 | default_camera_pose = { 14 | 'position': (142, -127, 56), # position given by (x, y, z) 15 | 'target': (-180, 234, -27) # target given by (x, y , z) 16 | } 17 | 18 | 19 | class GraspSimulator(GraspObject): 20 | """ Defines a grasping simulation scenario 21 | 22 | Scenarios define planar grasp execution in a scene of a Panda arm and various rigid objects 23 | """ 24 | def __init__(self, kit, dc, mp, dt=1/60.0, record=False, record_interval=10): 25 | """ 26 | Initializes grasp simulator 27 | 28 | Args: 29 | kit (omni.isaac.synthetic_utils.scripts.omnikit.OmniKitHelper): helper class for launching OmniKit from a python environment 30 | dc (omni.isaac.motion_planning._motion_planning.MotionPlanning): motion planning interface from RMP extension 31 | mp (omni.isaac.dynamic_control._dynamic_control.DynamicControl): dynamic control interface 32 | dt (float): simulation time step in seconds 33 | record (bool): flag for capturing screenshots throughout simulation for video recording 34 | record_interval (int): frame intervals for capturing screenshots 35 | """ 36 | super().__init__(kit, dc, mp) 37 | self.frame = 0 38 | self.dt = dt 39 | self.record = record 40 | self.record_interval = record_interval 41 | self.tmp_dir = tempfile.mkdtemp() 42 | self.sd_helper = SyntheticDataHelper() 43 | 44 | # create initial scene 45 | self.create_franka() 46 | 47 | # set camera pose 48 | self.set_camera_pose(default_camera_pose['position'], default_camera_pose['target']) 49 | 50 | def execute_grasp(self, position, angle): 51 | """ 52 | Executes a planar grasp with a panda arm. 53 | 54 | Args: 55 | position (list or numpy.darray): grasp position array of length 3 given by [x, y, z] 56 | angle (float): grap angle in degrees 57 | 58 | Returns: 59 | evaluation (enum.EnumMeta): GRASP_eval class containing two states {GRASP_eval.FAILURE, GRAPS_eval.SUCCESS} 60 | """ 61 | self.set_target_angle(angle) 62 | self.set_target_position(position) 63 | self.perform_tasks() 64 | # start simulation 65 | if self._kit.editor.is_playing(): previously_playing = True 66 | else: previously_playing = False 67 | 68 | if self.pick_and_place is not None: 69 | 70 | while True: 71 | self.step(0) 72 | self.update() 73 | if self.pick_and_place.evaluation is not None: 74 | break 75 | evaluation = self.pick_and_place.evaluation 76 | self.stop_tasks() 77 | self.step(0) 78 | self.update() 79 | 80 | # Stop physics simulation 81 | if not previously_playing: self.stop() 82 | 83 | return evaluation 84 | 85 | def wait_for_drop(self, max_steps=2000): 86 | """ 87 | Waits for all objects to drop. 88 | 89 | Args: 90 | max_steps (int): maximum number of timesteps before aborting wait 91 | """ 92 | # start simulation 93 | if self._kit.editor.is_playing(): previously_playing = True 94 | else: previously_playing = False 95 | 96 | if not previously_playing: self.play() 97 | step = 0 98 | while step < max_steps or self._kit.is_loading(): 99 | self.step(step) 100 | self.update() 101 | objects_speed = np.array([o.get_speed() for o in self.objects]) 102 | if np.all(objects_speed == 0): break 103 | step +=1 104 | 105 | # Stop physics simulation 106 | if not previously_playing: self.stop() 107 | 108 | def wait_for_loading(self): 109 | """ 110 | Waits for all scene visuals to load. 111 | """ 112 | while self.is_loading(): 113 | self.update() 114 | 115 | def play(self): 116 | """ 117 | Starts simulation. 118 | """ 119 | self._kit.play() 120 | if not hasattr(self, 'world') or not hasattr(self, 'franka_solid') or not hasattr(self, 'bin_solid') or not hasattr(self, 'pick_and_place'): 121 | self.register_scene() 122 | 123 | def stop(self): 124 | """ 125 | Stops simulation. 126 | """ 127 | self._kit.stop() 128 | 129 | def update(self): 130 | """ 131 | Simulate one time step. 132 | """ 133 | if self.record and self.sd_helper is not None and self.frame % self.record_interval == 0: 134 | 135 | screenshot(self.sd_helper, suffix=self.frame, directory=self.tmp_dir) 136 | 137 | self._kit.update(self.dt) 138 | self.frame += 1 139 | 140 | def is_loading(self): 141 | """ 142 | Determine if all scene visuals are loaded. 143 | 144 | Returns: 145 | (bool): flag for whether or not all scene visuals are loaded 146 | """ 147 | return self._kit.is_loading() 148 | 149 | def set_camera_pose(self, position, target): 150 | """ 151 | Set camera pose. 152 | 153 | Args: 154 | position (list or numpy.darray): camera position array of length 3 given by [x, y, z] 155 | target (list or numpy.darray): target position array of length 3 given by [x, y, z] 156 | """ 157 | self._editor.set_camera_position("/OmniverseKit_Persp", *position, True) 158 | self._editor.set_camera_target("/OmniverseKit_Persp", *target, True) 159 | 160 | def save_video(self, path): 161 | """ 162 | Save video recording of screenshots taken throughout the simulation. 163 | 164 | Args: 165 | path (str): output video filename 166 | """ 167 | framerate = int(round(1.0 / (self.record_interval * self.dt))) 168 | img2vid(os.path.join(self.tmp_dir, '*.png'), path, framerate=framerate) 169 | -------------------------------------------------------------------------------- /grasp/grasping_scenarios/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/erasromani/isaac-sim-python/f306d6dce278d30d6d75ba8e2cb5d89f8751bab9/grasp/grasping_scenarios/__init__.py -------------------------------------------------------------------------------- /grasp/grasping_scenarios/franka.py: -------------------------------------------------------------------------------- 1 | # Credits: The majority of this code is taken from build code associated with nvidia/isaac-sim:2020.2.2_ea with minor modifications. 2 | 3 | import time 4 | import os 5 | import numpy as np 6 | import carb.tokens 7 | import omni.kit.settings 8 | 9 | from pxr import Usd, UsdGeom, Gf 10 | from collections import deque 11 | 12 | from omni.isaac.dynamic_control import _dynamic_control 13 | from omni.isaac.motion_planning import _motion_planning 14 | from omni.isaac.samples.scripts.utils import math_utils 15 | 16 | 17 | # default joint configuration 18 | default_config = (0.00, -1.3, 0.00, -2.87, 0.00, 2.00, 0.75) 19 | 20 | 21 | # Alternative default config for motion planning 22 | alternate_config = [ 23 | (1.5356, -1.3813, -1.5151, -2.0015, -1.3937, 1.5887, 1.4597), 24 | (-1.5356, -1.3813, 1.5151, -2.0015, 1.3937, 1.5887, 0.4314), 25 | ] 26 | 27 | 28 | class Gripper: 29 | """ 30 | Gripper for franka. 31 | """ 32 | def __init__(self, dc, ar): 33 | """ 34 | Initialize gripper. 35 | 36 | Args: 37 | dc (omni.isaac.motion_planning._motion_planning.MotionPlanning): motion planning interface from RMP extension 38 | ar (int): articulation identifier 39 | """ 40 | self.dc = dc 41 | self.ar = ar 42 | self.finger_j1 = self.dc.find_articulation_dof(self.ar, "panda_finger_joint1") 43 | self.finger_j2 = self.dc.find_articulation_dof(self.ar, "panda_finger_joint2") 44 | self.width = 0 45 | self.width_history = deque(maxlen=50) 46 | 47 | def open(self, wait=False): 48 | """ 49 | Open gripper. 50 | """ 51 | if self.width < 0.045: 52 | self.move(0.045, wait=True) 53 | self.move(0.09, wait=wait) 54 | 55 | def close(self, wait=False, force=0): 56 | """ 57 | Close gripper. 58 | """ 59 | self.move(0, wait=wait) 60 | 61 | def move(self, width=0.03, speed=0.2, wait=False): 62 | """ 63 | Modify width. 64 | """ 65 | self.width = width 66 | # if wait: 67 | # time.sleep(0.5) 68 | 69 | def update(self): 70 | """ 71 | Actuate gripper. 72 | """ 73 | self.dc.set_dof_position_target(self.finger_j1, self.width * 0.5 * 100) 74 | self.dc.set_dof_position_target(self.finger_j2, self.width * 0.5 * 100) 75 | self.width_history.append(self.get_width()) 76 | 77 | def get_width(self): 78 | """ 79 | Get current width. 80 | """ 81 | return sum(self.get_position()) 82 | 83 | def get_position(self): 84 | """ 85 | Get left and right finger local position. 86 | """ 87 | return self.dc.get_dof_position(self.finger_j1), self.dc.get_dof_position(self.finger_j2) 88 | 89 | def get_velocity(self, from_articulation=True): 90 | """ 91 | Get left and right finger local velocity. 92 | """ 93 | if from_articulation: 94 | 95 | return (self.dc.get_dof_velocity(self.finger_j1), self.dc.get_dof_velocity(self.finger_j2)) 96 | 97 | else: 98 | 99 | leftfinger_handle = self.dc.get_rigid_body(self.dc.get_articulation_path(self.ar) + '/panda_leftfinger') 100 | rightfinger_handle = self.dc.get_rigid_body(self.dc.get_articulation_path(self.ar) + '/panda_rightfinger') 101 | leftfinger_velocity = np.linalg.norm(np.array(self.dc.get_rigid_body_local_linear_velocity(leftfinger_handle))) 102 | rightfinger_velocity = np.linalg.norm(np.array(self.dc.get_rigid_body_local_linear_velocity(rightfinger_handle))) 103 | return (leftfinger_velocity, rightfinger_velocity) 104 | 105 | def is_moving(self, tol=1e-2): 106 | """ 107 | Determine if gripper fingers are moving 108 | """ 109 | if len(self.width_history) < self.width_history.maxlen or np.array(self.width_history).std() > tol: 110 | return True 111 | else: 112 | return False 113 | 114 | def get_state(self): 115 | """ 116 | Get gripper state. 117 | """ 118 | dof_states = self.dc.get_articulation_dof_states(self.ar, _dynamic_control.STATE_ALL) 119 | return dof_states[-2], dof_states[-1] 120 | 121 | def is_closed(self, tol=1e-2): 122 | """ 123 | Determine if gripper is closed. 124 | """ 125 | if self.get_width() < tol: 126 | return True 127 | else: 128 | return False 129 | 130 | 131 | class Status: 132 | """ 133 | Class that contains status for end effector 134 | """ 135 | def __init__(self, mp, rmp_handle): 136 | """ 137 | Initialize status object. 138 | 139 | Args: 140 | mp (omni.isaac.dynamic_control._dynamic_control.DynamicControl): dynamic control interface 141 | rmp_handle (int): RMP handle identifier 142 | """ 143 | self.mp = mp 144 | self.rmp_handle = rmp_handle 145 | self.orig = np.array([0, 0, 0]) 146 | self.axis_x = np.array([1, 0, 0]) 147 | self.axis_y = np.array([0, 1, 0]) 148 | self.axis_z = np.array([0, 0, 1]) 149 | 150 | self.current_frame = {"orig": self.orig, "axis_x": self.axis_x, "axis_y": self.axis_y, "axis_z": self.axis_z} 151 | self.target_frame = {"orig": self.orig, "axis_x": self.axis_x, "axis_y": self.axis_y, "axis_z": self.axis_z} 152 | self.frame = self.current_frame 153 | 154 | def update(self): 155 | """ 156 | Update end effector state. 157 | """ 158 | state = self.mp.getRMPState(self.rmp_handle) 159 | target = self.mp.getRMPTarget(self.rmp_handle) 160 | self.orig = np.array([state[0].x, state[0].y, state[0].z]) 161 | self.axis_x = np.array([state[1].x, state[1].y, state[1].z]) 162 | self.axis_y = np.array([state[2].x, state[2].y, state[2].z]) 163 | self.axis_z = np.array([state[3].x, state[3].y, state[3].z]) 164 | 165 | self.current_frame = {"orig": self.orig, "axis_x": self.axis_x, "axis_y": self.axis_y, "axis_z": self.axis_z} 166 | self.frame = self.current_frame 167 | self.current_target = { 168 | "orig": np.array([target[0].x, target[0].y, target[0].z]), 169 | "axis_x": np.array([target[1].x, target[1].y, target[1].z]), 170 | "axis_y": np.array([target[2].x, target[2].y, target[2].z]), 171 | "axis_z": np.array([target[3].x, target[3].y, target[3].z]), 172 | } 173 | 174 | 175 | class EndEffector: 176 | """ 177 | End effector object that controls movement. 178 | """ 179 | def __init__(self, dc, mp, ar, rmp_handle): 180 | """ 181 | Initialize end effector. 182 | 183 | Args: 184 | dc (omni.isaac.motion_planning._motion_planning.MotionPlanning): motion planning interface from RMP extension 185 | mp (omni.isaac.dynamic_control._dynamic_control.DynamicControl): dynamic control interface 186 | ar (int): articulation identifier 187 | rmp_handle (int): RMP handle identifier 188 | """ 189 | self.dc = dc 190 | self.ar = ar 191 | self.mp = mp 192 | self.rmp_handle = rmp_handle 193 | self.gripper = Gripper(dc, ar) 194 | self.status = Status(mp, rmp_handle) 195 | self.UpRot = Gf.Rotation(Gf.Vec3d(0, 0, 1), 90) 196 | 197 | def freeze(self): 198 | self.go_local( 199 | orig=self.status.orig, axis_x=self.status.axis_x, axis_z=self.status.axis_z, wait_for_target=False 200 | ) 201 | 202 | def go_local( 203 | self, 204 | target=None, 205 | orig=[], 206 | axis_x=[], 207 | axis_y=[], 208 | axis_z=[], 209 | required_orig_err=0.01, 210 | required_axis_x_err=0.01, 211 | required_axis_y_err=0.01, 212 | required_axis_z_err=0.01, 213 | orig_thresh=None, 214 | axis_x_thresh=None, 215 | axis_y_thresh=None, 216 | axis_z_thresh=None, 217 | approach_direction=[], 218 | approach_standoff=0.1, 219 | approach_standoff_std_dev=0.001, 220 | use_level_surface_orientation=False, 221 | use_target_weight_override=True, 222 | use_default_config=False, 223 | wait_for_target=True, 224 | wait_time=None, 225 | ): 226 | 227 | self.target_weight_override_value = 10000.0 228 | self.target_weight_override_std_dev = 0.03 229 | if orig_thresh: 230 | required_orig_err = orig_thresh 231 | if axis_x_thresh: 232 | required_axis_x_err = axis_x_thresh 233 | if axis_y_thresh: 234 | required_axis_y_err = axis_y_thresh 235 | if axis_z_thresh: 236 | required_axis_z_err = axis_z_thresh 237 | 238 | if target: 239 | orig = target["orig"] 240 | if "axis_x" in target and target["axis_x"] is not None: 241 | axis_x = target["axis_x"] 242 | if "axis_y" in target and target["axis_y"] is not None: 243 | axis_y = target["axis_y"] 244 | if "axis_z" in target and target["axis_z"] is not None: 245 | axis_z = target["axis_z"] 246 | 247 | orig = np.array(orig) 248 | axis_x = np.array(axis_x) 249 | axis_y = np.array(axis_y) 250 | axis_z = np.array(axis_z) 251 | approach = _motion_planning.Approach((0, 0, 1), 0, 0) 252 | 253 | if len(approach_direction) != 0: 254 | approach = _motion_planning.Approach(approach_direction, approach_standoff, approach_standoff_std_dev) 255 | 256 | pose_command = _motion_planning.PartialPoseCommand() 257 | if len(orig) > 0: 258 | pose_command.set(_motion_planning.Command(orig, approach), int(_motion_planning.FrameElement.ORIG)) 259 | if len(axis_x) > 0: 260 | pose_command.set(_motion_planning.Command(axis_x), int(_motion_planning.FrameElement.AXIS_X)) 261 | if len(axis_y) > 0: 262 | pose_command.set(_motion_planning.Command(axis_y), int(_motion_planning.FrameElement.AXIS_Y)) 263 | if len(axis_z) > 0: 264 | pose_command.set(_motion_planning.Command(axis_z), int(_motion_planning.FrameElement.AXIS_Z)) 265 | 266 | self.mp.goLocal(self.rmp_handle, pose_command) 267 | 268 | if wait_for_target and wait_time: 269 | error = 1 270 | future_time = time.time() + wait_time 271 | 272 | while error > required_orig_err and time.time() < future_time: 273 | # time.sleep(0.1) 274 | error = self.mp.getError(self.rmp_handle) 275 | 276 | def look_at(self, gripper_pos, target): 277 | # Y up works for look at but sometimes flips, go_local might be a safer bet with a locked y_axis 278 | orientation = math_utils.lookAt(gripper_pos, target, (0, 1, 0)) 279 | mat = Gf.Matrix3d(orientation).GetTranspose() 280 | 281 | self.go_local( 282 | orig=[gripper_pos[0], gripper_pos[1], gripper_pos[2]], 283 | axis_x=[mat.GetColumn(0)[0], mat.GetColumn(0)[1], mat.GetColumn(0)[2]], 284 | axis_z=[mat.GetColumn(2)[0], mat.GetColumn(2)[1], mat.GetColumn(2)[2]], 285 | ) 286 | 287 | 288 | class Franka: 289 | """ 290 | Franka objects that contains implementation details for robot control. 291 | """ 292 | def __init__(self, stage, prim, dc, mp, world=None, group_path="", default_config=None, is_ghost=False): 293 | """ 294 | Initialize Franka controller. 295 | 296 | Args: 297 | stage (pxr.Usd.Stage): usd stage 298 | prim (pxr.Usd.Prim): robot prim 299 | dc (omni.isaac.motion_planning._motion_planning.MotionPlanning): motion planning interface from RMP extension 300 | mp (omni.isaac.dynamic_control._dynamic_control.DynamicControl): dynamic control interface 301 | world (omni.isaac.samples.scripts.utils.world.World): simulation world handler 302 | default_config (tuple or list): default configuration for robot revolute joint drivers 303 | is_ghost (bool): flag for turning off collision and modifying visuals for robot arm 304 | """ 305 | self.dc = dc 306 | self.mp = mp 307 | self.prim = prim 308 | self.stage = stage 309 | # get handle to the articulation for this franka 310 | self.ar = self.dc.get_articulation(prim.GetPath().pathString) 311 | self.is_ghost = is_ghost 312 | 313 | self.base = self.dc.get_articulation_root_body(self.ar) 314 | 315 | body_count = self.dc.get_articulation_body_count(self.ar) 316 | for bodyIdx in range(body_count): 317 | body = self.dc.get_articulation_body(self.ar, bodyIdx) 318 | self.dc.set_rigid_body_disable_gravity(body, True) 319 | 320 | exec_folder = os.path.abspath( 321 | carb.tokens.get_tokens_interface().resolve( 322 | f"{os.environ['ISAAC_PATH']}/exts/omni.isaac.motion_planning/resources/lula/lula_franka" 323 | ) 324 | ) 325 | 326 | self.rmp_handle = self.mp.registerRmp( 327 | exec_folder + "/urdf/lula_franka_gen.urdf", 328 | exec_folder + "/config/robot_descriptor.yaml", 329 | exec_folder + "/config/franka_rmpflow_common.yaml", 330 | prim.GetPath().pathString, 331 | "right_gripper", 332 | True, 333 | ) 334 | print("franka rmp handle", self.rmp_handle) 335 | if world is not None: 336 | self.world = world 337 | self.world.rmp_handle = self.rmp_handle 338 | self.world.register_parent(self.base, self.prim, "panda_link0") 339 | 340 | settings = omni.kit.settings.get_settings_interface() 341 | self.mp.setFrequency(self.rmp_handle, settings.get("/physics/timeStepsPerSecond"), True) 342 | 343 | self.end_effector = EndEffector(self.dc, self.mp, self.ar, self.rmp_handle) 344 | if default_config: 345 | self.mp.setDefaultConfig(self.rmp_handle, default_config) 346 | self.target_visibility = True 347 | if self.is_ghost: 348 | self.target_visibility = False 349 | 350 | self.imageable = UsdGeom.Imageable(self.prim) 351 | 352 | def __del__(self): 353 | """ 354 | Unregister RMP. 355 | """ 356 | self.mp.unregisterRmp(self.rmp_handle) 357 | print(" Delete Franka") 358 | 359 | def set_pose(self, pos, rot): 360 | """ 361 | Set robot pose. 362 | """ 363 | self._mp.setTargetLocal(self.rmp_handle, pos, rot) 364 | 365 | def set_speed(self, speed_level): 366 | """ 367 | Set robot speed. 368 | """ 369 | pass 370 | 371 | def update(self): 372 | """ 373 | Update robot state. 374 | """ 375 | self.end_effector.gripper.update() 376 | self.end_effector.status.update() 377 | if self.imageable: 378 | if self.target_visibility is not self.imageable.ComputeVisibility(Usd.TimeCode.Default()): 379 | if self.target_visibility: 380 | self.imageable.MakeVisible() 381 | else: 382 | self.imageable.MakeInvisible() 383 | 384 | def send_config(self, config): 385 | """ 386 | Set robot default configuration. 387 | """ 388 | if self.is_ghost is False: 389 | self.mp.setDefaultConfig(self.rmp_handle, config) 390 | -------------------------------------------------------------------------------- /grasp/grasping_scenarios/grasp_object.py: -------------------------------------------------------------------------------- 1 | # Credits: Starter code taken from build code associated with nvidia/isaac-sim:2020.2.2_ea. 2 | 3 | import os 4 | import random 5 | import numpy as np 6 | import glob 7 | import omni 8 | import carb 9 | 10 | from enum import Enum 11 | from collections import deque 12 | from pxr import Gf, UsdGeom 13 | from copy import copy 14 | 15 | from omni.physx.scripts.physicsUtils import add_ground_plane 16 | from omni.isaac.dynamic_control import _dynamic_control 17 | from omni.isaac.utils._isaac_utils import math as math_utils 18 | from omni.isaac.samples.scripts.utils.world import World 19 | from omni.isaac.utils.scripts.nucleus_utils import find_nucleus_server 20 | from omni.physx import _physx 21 | 22 | from grasp.utils.isaac_utils import create_prim_from_usd, RigidBody, set_translate, set_rotate, setup_physics 23 | from grasp.grasping_scenarios.franka import Franka, default_config 24 | from grasp.grasping_scenarios.scenario import Scenario 25 | 26 | 27 | statedic = {0: "orig", 1: "axis_x", 2: "axis_y", 3: "axis_z"} 28 | 29 | 30 | class SM_events(Enum): 31 | """ 32 | State machine events. 33 | """ 34 | START = 0 35 | WAYPOINT_REACHED = 1 36 | GOAL_REACHED = 2 37 | ATTACHED = 3 38 | DETACHED = 4 39 | TIMEOUT = 5 40 | STOP = 6 41 | NONE = 7 # no event ocurred, just clocks 42 | 43 | 44 | class SM_states(Enum): 45 | """ 46 | State machine states. 47 | """ 48 | STANDBY = 0 # Default state, does nothing unless enters with event START 49 | PICKING = 1 50 | ATTACH = 2 51 | HOLDING = 3 52 | GRASPING = 4 53 | LIFTING = 5 54 | 55 | 56 | class GRASP_eval(Enum): 57 | """ 58 | Grasp execution evaluation. 59 | """ 60 | FAILURE = 0 61 | SUCCESS = 1 62 | 63 | 64 | class PickAndPlaceStateMachine(object): 65 | """ 66 | Self-contained state machine class for Robot Behavior. Each machine state may react to different events, 67 | and the handlers are defined as in-class functions. 68 | """ 69 | def __init__(self, stage, robot, ee_prim, default_position): 70 | """ 71 | Initialize state machine. 72 | 73 | Args: 74 | stage (pxr.Usd.Stage): usd stage 75 | robot (grasp.grasping_scenarios.franka.Franka): robot controller object 76 | ee_prim (pxr.Usd.Prim): Panda arm end effector prim 77 | default_position (omni.isaac.dynamic_control._dynamic_control.Transform): default position of Panda arm 78 | """ 79 | self.robot = robot 80 | self.dc = robot.dc 81 | self.end_effector = ee_prim 82 | self.end_effector_handle = None 83 | self._stage = stage 84 | 85 | self.start_time = 0.0 86 | self.start = False 87 | self._time = 0.0 88 | self.default_timeout = 10 89 | self.default_position = copy(default_position) 90 | self.target_position = default_position 91 | self.target_point = default_position.p 92 | self.target_angle = 0 # grasp angle in degrees 93 | self.reset = False 94 | self.evaluation = None 95 | self.waypoints = deque() 96 | self.thresh = {} 97 | # Threshold to clear waypoints/goal 98 | # (any waypoint that is not final will be cleared with the least precision) 99 | self.precision_thresh = [ 100 | [0.0005, 0.0025, 0.0025, 0.0025], 101 | [0.0005, 0.005, 0.005, 0.005], 102 | [0.05, 0.2, 0.2, 0.2], 103 | [0.08, 0.4, 0.4, 0.4], 104 | [0.18, 0.6, 0.6, 0.6], 105 | ] 106 | self.add_object = None 107 | 108 | # Event management variables 109 | 110 | # Used to verify if the goal was reached due to robot moving or it had never left previous target 111 | self._is_moving = False 112 | self._attached = False # Used to flag the Attached/Detached events on a change of state from the end effector 113 | self._detached = False 114 | 115 | self.is_closed = False 116 | self.pick_count = 0 117 | # Define the state machine handling functions 118 | self.sm = {} 119 | # Make empty state machine for all events and states 120 | for s in SM_states: 121 | self.sm[s] = {} 122 | for e in SM_events: 123 | self.sm[s][e] = self._empty 124 | self.thresh[s] = 0 125 | 126 | # Fill in the functions to handle each event for each status 127 | self.sm[SM_states.STANDBY][SM_events.START] = self._standby_start 128 | self.sm[SM_states.STANDBY][SM_events.GOAL_REACHED] = self._standby_goal_reached 129 | self.thresh[SM_states.STANDBY] = 3 130 | 131 | self.sm[SM_states.PICKING][SM_events.GOAL_REACHED] = self._picking_goal_reached 132 | self.thresh[SM_states.PICKING] = 1 133 | 134 | self.sm[SM_states.GRASPING][SM_events.ATTACHED] = self._grasping_attached 135 | 136 | self.sm[SM_states.LIFTING][SM_events.GOAL_REACHED] = self._lifting_goal_reached 137 | 138 | for s in SM_states: 139 | self.sm[s][SM_events.DETACHED] = self._all_detached 140 | self.sm[s][SM_events.TIMEOUT] = self._all_timeout 141 | 142 | self.current_state = SM_states.STANDBY 143 | self.previous_state = -1 144 | self._physxIFace = _physx.acquire_physx_interface() 145 | 146 | # Auxiliary functions 147 | 148 | def _empty(self, *args): 149 | """ 150 | Empty function to use on states that do not react to some specific event. 151 | """ 152 | pass 153 | 154 | def change_state(self, new_state, print_state=True): 155 | """ 156 | Function called every time a event handling changes current state. 157 | """ 158 | self.current_state = new_state 159 | self.start_time = self._time 160 | if print_state: carb.log_warn(str(new_state)) 161 | 162 | def goalReached(self): 163 | """ 164 | Checks if the robot has reached a certain waypoint in the trajectory. 165 | """ 166 | if self._is_moving: 167 | state = self.robot.end_effector.status.current_frame 168 | target = self.robot.end_effector.status.current_target 169 | error = 0 170 | for i in [0, 2, 3]: 171 | k = statedic[i] 172 | state_v = state[k] 173 | target_v = target[k] 174 | error = np.linalg.norm(state_v - target_v) 175 | # General Threshold is the least strict 176 | thresh = self.precision_thresh[-1][i] 177 | if len(self.waypoints) == 0: 178 | thresh = self.precision_thresh[self.thresh[self.current_state]][i] 179 | if error > thresh: 180 | return False 181 | self._is_moving = False 182 | return True 183 | return False 184 | 185 | def get_current_state_tr(self): 186 | """ 187 | Gets current End Effector Transform, converted from Motion position and Rotation matrix. 188 | """ 189 | # Gets end effector frame 190 | state = self.robot.end_effector.status.current_frame 191 | 192 | orig = state["orig"] * 100.0 193 | 194 | mat = Gf.Matrix3f( 195 | *state["axis_x"].astype(float), *state["axis_y"].astype(float), *state["axis_z"].astype(float) 196 | ) 197 | q = mat.ExtractRotation().GetQuaternion() 198 | (q_x, q_y, q_z) = q.GetImaginary() 199 | q = [q_x, q_y, q_z, q.GetReal()] 200 | tr = _dynamic_control.Transform() 201 | tr.p = list(orig) 202 | tr.r = q 203 | return tr 204 | 205 | def lerp_to_pose(self, pose, n_waypoints=1): 206 | """ 207 | adds spherical linear interpolated waypoints from last pose in the waypoint list to the provided pose 208 | if the waypoit list is empty, use current pose. 209 | """ 210 | if len(self.waypoints) == 0: 211 | start = self.get_current_state_tr() 212 | start.p = math_utils.mul(start.p, 0.01) 213 | else: 214 | start = self.waypoints[-1] 215 | 216 | if n_waypoints > 1: 217 | for i in range(n_waypoints): 218 | self.waypoints.append(math_utils.slerp(start, pose, (i + 1.0) / n_waypoints)) 219 | else: 220 | self.waypoints.append(pose) 221 | 222 | def move_to_zero(self): 223 | self._is_moving = False 224 | self.robot.end_effector.go_local( 225 | orig=[], axis_x=[], axis_y=[], axis_z=[], use_default_config=True, wait_for_target=False, wait_time=5.0 226 | ) 227 | 228 | def move_to_target(self): 229 | """ 230 | Move arm towards target with RMP controller. 231 | """ 232 | xform_attr = self.target_position 233 | self._is_moving = True 234 | 235 | orig = np.array([xform_attr.p.x, xform_attr.p.y, xform_attr.p.z]) 236 | axis_y = np.array(math_utils.get_basis_vector_y(xform_attr.r)) 237 | axis_z = np.array(math_utils.get_basis_vector_z(xform_attr.r)) 238 | self.robot.end_effector.go_local( 239 | orig=orig, 240 | axis_x=[], 241 | axis_y=axis_y, 242 | axis_z=axis_z, 243 | use_default_config=True, 244 | wait_for_target=False, 245 | wait_time=5.0, 246 | ) 247 | 248 | def get_target_orientation(self): 249 | """ 250 | Gets target gripper orientation given target angle and a plannar grasp. 251 | """ 252 | angle = self.target_angle * np.pi / 180 253 | mat = Gf.Matrix3f( 254 | -np.cos(angle), -np.sin(angle), 0, -np.sin(angle), np.cos(angle), 0, 0, 0, -1 255 | ) 256 | q = mat.ExtractRotation().GetQuaternion() 257 | (q_x, q_y, q_z) = q.GetImaginary() 258 | q = [q_x, q_y, q_z, q.GetReal()] 259 | return q 260 | 261 | def get_target_to_point(self, offset_position=[]): 262 | """ 263 | Get target Panda arm pose from target position and angle. 264 | """ 265 | offset = _dynamic_control.Transform() 266 | if offset_position: 267 | 268 | offset.p.x = offset_position[0] 269 | offset.p.y = offset_position[1] 270 | offset.p.z = offset_position[2] 271 | 272 | target_pose = _dynamic_control.Transform() 273 | target_pose.p = self.target_point 274 | target_pose.r = self.get_target_orientation() 275 | target_pose = math_utils.mul(target_pose, offset) 276 | target_pose.p = math_utils.mul(target_pose.p, 0.01) 277 | return target_pose 278 | 279 | def set_target_to_point(self, offset_position=[], n_waypoints=1, clear_waypoints=True): 280 | """ 281 | Clears waypoints list, and sets a new waypoint list towards the a given point in space. 282 | """ 283 | target_position = self.get_target_to_point(offset_position=offset_position) 284 | # linear interpolate to target pose 285 | if clear_waypoints: 286 | self.waypoints.clear() 287 | self.lerp_to_pose(target_position, n_waypoints=n_waypoints) 288 | # Get first waypoint target 289 | self.target_position = self.waypoints.popleft() 290 | 291 | def step(self, timestamp, start=False, reset=False): 292 | """ 293 | Steps the State machine, handling which event to call. 294 | """ 295 | if self.current_state != self.previous_state: 296 | self.previous_state = self.current_state 297 | if not self.start: 298 | self.start = start 299 | 300 | if self.current_state in [SM_states.GRASPING, SM_states.LIFTING]: 301 | # object grasped 302 | if not self.robot.end_effector.gripper.is_closed(1e-1) and not self.robot.end_effector.gripper.is_moving(1e-2): 303 | self._attached = True 304 | # self.is_closed = False 305 | # object not grasped 306 | elif self.robot.end_effector.gripper.is_closed(1e-1): 307 | self._detached = True 308 | self.is_closed = True 309 | 310 | # Process events 311 | if reset: 312 | # reset to default pose, clear waypoints, and re-initialize event handlers 313 | self.current_state = SM_states.STANDBY 314 | self.previous_state = -1 315 | self.robot.end_effector.gripper.open() 316 | self.evaluation = None 317 | self.start = False 318 | self._time = 0 319 | self.start_time = self._time 320 | self.pick_count = 0 321 | self.waypoints.clear() 322 | self._detached = False 323 | self._attached = False 324 | self.target_position = self.default_position 325 | self.move_to_target() 326 | elif self._detached: 327 | self._detached = False 328 | self.sm[self.current_state][SM_events.DETACHED]() 329 | elif self.goalReached(): 330 | if len(self.waypoints) == 0: 331 | self.sm[self.current_state][SM_events.GOAL_REACHED]() 332 | else: 333 | self.target_position = self.waypoints.popleft() 334 | self.move_to_target() 335 | # self.start_time = self._time 336 | elif self.current_state == SM_states.STANDBY and self.start: 337 | self.sm[self.current_state][SM_events.START]() 338 | elif self._attached: 339 | self._attached = False 340 | self.sm[self.current_state][SM_events.ATTACHED]() 341 | elif self._time - self.start_time > self.default_timeout: 342 | self.sm[self.current_state][SM_events.TIMEOUT]() 343 | else: 344 | self.sm[self.current_state][SM_events.NONE]() 345 | self._time += 1.0 / 60.0 346 | 347 | # Event handling functions. Each state has its own event handler function depending on which event happened 348 | 349 | def _standby_start(self, *args): 350 | """ 351 | Handles the start event when in standby mode. 352 | Proceeds to move towards target grasp pose. 353 | """ 354 | # Tell motion planner controller to ignore current object as an obstacle 355 | self.pick_count = 0 356 | self.evaluation = None 357 | self.lerp_to_pose(self.default_position, 1) 358 | self.lerp_to_pose(self.default_position, 60) 359 | self.robot.end_effector.gripper.open() 360 | # set target above the current bin with offset of 10 cm 361 | self.set_target_to_point(offset_position=[0.0, 0.0, -10.0], n_waypoints=90, clear_waypoints=False) 362 | # pause before lowering to target object 363 | self.lerp_to_pose(self.waypoints[-1], 180) 364 | self.set_target_to_point(n_waypoints=90, clear_waypoints=False) 365 | # start arm movement 366 | self.move_to_target() 367 | # Move to next state 368 | self.change_state(SM_states.PICKING) 369 | 370 | # NOTE: As is, this method is never executed 371 | def _standby_goal_reached(self, *args): 372 | """ 373 | Reset grasp execution. 374 | """ 375 | self.move_to_zero() 376 | self.start = True 377 | 378 | def _picking_goal_reached(self, *args): 379 | """ 380 | Grap pose reached, close gripper. 381 | """ 382 | self.robot.end_effector.gripper.close() 383 | self.is_closed = True 384 | # Move to next state 385 | self.move_to_target() 386 | self.robot.end_effector.gripper.width_history.clear() 387 | self.change_state(SM_states.GRASPING) 388 | 389 | def _grasping_attached(self, *args): 390 | """ 391 | Object grasped, lift arm. 392 | """ 393 | self.waypoints.clear() 394 | offset = _dynamic_control.Transform() 395 | offset.p.z = -10 396 | target_pose = math_utils.mul(self.get_current_state_tr(), offset) 397 | target_pose.p = math_utils.mul(target_pose.p, 0.01) 398 | self.lerp_to_pose(target_pose, n_waypoints=60) 399 | self.lerp_to_pose(target_pose, n_waypoints=120) 400 | # Move to next state 401 | self.move_to_target() 402 | self.robot.end_effector.gripper.width_history.clear() 403 | self.change_state(SM_states.LIFTING) 404 | 405 | def _lifting_goal_reached(self, *args): 406 | """ 407 | Finished executing grasp successfully, resets for next grasp execution. 408 | """ 409 | self.is_closed = False 410 | self.robot.end_effector.gripper.open() 411 | self._all_detached() 412 | self.pick_count += 1 413 | self.evaluation = GRASP_eval.SUCCESS 414 | carb.log_warn(str(GRASP_eval.SUCCESS)) 415 | 416 | def _all_timeout(self, *args): 417 | """ 418 | Timeout reached and reset. 419 | """ 420 | self.change_state(SM_states.STANDBY, print_state=False) 421 | self.robot.end_effector.gripper.open() 422 | self.start = False 423 | self.waypoints.clear() 424 | self.target_position = self.default_position 425 | self.lerp_to_pose(self.default_position, 1) 426 | self.lerp_to_pose(self.default_position, 10) 427 | self.lerp_to_pose(self.default_position, 60) 428 | self.move_to_target() 429 | self.evaluation = GRASP_eval.FAILURE 430 | carb.log_warn(str(GRASP_eval.FAILURE)) 431 | 432 | def _all_detached(self, *args): 433 | """ 434 | Object detached and reset. 435 | """ 436 | self.change_state(SM_states.STANDBY, print_state=False) 437 | self.start = False 438 | self.waypoints.clear() 439 | self.lerp_to_pose(self.target_position, 60) 440 | self.lerp_to_pose(self.default_position, 10) 441 | self.lerp_to_pose(self.default_position, 60) 442 | self.move_to_target() 443 | self.evaluation = GRASP_eval.FAILURE 444 | carb.log_warn(str(GRASP_eval.FAILURE)) 445 | 446 | 447 | class GraspObject(Scenario): 448 | """ Defines an obstacle avoidance scenario 449 | 450 | Scenarios define the life cycle within kit and handle init, startup, shutdown etc. 451 | """ 452 | def __init__(self, kit, dc, mp): 453 | """ 454 | Initialize scenario. 455 | 456 | Args: 457 | kit (omni.isaac.synthetic_utils.scripts.omnikit.OmniKitHelper): helper class for launching OmniKit from a python environment 458 | dc (omni.isaac.motion_planning._motion_planning.MotionPlanning): motion planning interface from RMP extension 459 | mp (omni.isaac.dynamic_control._dynamic_control.DynamicControl): dynamic control interface 460 | """ 461 | super().__init__(kit.editor, dc, mp) 462 | self._kit = kit 463 | self._paused = True 464 | self._start = False 465 | self._reset = False 466 | self._time = 0 467 | self._start_time = 0 468 | self.current_state = SM_states.STANDBY 469 | self.timeout_max = 8.0 470 | self.pick_and_place = None 471 | self._pending_stop = False 472 | self._gripper_open = False 473 | 474 | self.current_obj = 0 475 | self.max_objs = 100 476 | self.num_objs = 3 477 | 478 | self.add_objects_timeout = -1 479 | self.franka_solid = None 480 | 481 | result, nucleus_server = find_nucleus_server() 482 | if result is False: 483 | carb.log_error("Could not find nucleus server with /Isaac folder") 484 | else: 485 | self.nucleus_server = nucleus_server 486 | 487 | def __del__(self): 488 | """ 489 | Cleanup scenario objects when deleted, force garbage collection. 490 | """ 491 | if self.franka_solid: 492 | self.franka_solid.end_effector.gripper = None 493 | super().__del__() 494 | 495 | def add_object_path(self, object_path, from_server=False): 496 | """ 497 | Add object usd path. 498 | """ 499 | if from_server and hasattr(self, 'nucleus_server'): 500 | object_path = os.path.join(self.nucleus_server, object_path) 501 | if not from_server and os.path.isdir(object_path): objects_usd = glob.glob(os.path.join(object_path, '**/*.usd'), recursive=True) 502 | else: object_usd = [object_path] 503 | if hasattr(self, 'objects_usd'): 504 | self.objects_usd.extend(object_usd) 505 | else: 506 | self.objects_usd = object_usd 507 | 508 | def create_franka(self, *args): 509 | """ 510 | Create franka USD objects and bin USD objects. 511 | """ 512 | super().create_franka() 513 | if self.asset_path is None: 514 | return 515 | 516 | # Load robot environment and set its transform 517 | self.env_path = "/scene" 518 | robot_usd = self.asset_path + "/Robots/Franka/franka.usd" 519 | robot_path = "/scene/robot" 520 | create_prim_from_usd(self._stage, robot_path, robot_usd, Gf.Vec3d(0, 0, 0)) 521 | 522 | bin_usd = self.asset_path + "/Props/KLT_Bin/large_KLT.usd" 523 | bin_path = "/scene/bin" 524 | create_prim_from_usd(self._stage, bin_path, bin_usd, Gf.Vec3d(40, 0, 4)) 525 | 526 | # Set robot end effector Target 527 | target_path = "/scene/target" 528 | if self._stage.GetPrimAtPath(target_path): 529 | return 530 | 531 | GoalPrim = self._stage.DefinePrim(target_path, "Xform") 532 | self.default_position = _dynamic_control.Transform() 533 | self.default_position.p = [0.4, 0.0, 0.3] 534 | self.default_position.r = [0.0, 1.0, 0.0, 0.0] #TODO: Check values for stability 535 | p = self.default_position.p 536 | r = self.default_position.r 537 | set_translate(GoalPrim, Gf.Vec3d(p.x * 100, p.y * 100, p.z * 100)) 538 | set_rotate(GoalPrim, Gf.Matrix3d(Gf.Quatd(r.w, r.x, r.y, r.z))) 539 | 540 | # Setup physics simulation 541 | add_ground_plane(self._stage, "/groundPlane", "Z", 1000.0, Gf.Vec3f(0.0), Gf.Vec3f(1.0)) 542 | setup_physics(self._stage) 543 | 544 | def rand_position(self, bound, margin=0, z_range=None): 545 | """ 546 | Obtain random position contained within a specified bound. 547 | """ 548 | x_range = (bound[0][0] * (1 - margin), bound[1][0] * (1 - margin)) 549 | y_range = (bound[0][1] * (1 - margin), bound[1][1] * (1 - margin)) 550 | if z_range is None: 551 | z_range = (bound[0][2] * (1 - margin), bound[1][2] * (1 - margin)) 552 | x = np.random.uniform(*x_range) 553 | y = np.random.uniform(*y_range) 554 | z = np.random.uniform(*z_range) 555 | return Gf.Vec3d(x, y, z) 556 | 557 | # combine add_object and add_and_register_object 558 | def add_object(self, *args, register=True, position=None): 559 | """ 560 | Add object to scene. 561 | """ 562 | prim = self.create_new_objects(position=position) 563 | if not register: 564 | return prim 565 | 566 | self._kit.update() 567 | if not hasattr(self, 'objects'): 568 | self.objects = [] 569 | self.objects.append(RigidBody(prim, self._dc)) 570 | 571 | def create_new_objects(self, *args, position=None): 572 | """ 573 | Randomly select and create prim of object in scene. 574 | """ 575 | if not hasattr(self, 'objects_usd'): 576 | return 577 | prim_usd_path = self.objects_usd[random.randint(0, len(self.objects_usd) - 1)] 578 | prim_env_path = "/scene/objects/object_{}".format(self.current_obj) 579 | if position is None: 580 | position = self.rand_position(self.bin_solid.get_bound(), margin=0.2, z_range=(10, 10)) 581 | prim = create_prim_from_usd(self._stage, prim_env_path, prim_usd_path, position) 582 | if hasattr(self, 'current_obj'): self.current_obj += 1 583 | else: self.current_obj = 0 584 | return prim 585 | 586 | def register_objects(self, *args): 587 | """ 588 | Register all objects. 589 | """ 590 | self.objects = [] 591 | objects_path = '/scene/objects' 592 | objects_prim = self._stage.GetPrimAtPath(objects_path) 593 | if objects_prim.IsValid(): 594 | for object_prim in objects_prim.GetChildren(): 595 | self.objects.append(RigidBody(object_prim, self._dc)) 596 | 597 | # TODO: Delete method 598 | def add_and_register_object(self, *args): 599 | prim = self.create_new_objects() 600 | self._kit.update() 601 | if not hasattr(self, 'objects'): 602 | self.objects = [] 603 | self.objects.append(RigidBody(prim, self._dc)) 604 | 605 | def register_scene(self, *args): 606 | """ 607 | Register world, panda arm, bin, and objects. 608 | """ 609 | self.world = World(self._dc, self._mp) 610 | self.register_assets(args) 611 | self.register_objects(args) 612 | 613 | def register_assets(self, *args): 614 | """ 615 | Connect franka controller to usd assets. 616 | """ 617 | # register robot with RMP 618 | robot_path = "/scene/robot" 619 | self.franka_solid = Franka( 620 | self._stage, self._stage.GetPrimAtPath(robot_path), self._dc, self._mp, self.world, default_config 621 | ) 622 | 623 | # register bin 624 | bin_path = "/scene/bin" 625 | bin_prim = self._stage.GetPrimAtPath(bin_path) 626 | self.bin_solid = RigidBody(bin_prim, self._dc) 627 | 628 | # register stage machine 629 | self.pick_and_place = PickAndPlaceStateMachine( 630 | self._stage, 631 | self.franka_solid, 632 | self._stage.GetPrimAtPath("/scene/robot/panda_hand"), 633 | self.default_position, 634 | ) 635 | 636 | def perform_tasks(self, *args): 637 | """ 638 | Perform all tasks in scenario if multiple robots are present. 639 | """ 640 | self._start = True 641 | self._paused = False 642 | return False 643 | 644 | def step(self, step): 645 | """ 646 | Step the scenario, can be used to update things in the scenario per frame. 647 | """ 648 | if self._editor.is_playing(): 649 | if self._pending_stop: 650 | self.stop_tasks() 651 | return 652 | # Updates current references and locations for the robot. 653 | self.world.update() 654 | self.franka_solid.update() 655 | 656 | target = self._stage.GetPrimAtPath("/scene/target") 657 | xform_attr = target.GetAttribute("xformOp:transform") 658 | if self._reset: 659 | self._paused = False 660 | if not self._paused: 661 | self._time += 1.0 / 60.0 662 | self.pick_and_place.step(self._time, self._start, self._reset) 663 | if self._reset: 664 | self._paused = True 665 | self._time = 0 666 | self._start_time = 0 667 | p = self.default_position.p 668 | r = self.default_position.r 669 | set_translate(target, Gf.Vec3d(p.x * 100, p.y * 100, p.z * 100)) 670 | set_rotate(target, Gf.Matrix3d(Gf.Quatd(r.w, r.x, r.y, r.z))) 671 | else: 672 | state = self.franka_solid.end_effector.status.current_target 673 | state_1 = self.pick_and_place.target_position 674 | tr = state["orig"] * 100.0 675 | set_translate(target, Gf.Vec3d(tr[0], tr[1], tr[2])) 676 | set_rotate(target, Gf.Matrix3d(Gf.Quatd(state_1.r.w, state_1.r.x, state_1.r.y, state_1.r.z))) 677 | self._start = False 678 | self._reset = False 679 | if self.add_objects_timeout > 0: 680 | self.add_objects_timeout -= 1 681 | if self.add_objects_timeout == 0: 682 | self.create_new_objects() 683 | else: 684 | translate_attr = xform_attr.Get().GetRow3(3) 685 | rotate_x = xform_attr.Get().GetRow3(0) 686 | rotate_y = xform_attr.Get().GetRow3(1) 687 | rotate_z = xform_attr.Get().GetRow3(2) 688 | 689 | orig = np.array(translate_attr) / 100.0 690 | axis_x = np.array(rotate_x) 691 | axis_y = np.array(rotate_y) 692 | axis_z = np.array(rotate_z) 693 | self.franka_solid.end_effector.go_local( 694 | orig=orig, 695 | axis_x=axis_x, # TODO: consider setting this to [] for stability reasons 696 | axis_y=axis_y, 697 | axis_z=axis_z, 698 | use_default_config=True, 699 | wait_for_target=False, 700 | wait_time=5.0, 701 | ) 702 | 703 | def stop_tasks(self, *args): 704 | """ 705 | Stop tasks in the scenario if any. 706 | """ 707 | if self.pick_and_place is not None: 708 | if self._editor.is_playing(): 709 | self._reset = True 710 | self._pending_stop = False 711 | else: 712 | self._pending_stop = True 713 | 714 | def pause_tasks(self, *args): 715 | """ 716 | Pause tasks in the scenario. 717 | """ 718 | self._paused = not self._paused 719 | return self._paused 720 | 721 | # TODO: use gripper.width == 0 as a proxy for _gripper_open == False 722 | def actuate_gripper(self): 723 | """ 724 | Actuate Panda gripper. 725 | """ 726 | if self._gripper_open: 727 | self.franka_solid.end_effector.gripper.close() 728 | self._gripper_open = False 729 | else: 730 | self.franka_solid.end_effector.gripper.open() 731 | self._gripper_open = True 732 | 733 | def set_target_angle(self, angle): 734 | """ 735 | Set grasp angle in degrees. 736 | """ 737 | if self.pick_and_place is not None: 738 | self.pick_and_place.target_angle = angle 739 | 740 | def set_target_position(self, position): 741 | """ 742 | Set grasp position. 743 | """ 744 | if self.pick_and_place is not None: 745 | self.pick_and_place.target_point = position 746 | -------------------------------------------------------------------------------- /grasp/grasping_scenarios/scenario.py: -------------------------------------------------------------------------------- 1 | # Credits: The majority of this code is taken from build code associated with nvidia/isaac-sim:2020.2.2_ea with minor modifications. 2 | 3 | import gc 4 | import carb 5 | import omni.usd 6 | 7 | from omni.isaac.utils.scripts.nucleus_utils import find_nucleus_server 8 | from grasp.utils.isaac_utils import set_up_z_axis 9 | 10 | 11 | class Scenario: 12 | """ 13 | Defines a block stacking scenario. 14 | 15 | Scenarios define the life cycle within kit and handle init, startup, shutdown etc. 16 | """ 17 | 18 | def __init__(self, editor, dc, mp): 19 | """ 20 | Initialize scenario. 21 | 22 | Args: 23 | editor (omni.kit.editor._editor.IEditor): editor object from isaac-sim simulation 24 | dc (omni.isaac.motion_planning._motion_planning.MotionPlanning): motion planning interface from RMP extension 25 | mp (omni.isaac.dynamic_control._dynamic_control.DynamicControl): dynamic control interface 26 | """ 27 | self._editor = editor # Reference to the Kit editor 28 | self._stage = omni.usd.get_context().get_stage() # Reference to the current USD stage 29 | self._dc = dc # Reference to the dynamic control plugin 30 | self._mp = mp # Reference to the motion planning plugin 31 | self._domains = [] # Contains instances of environment 32 | self._obstacles = [] # Containts references to any obstacles in the scenario 33 | self._executor = None # Contains the thread pool used to run tasks 34 | self._created = False # Is the robot created or not 35 | self._running = False # Is the task running or not 36 | 37 | def __del__(self): 38 | """ 39 | Cleanup scenario objects when deleted, force garbage collection. 40 | """ 41 | self.robot_created = False 42 | self._domains = [] 43 | self._obstacles = [] 44 | self._executor = None 45 | gc.collect() 46 | 47 | def reset_blocks(self, *args): 48 | """ 49 | Funtion called when block poses are reset. 50 | """ 51 | pass 52 | 53 | def stop_tasks(self, *args): 54 | """ 55 | Stop tasks in the scenario if any. 56 | """ 57 | self._running = False 58 | pass 59 | 60 | def step(self, step): 61 | """ 62 | Step the scenario, can be used to update things in the scenario per frame. 63 | """ 64 | pass 65 | 66 | def create_franka(self, *args): 67 | """ 68 | Create franka USD objects. 69 | """ 70 | result, nucleus_server = find_nucleus_server() 71 | if result is False: 72 | carb.log_error("Could not find nucleus server with /Isaac folder") 73 | return 74 | self.asset_path = nucleus_server + "/Isaac" 75 | 76 | # USD paths loaded by scenarios 77 | 78 | self.franka_table_usd = self.asset_path + "/Samples/Leonardo/Stage/franka_block_stacking.usd" 79 | self.franka_ghost_usd = self.asset_path + "/Samples/Leonardo/Robots/franka_ghost.usd" 80 | self.background_usd = self.asset_path + "/Environments/Grid/gridroom_curved.usd" 81 | self.rubiks_cube_usd = self.asset_path + "/Props/Rubiks_Cube/rubiks_cube.usd" 82 | self.red_cube_usd = self.asset_path + "/Props/Blocks/red_block.usd" 83 | self.yellow_cube_usd = self.asset_path + "/Props/Blocks/yellow_block.usd" 84 | self.green_cube_usd = self.asset_path + "/Props/Blocks/green_block.usd" 85 | self.blue_cube_usd = self.asset_path + "/Props/Blocks/blue_block.usd" 86 | 87 | self._created = True 88 | self._stage = omni.usd.get_context().get_stage() 89 | set_up_z_axis(self._stage) 90 | self.stop_tasks() 91 | pass 92 | 93 | def register_assets(self, *args): 94 | """ 95 | Connect franka controller to usd assets 96 | """ 97 | pass 98 | 99 | def task(self, domain): 100 | """ 101 | Task to be performed for a given robot. 102 | """ 103 | pass 104 | 105 | def perform_tasks(self, *args): 106 | """ 107 | Perform all tasks in scenario if multiple robots are present. 108 | """ 109 | self._running = True 110 | pass 111 | 112 | def is_created(self): 113 | """ 114 | Return if the franka was already created. 115 | """ 116 | return self._created 117 | -------------------------------------------------------------------------------- /grasp/utils/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/erasromani/isaac-sim-python/f306d6dce278d30d6d75ba8e2cb5d89f8751bab9/grasp/utils/__init__.py -------------------------------------------------------------------------------- /grasp/utils/isaac_utils.py: -------------------------------------------------------------------------------- 1 | # Credits: All code except class RigidBody and Camera is taken from build code associated with nvidia/isaac-sim:2020.2.2_ea. 2 | 3 | import numpy as np 4 | import omni.kit 5 | 6 | from pxr import Usd, UsdGeom, Gf, PhysicsSchema, PhysxSchema 7 | 8 | 9 | def create_prim_from_usd(stage, prim_env_path, prim_usd_path, location): 10 | """ 11 | Create prim from usd. 12 | """ 13 | envPrim = stage.DefinePrim(prim_env_path, "Xform") # create an empty Xform at the given path 14 | envPrim.GetReferences().AddReference(prim_usd_path) # attach the USD to the given path 15 | set_translate(envPrim, location) # set pose 16 | return stage.GetPrimAtPath(envPrim.GetPath().pathString) 17 | 18 | 19 | def set_up_z_axis(stage): 20 | """ 21 | Utility function to specify the stage with the z axis as "up". 22 | """ 23 | rootLayer = stage.GetRootLayer() 24 | rootLayer.SetPermissionToEdit(True) 25 | with Usd.EditContext(stage, rootLayer): 26 | UsdGeom.SetStageUpAxis(stage, UsdGeom.Tokens.z) 27 | 28 | 29 | def set_translate(prim, new_loc): 30 | """ 31 | Specify position of a given prim, reuse any existing transform ops when possible. 32 | """ 33 | properties = prim.GetPropertyNames() 34 | if "xformOp:translate" in properties: 35 | translate_attr = prim.GetAttribute("xformOp:translate") 36 | translate_attr.Set(new_loc) 37 | elif "xformOp:translation" in properties: 38 | translation_attr = prim.GetAttribute("xformOp:translate") 39 | translation_attr.Set(new_loc) 40 | elif "xformOp:transform" in properties: 41 | transform_attr = prim.GetAttribute("xformOp:transform") 42 | matrix = prim.GetAttribute("xformOp:transform").Get() 43 | matrix.SetTranslateOnly(new_loc) 44 | transform_attr.Set(matrix) 45 | else: 46 | xform = UsdGeom.Xformable(prim) 47 | xform_op = xform.AddXformOp(UsdGeom.XformOp.TypeTransform, UsdGeom.XformOp.PrecisionDouble, "") 48 | xform_op.Set(Gf.Matrix4d().SetTranslate(new_loc)) 49 | 50 | 51 | def set_rotate(prim, rot_mat): 52 | """ 53 | Specify orientation of a given prim, reuse any existing transform ops when possible. 54 | """ 55 | properties = prim.GetPropertyNames() 56 | if "xformOp:rotate" in properties: 57 | rotate_attr = prim.GetAttribute("xformOp:rotate") 58 | rotate_attr.Set(rot_mat) 59 | elif "xformOp:transform" in properties: 60 | transform_attr = prim.GetAttribute("xformOp:transform") 61 | matrix = prim.GetAttribute("xformOp:transform").Get() 62 | matrix.SetRotateOnly(rot_mat.ExtractRotation()) 63 | transform_attr.Set(matrix) 64 | else: 65 | xform = UsdGeom.Xformable(prim) 66 | xform_op = xform.AddXformOp(UsdGeom.XformOp.TypeTransform, UsdGeom.XformOp.PrecisionDouble, "") 67 | xform_op.Set(Gf.Matrix4d().SetRotate(rot_mat)) 68 | 69 | 70 | def create_background(stage, background_stage): 71 | """ 72 | Create background stage. 73 | """ 74 | background_path = "/background" 75 | if not stage.GetPrimAtPath(background_path): 76 | backPrim = stage.DefinePrim(background_path, "Xform") 77 | backPrim.GetReferences().AddReference(background_stage) 78 | # Move the stage down -104cm so that the floor is below the table wheels, move in y axis to get light closer 79 | set_translate(backPrim, Gf.Vec3d(0, -400, -104)) 80 | 81 | 82 | def setup_physics(stage): 83 | """ 84 | Set default physics parameters. 85 | """ 86 | # Specify gravity 87 | metersPerUnit = UsdGeom.GetStageMetersPerUnit(stage) 88 | gravityScale = 9.81 / metersPerUnit 89 | gravity = Gf.Vec3f(0.0, 0.0, -gravityScale) 90 | scene = PhysicsSchema.PhysicsScene.Define(stage, "/physics/scene") 91 | scene.CreateGravityAttr().Set(gravity) 92 | 93 | PhysxSchema.PhysxSceneAPI.Apply(stage.GetPrimAtPath("/physics/scene")) 94 | physxSceneAPI = PhysxSchema.PhysxSceneAPI.Get(stage, "/physics/scene") 95 | physxSceneAPI.CreatePhysxSceneEnableCCDAttr(True) 96 | physxSceneAPI.CreatePhysxSceneEnableStabilizationAttr(True) 97 | physxSceneAPI.CreatePhysxSceneEnableGPUDynamicsAttr(False) 98 | physxSceneAPI.CreatePhysxSceneBroadphaseTypeAttr("MBP") 99 | physxSceneAPI.CreatePhysxSceneSolverTypeAttr("TGS") 100 | 101 | 102 | class Camera: 103 | """ 104 | Camera object that contain state information for a camera in the scene. 105 | """ 106 | def __init__(self, camera_path, translation, rotation): 107 | """ 108 | Initializes the Camera object. 109 | 110 | Args: 111 | camera_path (str): path of camera in stage hierarchy 112 | translation (list or tuple): camera position 113 | rotation (list or tuple): camera orientation described by euler angles in degrees 114 | """ 115 | self.prim = self._kit.create_prim( 116 | camera_path, 117 | "Camera", 118 | translation=translation, 119 | rotation=rotatation, 120 | ) 121 | self.name = self.prim.GetPrimPath().name 122 | self.vpi = omni.kit.viewport.get_viewport_interface 123 | 124 | def set_translate(self, position): 125 | """ 126 | Set camera position. 127 | 128 | Args: 129 | position (tuple): camera position specified by (X, Y, Z) 130 | """ 131 | if not isinstance(position, tuple): position = tuple(position) 132 | translate_attr = self.prim.GetAttribute("xformOp:translate") 133 | translate_attr.Set(position) 134 | 135 | def set_rotate(self, rotation): 136 | """ 137 | Set camera position. 138 | 139 | Args: 140 | rotation (tuple): camera orientation specified by three euler angles in degrees 141 | """ 142 | if not isinstance(rotation, tuple): rotation = tuple(rotation) 143 | rotate_attr = self.prim.GetAttribute("xformOp:rotateZYX") 144 | rotate_attr.Set(rotation) 145 | 146 | def activate(self): 147 | """ 148 | Activate camera to viewport. 149 | """ 150 | self.vpi.get_viewport_window().set_active_camera(str(self.prim.GetPath())) 151 | 152 | def __repr__(self): 153 | return self.name 154 | 155 | 156 | class Camera: 157 | """ 158 | Camera object that contain state information for a camera in the scene. 159 | """ 160 | def __init__(self, camera_path, translation, rotation): 161 | """ 162 | Initializes the Camera object. 163 | 164 | Args: 165 | camera_path (str): path of camera in stage hierarchy 166 | translation (list or tuple): camera position 167 | rotation (list or tuple): camera orientation described by euler angles in degrees 168 | """ 169 | self.prim = self._kit.create_prim( 170 | camera_path, 171 | "Camera", 172 | translation=translation, 173 | rotation=rotation, 174 | ) 175 | self.name = self.prim.GetPrimPath().name 176 | self.vpi = omni.kit.viewport.get_viewport_interface 177 | 178 | def set_translate(self, position): 179 | """ 180 | Set camera position. 181 | 182 | Args: 183 | position (tuple): camera position specified by (X, Y, Z) 184 | """ 185 | if not isinstance(position, tuple): position = tuple(position) 186 | translate_attr = self.prim.GetAttribute("xformOp:translate") 187 | translate_attr.Set(position) 188 | 189 | def set_rotate(self, rotation): 190 | """ 191 | Set camera position. 192 | 193 | Args: 194 | rotation (tuple): camera orientation specified by three euler angles in degrees 195 | """ 196 | if not isinstance(rotation, tuple): rotation = tuple(rotation) 197 | rotate_attr = self.prim.GetAttribute("xformOp:rotateZYX") 198 | rotate_attr.Set(rotation) 199 | 200 | def activate(self): 201 | """ 202 | Activate camera to viewport. 203 | """ 204 | self.vpi.get_viewport_window().set_active_camera(str(self.prim.GetPath())) 205 | 206 | def __repr__(self): 207 | return self.name 208 | 209 | 210 | class RigidBody: 211 | """ 212 | RigidBody objects that contains state information of the rigid body. 213 | """ 214 | def __init__(self, prim, dc): 215 | """ 216 | Initializes for RigidBody object 217 | 218 | Args: 219 | prim (pxr.Usd.Prim): rigid body prim 220 | dc (omni.isaac.motion_planning._motion_planning.MotionPlanning): motion planning interface from RMP extension 221 | """ 222 | self.prim = prim 223 | self._dc = dc 224 | self.name = prim.GetPrimPath().name 225 | self.handle = self.get_rigid_body_handle() 226 | 227 | def get_rigid_body_handle(self): 228 | """ 229 | Get rigid body handle. 230 | """ 231 | object_children = self.prim.GetChildren() 232 | for child in object_children: 233 | child_path = child.GetPath().pathString 234 | body_handle = self._dc.get_rigid_body(child_path) 235 | if body_handle != 0: 236 | bin_path = child_path 237 | 238 | object_handle = self._dc.get_rigid_body(bin_path) 239 | if object_handle != 0: return object_handle 240 | 241 | def get_linear_velocity(self): 242 | """ 243 | Get linear velocity of rigid body. 244 | """ 245 | return np.array(self._dc.get_rigid_body_linear_velocity(self.handle)) 246 | 247 | def get_angular_velocity(self): 248 | """ 249 | Get angular velocity of rigid body. 250 | """ 251 | return np.array(self._dc.get_rigid_body_angular_velocity(self.handle)) 252 | 253 | def get_speed(self): 254 | """ 255 | Get speed of rigid body given by the l2 norm of the velocity. 256 | """ 257 | velocity = self.get_linear_velocity() 258 | speed = np.linalg.norm(velocity) 259 | return speed 260 | 261 | def get_pose(self): 262 | """ 263 | Get pose of the rigid body containing the position and orientation information. 264 | """ 265 | return self._dc.get_rigid_body_pose(self.handle) 266 | 267 | def get_position(self): 268 | """ 269 | Get the position of the rigid body object. 270 | """ 271 | pose = self.get_pose() 272 | position = np.array(pose.p) 273 | return position 274 | 275 | def get_orientation(self): 276 | """ 277 | Get orientation of the rigid body object. 278 | """ 279 | pose = self.get_pose() 280 | orientation = np.array(pose.r) 281 | return orientation 282 | 283 | def get_bound(self): 284 | """ 285 | Get bounds of the rigid body object in global coordinates. 286 | """ 287 | bound = UsdGeom.Mesh(self.prim).ComputeWorldBound(0.0, "default").GetBox() 288 | return [np.array(bound.GetMin()), np.array(bound.GetMax())] 289 | 290 | def __repr__(self): 291 | return self.name 292 | -------------------------------------------------------------------------------- /grasp/utils/visualize.py: -------------------------------------------------------------------------------- 1 | import os 2 | import ffmpeg 3 | import matplotlib.pyplot as plt 4 | 5 | 6 | def screenshot(sd_helper, suffix="", prefix="image", directory="images/"): 7 | """ 8 | Take a screenshot of the current time step of a running NVIDIA Omniverse Isaac-Sim simulation. 9 | 10 | Args: 11 | sd_helper (omni.isaac.synthetic_utils.SyntheticDataHelper): helper class for visualizing OmniKit simulation 12 | suffix (str or int): suffix for output filename of image screenshot of current time step of simulation 13 | prefix (str): prefix for output filename of image screenshot of current time step of simulation 14 | directory (str): output directory of image screenshot of current time step of simulation 15 | """ 16 | gt = sd_helper.get_groundtruth( 17 | [ 18 | "rgb", 19 | ] 20 | ) 21 | 22 | image = gt["rgb"][..., :3] 23 | plt.imshow(image) 24 | 25 | if suffix == "": 26 | suffix = 0 27 | 28 | if isinstance(suffix, int): 29 | filename = os.path.join(directory, f'{prefix}_{suffix:05}.png') 30 | else: 31 | filename = os.path.join(directory, f'{prefix}_{suffix}.png') 32 | plt.axis('off') 33 | plt.savefig(filename) 34 | 35 | 36 | def img2vid(input_pattern, output_fn, pattern_type='glob', framerate=25): 37 | """ 38 | Create video from a collection of images. 39 | 40 | Args: 41 | input_pattern (str): input pattern for a path of collection of images 42 | output_fn (str): video output filename 43 | pattern_type (str): pattern type for input pattern 44 | framerate (int): video framerate 45 | """ 46 | ( 47 | ffmpeg 48 | .input(input_pattern, pattern_type=pattern_type, framerate=framerate) 49 | .output(output_fn) 50 | .run(overwrite_output=True, quiet=True) 51 | ) 52 | -------------------------------------------------------------------------------- /simulate_grasp.py: -------------------------------------------------------------------------------- 1 | import os 2 | import argparse 3 | 4 | from grasp.grasp_sim import GraspSimulator 5 | 6 | from omni.isaac.motion_planning import _motion_planning 7 | from omni.isaac.dynamic_control import _dynamic_control 8 | from omni.isaac.synthetic_utils import OmniKitHelper 9 | 10 | 11 | def main(args): 12 | 13 | kit = OmniKitHelper( 14 | {"renderer": "RayTracedLighting", "experience": f"{os.environ['EXP_PATH']}/isaac-sim-python.json", "width": args.width, "height": args.height} 15 | ) 16 | _mp = _motion_planning.acquire_motion_planning_interface() 17 | _dc = _dynamic_control.acquire_dynamic_control_interface() 18 | 19 | if args.video: record = True 20 | else: record = False 21 | 22 | sim = GraspSimulator(kit, _dc, _mp, record=record) 23 | 24 | # add object path 25 | if args.location == 'local': from_server = False 26 | else: from_server = True 27 | 28 | for path in args.path: 29 | 30 | sim.add_object_path(path, from_server=from_server) 31 | 32 | # start simulation 33 | sim.play() 34 | 35 | for _ in range(args.num): 36 | 37 | sim.add_object(position=(40, 0, 10)) 38 | 39 | sim.wait_for_drop() 40 | sim.wait_for_loading() 41 | 42 | evaluation = sim.execute_grasp(args.position, args.angle) 43 | 44 | output_string = f"Grasp evaluation: {evaluation}" 45 | print('\n' + ''.join(['#'] * len(output_string))) 46 | print(output_string) 47 | print(''.join(['#'] * len(output_string)) + '\n') 48 | 49 | # Stop physics simulation 50 | sim.stop() 51 | if record: sim.save_video(args.video) 52 | 53 | 54 | if __name__ == '__main__': 55 | 56 | parser = argparse.ArgumentParser(description='Simulate Panda arm planar grasp execution in NVIDIA Omniverse Isaac Sim') 57 | required = parser.add_argument_group('required arguments') 58 | required.add_argument('-P', '--path', type=str, nargs='+', metavar='', required=True, help='path to usd file or content folder') 59 | required.add_argument('-p', '--position', type=float, nargs=3, metavar='', required=True, help='grasp position, X Y Z') 60 | required.add_argument('-a', '--angle', type=float, metavar='', required=True, help='grasp angle in degrees') 61 | parser.add_argument('-l', '--location', type=str, metavar='', required=False, help='location of usd path, choices={local, nucleus_server}', choices=['local', 'nucleus_server'], default='local') 62 | parser.add_argument('-n', '--num', type=int, metavar='', required=False, help='number of objects to spawn in the scene', default=1) 63 | parser.add_argument('-v', '--video', type=str, metavar='', required=False, help='output filename of grasp simulation video') 64 | parser.add_argument('-W', '--width', type=int, metavar='', required=False, help='width of the viewport and generated images', default=1024) 65 | parser.add_argument('-H', '--height', type=int, metavar='', required=False, help='height of the viewport and generated images', default=800) 66 | args = parser.parse_args() 67 | 68 | print(args.path) 69 | 70 | main(args) --------------------------------------------------------------------------------