Skip to content

Commit 4725658

Browse files
LaniakearrLaniakearr
committed
Merge remote-tracking branch 'origin/main' into ros2_bridge
2 parents 9a91b2e + 50b9a14 commit 4725658

File tree

3 files changed

+100
-232
lines changed

3 files changed

+100
-232
lines changed

‎autonomy/simulation/Wato_additional_scripts/robot_arm_controllers/joint_space_test.py‎

Lines changed: 30 additions & 49 deletions
Original file line numberDiff line numberDiff line change
@@ -1,30 +1,30 @@
1-
from isaaclab_assets import UR10_CFG
2-
from isaaclab.utils.math import subtract_frame_transforms
3-
from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR
4-
from isaaclab.utils import configclass
5-
from isaaclab.scene import InteractiveScene, InteractiveSceneCfg
6-
from isaaclab.markers.config import FRAME_MARKER_CFG
7-
from isaaclab.markers import VisualizationMarkers
8-
from isaaclab.managers import SceneEntityCfg
9-
from isaaclab.controllers import DifferentialIKController, DifferentialIKControllerCfg
10-
from isaaclab.assets import AssetBaseCfg
11-
import isaaclab.sim as sim_utils
12-
import torch
131
import argparse
142
from isaaclab.app import AppLauncher
153

16-
parser = argparse.ArgumentParser(
17-
description="Robot Arm with Joint Space Control")
4+
parser = argparse.ArgumentParser(description="Robot Arm with Joint Space Control")
185
AppLauncher.add_app_launcher_args(parser)
196
args_cli = parser.parse_args()
207

218
app_launcher = AppLauncher(args_cli)
229
simulation_app = app_launcher.app
2310

11+
import torch
12+
13+
import isaaclab.sim as sim_utils
14+
from isaaclab.assets import AssetBaseCfg
15+
from isaaclab.controllers import DifferentialIKController, DifferentialIKControllerCfg
16+
from isaaclab.managers import SceneEntityCfg
17+
from isaaclab.markers import VisualizationMarkers
18+
from isaaclab.markers.config import FRAME_MARKER_CFG
19+
from isaaclab.scene import InteractiveScene, InteractiveSceneCfg
20+
from isaaclab.utils import configclass
21+
from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR
22+
from isaaclab.utils.math import subtract_frame_transforms
2423

2524
##
2625
# Pre-defined configs
2726
##
27+
from isaaclab_assets import UR10_CFG
2828

2929

3030
@configclass
@@ -41,55 +41,41 @@ class TableTopSceneCfg(InteractiveSceneCfg):
4141
# lights
4242
dome_light = AssetBaseCfg(
4343
prim_path="/World/Light",
44-
spawn=sim_utils.DomeLightCfg(
45-
intensity=3000.0,
46-
color=(
47-
0.75,
48-
0.75,
49-
0.75)))
44+
spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75))
45+
)
5046

5147
# mount
5248
table = AssetBaseCfg(
5349
prim_path="{ENV_REGEX_NS}/Table",
5450
spawn=sim_utils.UsdFileCfg(
55-
usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/Stand/stand_instanceable.usd",
56-
scale=(
57-
2.0,
58-
2.0,
59-
2.0)),
51+
usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/Stand/stand_instanceable.usd", scale=(2.0, 2.0, 2.0)
52+
),
6053
)
6154

6255
# articulation
6356
robot = UR10_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")
6457

6558

6659
def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene):
67-
60+
6861
robot = scene["robot"]
6962

7063
# Create controller
71-
diff_ik_cfg = DifferentialIKControllerCfg(
72-
command_type="pose", use_relative_mode=False, ik_method="dls")
73-
diff_ik_controller = DifferentialIKController(
74-
diff_ik_cfg, num_envs=scene.num_envs, device=sim.device)
64+
diff_ik_cfg = DifferentialIKControllerCfg(command_type="pose", use_relative_mode=False, ik_method="dls")
65+
diff_ik_controller = DifferentialIKController(diff_ik_cfg, num_envs=scene.num_envs, device=sim.device)
7566

7667
# Markers
7768
frame_marker_cfg = FRAME_MARKER_CFG.copy()
7869
frame_marker_cfg.markers["frame"].scale = (0.1, 0.1, 0.1)
79-
ee_marker = VisualizationMarkers(
80-
frame_marker_cfg.replace(
81-
prim_path="/Visuals/ee_current"))
70+
ee_marker = VisualizationMarkers(frame_marker_cfg.replace(prim_path="/Visuals/ee_current"))
8271

