-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathfranka.py
220 lines (191 loc) · 8.42 KB
/
franka.py
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
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
"""
This script demonstrates how to use the inverse kinematics controller with the simulator.
The differential IK controller can be configured in different modes. It uses the Jacobians computed by
PhysX. This helps perform parallelized computation of the inverse kinematics.
"""
"""Launch Isaac Sim Simulator first."""
import argparse
from franka_lib import To_Position
from omni.isaac.kit import SimulationApp
# add argparse arguments
parser = argparse.ArgumentParser("Welcome to Orbit: Omniverse Robotics Environments!")
parser.add_argument("--headless", action="store_true", default=False, help="Force display off at all times.")
parser.add_argument("--robot", type=str, default="franka_panda", help="Name of the robot.")
parser.add_argument("--num_envs", type=int, default=1, help="Number of environments to spawn.")
args_cli = parser.parse_args()
# launch omniverse app
config = {"headless": args_cli.headless}
simulation_app = SimulationApp(config)
"""Rest everything follows."""
import torch
import numpy as np
import omni.isaac.core.utils.prims as prim_utils
from omni.isaac.cloner import GridCloner
from omni.isaac.core.simulation_context import SimulationContext
from omni.isaac.core.utils.carb import set_carb_setting
from omni.isaac.core.utils.viewports import set_camera_view
import omni.isaac.orbit.utils.kit as kit_utils
from omni.isaac.orbit.controllers.differential_inverse_kinematics import (
DifferentialInverseKinematics,
DifferentialInverseKinematicsCfg,
)
from omni.isaac.orbit.markers import StaticMarker
from omni.isaac.orbit.robots.config.franka import FRANKA_PANDA_ARM_WITH_PANDA_HAND_CFG
from omni.isaac.orbit.robots.config.universal_robots import UR10_CFG
from omni.isaac.orbit.robots.single_arm import SingleArmManipulator
from omni.isaac.orbit.utils.assets import ISAAC_NUCLEUS_DIR
from omni.isaac.core.objects import DynamicCuboid
"""
Main
"""
def main():
"""Spawns a single-arm manipulator and applies commands through inverse kinematics control."""
# Load kit helper
sim = SimulationContext(physics_dt=0.01, rendering_dt=0.01, backend="torch", device="cuda:0")
# Set main camera
set_camera_view([1.5, 0, 1], [0.0, 0.0, 0.0])
# Enable GPU pipeline and flatcache
if sim.get_physics_context().use_gpu_pipeline:
sim.get_physics_context().enable_flatcache(True)
# Enable hydra scene-graph instancing
set_carb_setting(sim._settings, "/persistent/omnihydra/useSceneGraphInstancing", True)
# Create interface to clone the scene
cloner = GridCloner(spacing=2.0)
cloner.define_base_env("/World/envs")
# Everything under the namespace "/World/envs/env_0" will be cloned
prim_utils.define_prim("/World/envs/env_0")
# Spawn things into stage
# Markers
ee_marker = StaticMarker("/Visuals/ee_current", count=args_cli.num_envs, scale=(0.1, 0.1, 0.1))
goal_marker = StaticMarker("/Visuals/ee_goal", count=args_cli.num_envs, scale=(0.1, 0.1, 0.1))
# Ground-plane
kit_utils.create_ground_plane("/World/defaultGroundPlane", z_position=0)
# Lights-1
prim_utils.create_prim(
"/World/Light/GreySphere",
"SphereLight",
translation=(4.5, 3.5, 10.0),
attributes={"radius": 2.5, "intensity": 600.0, "color": (0.75, 0.75, 0.75)},
)
# Lights-2
prim_utils.create_prim(
"/World/Light/WhiteSphere",
"SphereLight",
translation=(-4.5, 3.5, 10.0),
attributes={"radius": 2.5, "intensity": 600.0, "color": (1.0, 1.0, 1.0)},
)
# -- Robot
robot_cfg = FRANKA_PANDA_ARM_WITH_PANDA_HAND_CFG
# configure robot settings to use IK controller
robot_cfg.data_info.enable_jacobian = True
robot_cfg.rigid_props.disable_gravity = True
# spawn robot
robot = SingleArmManipulator(cfg=robot_cfg)
robot.spawn("/World/envs/env_0/Robot", translation=(0.0, 0.0, 0.0))
cube = DynamicCuboid(
prim_path="/World/random_cube", # The prim path of the cube in the USD stage
name="fancy_cube", # The unique name used to retrieve the object from the scene later on
position=np.array([0.6, 0, 0.025]), # Using the current stage units which is in meters by default.
scale=np.array([0.05, 0.05, 0.05]), # most arguments accept mainly numpy arrays.
color=np.array([0, 0, 1.0]), # RGB channels, going from 0-1
mass = 1
)
cube.enable_rigid_body_physics()
# Clone the scene
num_envs = args_cli.num_envs
envs_prim_paths = cloner.generate_paths("/World/envs/env", num_paths=num_envs)
envs_positions = cloner.clone(source_prim_path="/World/envs/env_0", prim_paths=envs_prim_paths)
# convert environment positions to torch tensor
envs_positions = torch.tensor(envs_positions, dtype=torch.float, device=sim.device)
# filter collisions within each environment instance
physics_scene_path = sim.get_physics_context().prim_path
cloner.filter_collisions(
physics_scene_path, "/World/collisions", envs_prim_paths, global_paths=["/World/defaultGroundPlane"]
)
# Create controller
# the controller takes as command type: {position/pose}_{abs/rel}
ik_control_cfg = DifferentialInverseKinematicsCfg(
command_type="pose_abs",
ik_method="dls",
position_offset=robot.cfg.ee_info.pos_offset,
rotation_offset=robot.cfg.ee_info.rot_offset,
)
ik_controller = DifferentialInverseKinematics(ik_control_cfg, num_envs, sim.device)
# Play the simulator
sim.reset()
# Acquire handles
# Initialize handles
robot.initialize("/World/envs/env_.*/Robot")
ik_controller.initialize()
# Reset states
robot.reset_buffers()
ik_controller.reset_idx()
# Now we are ready!
print("[INFO]: Setup complete...")
# Create buffers to store actions
ik_commands = torch.zeros(robot.count, ik_controller.num_actions, device=robot.device)
robot_actions = torch.ones(robot.count, robot.num_actions, device=robot.device)
To_position = To_Position(ik_commands = ik_commands,
robot_actions = robot_actions,
device = sim.device,
controller = ik_controller,
envs_positions = envs_positions,
robot = robot)
# Define simulation stepping
sim_dt = sim.get_physics_dt()
# episode counter
sim_time = 0.0
count = 0
# Note: We need to update buffers before the first step for the controller.
robot.update_buffers(sim_dt)
# Simulate physics
# Simulate physics
while simulation_app.is_running():
# If simulation is stopped, then exit.
if sim.is_stopped():
break
# If simulation is paused, then skip.
if not sim.is_playing():
sim.step(render=not args_cli.headless)
continue
# reset
if count == 0:
# reset time
count = 0
sim_time = 0.0
# reset dof state
dof_pos, dof_vel = robot.get_default_dof_state()
robot.set_dof_state(dof_pos, dof_vel)
robot.reset_buffers()
# reset controller
ik_controller.reset_idx()
# reset actions
if count <= 100:
To_position.gripper(is_Open= True)
To_position.execute(position=[0.6, 0, 0.025, 0, 1, 0, 0])
if count > 100 and count <= 200:
To_position.gripper(is_Open=False)
To_position.execute(position=[0.6, 0, 0.025, 0, 1, 0, 0])
if count > 200:
To_position.gripper(is_Open=False)
To_position.execute(position=[0.6, 0, 0.4, 0, 1, 0, 0])
sim.step(render=not args_cli.headless)
# update sim-time
sim_time += sim_dt
count += 1
# note: to deal with timeline events such as stopping, we need to check if the simulation is playing
if sim.is_playing():
# update buffers
robot.update_buffers(sim_dt)
# update marker positions
ee_marker.set_world_poses(robot.data.ee_state_w[:, 0:3], robot.data.ee_state_w[:, 3:7])
goal_marker.set_world_poses(ik_commands[:, 0:3] + envs_positions, ik_commands[:, 3:7])
if __name__ == "__main__":
# Run IK example with Manipulator
main()
# Close the simulator
simulation_app.close()