8372
# Specify robot-specific parameters
84-
robot_entity_cfg = SceneEntityCfg(
85-
"robot", joint_names=[".*"], body_names=["ee_link"])
73+
robot_entity_cfg = SceneEntityCfg("robot", joint_names=[".*"], body_names=["ee_link"])
8674

8775
# Resolving the scene entities
8876
robot_entity_cfg.resolve(scene)
8977

90-
# Obtain the frame index of the end-effector ; For a fixed base robot, the
91-
# frame index is one less than the body index. This is because ; the root
92-
# body is not included in the returned Jacobians.
78+
# Obtain the frame index of the end-effector ; For a fixed base robot, the frame index is one less than the body index. This is because ; the root body is not included in the returned Jacobians.
9379
if robot.is_fixed_base:
9480
ee_jacobi_idx = robot_entity_cfg.body_ids[0] - 1
9581
else:
@@ -113,24 +99,20 @@ def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene):
11399
[1.0, -1.712, 1.712, 0.0, 0.0, 0.0],
114100
[0.0, 1.712, 1.712, 0.0, 0.0, 0.0],
115101
]
116-
joint_position = torch.tensor(
117-
joint_position_list[joint_position_index],
118-
device=sim.device)
102+
joint_position = torch.tensor(joint_position_list[joint_position_index], device=sim.device)
119103
joint_vel = robot.data.default_joint_vel.clone()
120104

121105
if joint_position_index >= len(joint_position_list) - 1:
122106
joint_position_index = 0
123107
else:
124108
joint_position_index += 1
125-
109+
126110
robot.reset()
127111

128-
joint_pos_des = joint_position.unsqueeze(
129-
0)[:, robot_entity_cfg.joint_ids].clone()
112+
joint_pos_des = joint_position.unsqueeze(0)[:, robot_entity_cfg.joint_ids].clone()
130113

131114
# apply actions (Smooth movement)
132-
robot.set_joint_position_target(
133-
joint_pos_des, joint_ids=robot_entity_cfg.joint_ids)
115+
robot.set_joint_position_target(joint_pos_des, joint_ids=robot_entity_cfg.joint_ids)
134116
scene.write_data_to_sim()
135117

136118
# perform step
@@ -143,8 +125,7 @@ def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene):
143125
scene.update(sim_dt)
144126

145127
# obtain quantities from simulation
146-
ee_pose_w = robot.data.body_state_w[:,
147-
robot_entity_cfg.body_ids[0], 0:7]
128+
ee_pose_w = robot.data.body_state_w[:, robot_entity_cfg.body_ids[0], 0:7]
148129
# update marker positions
149130
ee_marker.visualize(ee_pose_w[:, 0:3], ee_pose_w[:, 3:7])
150131

@@ -172,4 +153,4 @@ def main():
172153

173154
if __name__ == "__main__":
174155
main()
175-
simulation_app.close()
156+
simulation_app.close()

‎autonomy/simulation/Wato_additional_scripts/robot_arm_controllers/task_space_test.py‎

Lines changed: 40 additions & 76 deletions
Original file line numberDiff line numberDiff line change
@@ -1,35 +1,31 @@
1-
from isaaclab_assets import UR10_CFG, FRANKA_PANDA_HIGH_PD_CFG
2-
from isaaclab.utils.math import subtract_frame_transforms
3-
from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR
4-
from isaaclab.utils import configclass
5-
from isaaclab.scene import InteractiveScene, InteractiveSceneCfg
6-
from isaaclab.markers.config import FRAME_MARKER_CFG
7-
from isaaclab.markers import VisualizationMarkers
8-
from isaaclab.managers import SceneEntityCfg
9-
from isaaclab.controllers import DifferentialIKController, DifferentialIKControllerCfg
10-
from isaaclab.assets import AssetBaseCfg
11-
import isaaclab.sim as sim_utils
12-
import torch
131
import argparse
142
from isaaclab.app import AppLauncher
153

16-
parser = argparse.ArgumentParser(
17-
description="Robot Arm with Task Space IK Control")
18-
parser.add_argument(
19-
"--robot",
20-
type=str,
21-
default="franka_panda",
22-
help="Name of the robot.")
4+
parser = argparse.ArgumentParser(description="Robot Arm with Task Space IK Control")
5+
parser.add_argument("--robot", type=str, default="franka_panda", help="Name of the robot.")
236
AppLauncher.add_app_launcher_args(parser)
247
args_cli = parser.parse_args()
258

269
app_launcher = AppLauncher(args_cli)
2710
simulation_app = app_launcher.app
2811

12+
import torch
13+
14+
import isaaclab.sim as sim_utils
15+
from isaaclab.assets import AssetBaseCfg
16+
from isaaclab.controllers import DifferentialIKController, DifferentialIKControllerCfg
17+
from isaaclab.managers import SceneEntityCfg
18+
from isaaclab.markers import VisualizationMarkers
19+
from isaaclab.markers.config import FRAME_MARKER_CFG
20+
from isaaclab.scene import InteractiveScene, InteractiveSceneCfg
21+
from isaaclab.utils import configclass
22+
from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR
23+
from isaaclab.utils.math import subtract_frame_transforms
2924

3025
##
3126
# Pre-defined configs
3227
##
28+
from isaaclab_assets import UR10_CFG, FRANKA_PANDA_HIGH_PD_CFG
3329

3430

3531
@configclass
@@ -46,22 +42,15 @@ class TableTopSceneCfg(InteractiveSceneCfg):
4642
# lights
4743
dome_light = AssetBaseCfg(
4844
prim_path="/World/Light",
49-
spawn=sim_utils.DomeLightCfg(
50-
intensity=3000.0,
51-
color=(
52-
0.75,
53-
0.75,
54-
0.75)))
45+
spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75))
46+
)
5547

5648
# mount
5749
table = AssetBaseCfg(
5850
prim_path="{ENV_REGEX_NS}/Table",
5951
spawn=sim_utils.UsdFileCfg(
60-
usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/Stand/stand_instanceable.usd",
61-
scale=(
62-
2.0,
63-
2.0,
64-
2.0)),
52+
usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/Stand/stand_instanceable.usd", scale=(2.0, 2.0, 2.0)
53+
),
6554
)
6655

6756
# Cube
@@ -75,56 +64,41 @@ class TableTopSceneCfg(InteractiveSceneCfg):
7564

7665
# articulation
7766
if args_cli.robot == "franka_panda":
78-
robot = FRANKA_PANDA_HIGH_PD_CFG.replace(
79-
prim_path="{ENV_REGEX_NS}/Robot")
67+
robot = FRANKA_PANDA_HIGH_PD_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")
8068
elif args_cli.robot == "ur10":
8169
robot = UR10_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")
8270
else:
83-
raise ValueError(
84-
f"Robot {args_cli.robot} is not supported. Valid: franka_panda, ur10")
71+
raise ValueError(f"Robot {args_cli.robot} is not supported. Valid: franka_panda, ur10")
8572
# robot = UR10_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")
8673

8774

8875
def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene):
89-
76+
9077
robot = scene["robot"]
9178

9279
# Create controller
93-
diff_ik_cfg = DifferentialIKControllerCfg(
94-
command_type="pose", use_relative_mode=False, ik_method="dls")
95-
diff_ik_controller = DifferentialIKController(
96-
diff_ik_cfg, num_envs=scene.num_envs, device=sim.device)
80+
diff_ik_cfg = DifferentialIKControllerCfg(command_type="pose", use_relative_mode=False, ik_method="dls")
81+
diff_ik_controller = DifferentialIKController(diff_ik_cfg, num_envs=scene.num_envs, device=sim.device)
9782

9883
# Markers
9984
frame_marker_cfg = FRAME_MARKER_CFG.copy()
10085
frame_marker_cfg.markers["frame"].scale = (0.1, 0.1, 0.1)
101-
ee_marker = VisualizationMarkers(
102-
frame_marker_cfg.replace(
103-
prim_path="/Visuals/ee_current"))
104-
goal_marker = VisualizationMarkers(
105-
frame_marker_cfg.replace(
106-
prim_path="/Visuals/ee_goal"))
86+
ee_marker = VisualizationMarkers(frame_marker_cfg.replace(prim_path="/Visuals/ee_current"))
87+
goal_marker = VisualizationMarkers(frame_marker_cfg.replace(prim_path="/Visuals/ee_goal"))
10788

10889
# Specify robot-specific parameters
10990
if args_cli.robot == "franka_panda":
110-
robot_entity_cfg = SceneEntityCfg(
111-
"robot",
112-
joint_names=["panda_joint.*"],
113-
body_names=["panda_hand"])
91+
robot_entity_cfg = SceneEntityCfg("robot", joint_names=["panda_joint.*"], body_names=["panda_hand"])
11492
elif args_cli.robot == "ur10":
115-
robot_entity_cfg = SceneEntityCfg(
116-
"robot", joint_names=[".*"], body_names=["ee_link"])
93+
robot_entity_cfg = SceneEntityCfg("robot", joint_names=[".*"], body_names=["ee_link"])
11794
else:
118-
raise ValueError(
119-
f"Robot {args_cli.robot} is not supported. Valid: franka_panda, ur10")
95+
raise ValueError(f"Robot {args_cli.robot} is not supported. Valid: franka_panda, ur10")
12096
# robot_entity_cfg = SceneEntityCfg("robot", joint_names=[".*"], body_names=["ee_link"])
12197

12298
# Resolving the scene entities
12399
robot_entity_cfg.resolve(scene)
124100

125-
# Obtain the frame index of the end-effector ; For a fixed base robot, the
126-
# frame index is one less than the body index. This is because ; the root
127-
# body is not included in the returned Jacobians.
101+
# Obtain the frame index of the end-effector ; For a fixed base robot, the frame index is one less than the body index. This is because ; the root body is not included in the returned Jacobians.
128102
if robot.is_fixed_base:
129103
ee_jacobi_idx = robot_entity_cfg.body_ids[0] - 1
130104
else:
@@ -143,27 +117,21 @@ def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene):
143117
joint_position_list = [
144118
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
145119
]
146-
joint_position = torch.tensor(
147-
joint_position_list[joint_position_index],
148-
device=sim.device)
120+
joint_position = torch.tensor(joint_position_list[joint_position_index], device=sim.device)
149121
joint_vel = robot.data.default_joint_vel.clone()
150-
joint_pos_des = joint_position.unsqueeze(
151-
0)[:, robot_entity_cfg.joint_ids].clone()
122+
joint_pos_des = joint_position.unsqueeze(0)[:, robot_entity_cfg.joint_ids].clone()
152123
robot.write_joint_state_to_sim(joint_pos_des, joint_vel)
153124

154125
while simulation_app.is_running():
155126
# Get cube/target_point coordinates
156127
position, quaternion = scene["cube"].get_world_poses()
157-
# Quaternion is in (w, x, y, z)
158-
ik_commands = torch.cat([position, quaternion], dim=1)
128+
ik_commands = torch.cat([position, quaternion], dim=1) # Quaternion is in (w, x, y, z)
159129

160130
diff_ik_controller.set_command(ik_commands)
161131

162132
# obtain quantities from simulation
163-
jacobian = robot.root_physx_view.get_jacobians()[
164-
:, ee_jacobi_idx, :, robot_entity_cfg.joint_ids]
165-
ee_pose_w = robot.data.body_state_w[:,
166-
robot_entity_cfg.body_ids[0], 0:7]
133+
jacobian = robot.root_physx_view.get_jacobians()[:, ee_jacobi_idx, :, robot_entity_cfg.joint_ids]
134+
ee_pose_w = robot.data.body_state_w[:, robot_entity_cfg.body_ids[0], 0:7]
167135
root_pose_w = robot.data.root_state_w[:, 0:7]
168136
joint_pos = robot.data.joint_pos[:, robot_entity_cfg.joint_ids]
169137

@@ -173,12 +141,10 @@ def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene):
173141
)
174142

175143
# compute the joint commands
176-
joint_pos_des = diff_ik_controller.compute(
177-
ee_pos_b, ee_quat_b, jacobian, joint_pos)
144+
joint_pos_des = diff_ik_controller.compute(ee_pos_b, ee_quat_b, jacobian, joint_pos)
178145

179146
# apply actions (Smooth movement)
180-
robot.set_joint_position_target(
181-
joint_pos_des, joint_ids=robot_entity_cfg.joint_ids)
147+
robot.set_joint_position_target(joint_pos_des, joint_ids=robot_entity_cfg.joint_ids)
182148
scene.write_data_to_sim()
183149

184150
# perform step
@@ -188,13 +154,11 @@ def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene):
188154
scene.update(sim_dt)
189155

190156
# obtain quantities from simulation
191-
ee_pose_w = robot.data.body_state_w[:,
192-
robot_entity_cfg.body_ids[0], 0:7]
157+
ee_pose_w = robot.data.body_state_w[:, robot_entity_cfg.body_ids[0], 0:7]
193158

194159
# update marker positions
195160
ee_marker.visualize(ee_pose_w[:, 0:3], ee_pose_w[:, 3:7])
196-
goal_marker.visualize(
197-
ik_commands[:, 0:3] + scene.env_origins, ik_commands[:, 3:7])
161+
goal_marker.visualize(ik_commands[:, 0:3] + scene.env_origins, ik_commands[:, 3:7])
198162

199163

200164
def main():
@@ -220,4 +184,4 @@ def main():
220184

221185
if __name__ == "__main__":
222186
main()
223-
simulation_app.close()
187+
simulation_app.close()

0 commit comments

Comments
 (0)