Frame Transformer#
One of the most common operations that needs to be performed within a physics simulation is the frame transformation: rewriting a vector or quaternion in the basis of an arbitrary euclidean coordinate system. There are many ways to accomplish this within Isaac and USD, but these methods can be cumbersome to implement within Isaac Lab’s GPU based simulation and cloned environments. To mitigate this problem, we have designed the Frame Transformer Sensor, that tracks and calculate the relative frame transformations for rigid bodies of interest to the scene.
The sensory is minimally defined by a source frame and a list of target frames. These definitions take the form of a prim path (for the source) and list of regex capable prim paths the rigid bodies to be tracked (for the targets).
from isaaclab.utils.configclass import configclass
##
# Pre-defined configs
##
from isaaclab_assets.robots.anymal import ANYMAL_C_CFG # isort: skip
@configclass
class FrameTransformerSensorSceneCfg(InteractiveSceneCfg):
"""Design the scene with sensors on the robot."""
# ground plane
ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg())
# lights
dome_light = AssetBaseCfg(
prim_path="/World/Light", spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75))
)
# robot
robot = ANYMAL_C_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")
# Rigid Object
cube = RigidObjectCfg(
prim_path="{ENV_REGEX_NS}/Cube",
spawn=sim_utils.CuboidCfg(
size=(1, 1, 1),
rigid_props=sim_utils.RigidBodyPropertiesCfg(),
mass_props=sim_utils.MassPropertiesCfg(mass=100.0),
collision_props=sim_utils.CollisionPropertiesCfg(),
physics_material=sim_utils.RigidBodyMaterialCfg(static_friction=1.0),
visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 1.0, 0.0), metallic=0.2),
),
init_state=RigidObjectCfg.InitialStateCfg(pos=(5, 0, 0.5)),
)
specific_transforms = FrameTransformerCfg(
prim_path="{ENV_REGEX_NS}/Robot/base",
target_frames=[
FrameTransformerCfg.FrameCfg(prim_path="{ENV_REGEX_NS}/Robot/LF_FOOT"),
FrameTransformerCfg.FrameCfg(prim_path="{ENV_REGEX_NS}/Robot/RF_FOOT"),
],
debug_vis=True,
)
cube_transform = FrameTransformerCfg(
prim_path="{ENV_REGEX_NS}/Robot/base",
target_frames=[FrameTransformerCfg.FrameCfg(prim_path="{ENV_REGEX_NS}/Cube")],
We can now run the scene and query the sensor for data
def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene):
.
.
.
# Simulate physics
while simulation_app.is_running():
.
.
.
# print information from the sensors
print("-------------------------------")
print(scene["specific_transforms"])
print("relative transforms:", scene["specific_transforms"].data.target_pos_source)
print("relative orientations:", scene["specific_transforms"].data.target_quat_source)
print("-------------------------------")
print(scene["cube_transform"])
print("relative transform:", scene["cube_transform"].data.target_pos_source)
print("-------------------------------")
print(scene["robot_transforms"])
print("relative transforms:", scene["robot_transforms"].data.target_pos_source)
Let’s take a look at the result for tracking specific objects. First, we can take a look at the data coming from the sensors on the feet
-------------------------------
FrameTransformer @ '/World/envs/env_.*/Robot/base':
tracked body frames: ['base', 'LF_FOOT', 'RF_FOOT']
number of envs: 1
source body frame: base
target frames (count: ['LF_FOOT', 'RF_FOOT']): 2
relative transforms: tensor([[[ 0.4658, 0.3085, -0.4840],
[ 0.4487, -0.2959, -0.4828]]], device='cuda:0')
relative orientations: tensor([[[ 0.9623, 0.0072, -0.2717, -0.0020],
[ 0.9639, 0.0052, -0.2663, -0.0014]]], device='cuda:0')
By activating the visualizer, we can see that the frames of the feet are rotated “upward” slightly. We can also see the explicit relative positions and rotations by querying the sensor for data, which returns these values as a list with the same order as the tracked frames. This becomes even more apparent if we examine the transforms specified by regex.
-------------------------------
FrameTransformer @ '/World/envs/env_.*/Robot/base':
tracked body frames: ['base', 'LF_FOOT', 'LF_HIP', 'LF_SHANK', 'LF_THIGH', 'LH_FOOT', 'LH_HIP', 'LH_SHANK', 'LH_THIGH', 'RF_FOOT', 'RF_HIP', 'RF_SHANK', 'RF_THIGH', 'RH_FOOT', 'RH_HIP', 'RH_SHANK', 'RH_THIGH', 'base']
number of envs: 1
source body frame: base
target frames (count: ['LF_FOOT', 'LF_HIP', 'LF_SHANK', 'LF_THIGH', 'LH_FOOT', 'LH_HIP', 'LH_SHANK', 'LH_THIGH', 'RF_FOOT', 'RF_HIP', 'RF_SHANK', 'RF_THIGH', 'RH_FOOT', 'RH_HIP', 'RH_SHANK', 'RH_THIGH', 'base']): 17
relative transforms: tensor([[[ 4.6581e-01, 3.0846e-01, -4.8398e-01],
[ 2.9990e-01, 1.0400e-01, -1.7062e-09],
[ 2.1409e-01, 2.9177e-01, -2.4214e-01],
[ 3.5980e-01, 1.8780e-01, 1.2608e-03],
[-4.8813e-01, 3.0973e-01, -4.5927e-01],
[-2.9990e-01, 1.0400e-01, 2.7044e-09],
[-2.1495e-01, 2.9264e-01, -2.4198e-01],
[-3.5980e-01, 1.8780e-01, 1.5582e-03],
[ 4.4871e-01, -2.9593e-01, -4.8277e-01],
[ 2.9990e-01, -1.0400e-01, -2.7057e-09],
[ 1.9971e-01, -2.8554e-01, -2.3778e-01],
[ 3.5980e-01, -1.8781e-01, -9.1049e-04],
[-5.0090e-01, -2.9095e-01, -4.5746e-01],
[-2.9990e-01, -1.0400e-01, 6.3592e-09],
[-2.1860e-01, -2.8251e-01, -2.5163e-01],
[-3.5980e-01, -1.8779e-01, -1.8792e-03],
[ 0.0000e+00, 0.0000e+00, 0.0000e+00]]], device='cuda:0')
Here, the sensor is tracking all rigid body children of Robot/base, but this expression is inclusive, meaning that the source body itself is also a target. This can be seen both by examining the source and target list, where base appears twice, and also in the returned data, where the sensor returns the relative transform to itself, (0, 0, 0).
Code for frame_transformer_sensor.py
1# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
2# All rights reserved.
3#
4# SPDX-License-Identifier: BSD-3-Clause
5
6import argparse
7
8from isaaclab.app import AppLauncher
9
10# add argparse arguments
11parser = argparse.ArgumentParser(description="Example on using the frame transformer sensor.")
12parser.add_argument("--num_envs", type=int, default=1, help="Number of environments to spawn.")
13parser.add_argument(
14 "--physics",
15 default="isaacsim_physx",
16 choices=["isaacsim_physx"],
17 help="Physics backend.",
18)
19# append AppLauncher cli args
20AppLauncher.add_app_launcher_args(parser)
21# demos should open Kit visualizer by default
22parser.set_defaults(visualizer=["kit"])
23# parse the arguments
24args_cli = parser.parse_args()
25
26# launch omniverse app
27app_launcher = AppLauncher(args_cli)
28simulation_app = app_launcher.app
29
30"""Rest everything follows."""
31
32import torch
33
34import isaaclab.sim as sim_utils
35from isaaclab.assets import AssetBaseCfg, RigidObjectCfg
36from isaaclab.scene import InteractiveScene, InteractiveSceneCfg
37from isaaclab.sensors import FrameTransformerCfg
38from isaaclab.utils.configclass import configclass
39
40##
41# Pre-defined configs
42##
43from isaaclab_assets.robots.anymal import ANYMAL_C_CFG # isort: skip
44
45
46@configclass
47class FrameTransformerSensorSceneCfg(InteractiveSceneCfg):
48 """Design the scene with sensors on the robot."""
49
50 # ground plane
51 ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg())
52
53 # lights
54 dome_light = AssetBaseCfg(
55 prim_path="/World/Light", spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75))
56 )
57
58 # robot
59 robot = ANYMAL_C_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")
60
61 # Rigid Object
62 cube = RigidObjectCfg(
63 prim_path="{ENV_REGEX_NS}/Cube",
64 spawn=sim_utils.CuboidCfg(
65 size=(1, 1, 1),
66 rigid_props=sim_utils.RigidBodyPropertiesCfg(),
67 mass_props=sim_utils.MassPropertiesCfg(mass=100.0),
68 collision_props=sim_utils.CollisionPropertiesCfg(),
69 physics_material=sim_utils.RigidBodyMaterialCfg(static_friction=1.0),
70 visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 1.0, 0.0), metallic=0.2),
71 ),
72 init_state=RigidObjectCfg.InitialStateCfg(pos=(5, 0, 0.5)),
73 )
74
75 specific_transforms = FrameTransformerCfg(
76 prim_path="{ENV_REGEX_NS}/Robot/base",
77 target_frames=[
78 FrameTransformerCfg.FrameCfg(prim_path="{ENV_REGEX_NS}/Robot/LF_FOOT"),
79 FrameTransformerCfg.FrameCfg(prim_path="{ENV_REGEX_NS}/Robot/RF_FOOT"),
80 ],
81 debug_vis=True,
82 )
83
84 cube_transform = FrameTransformerCfg(
85 prim_path="{ENV_REGEX_NS}/Robot/base",
86 target_frames=[FrameTransformerCfg.FrameCfg(prim_path="{ENV_REGEX_NS}/Cube")],
87 debug_vis=False,
88 )
89
90 robot_transforms = FrameTransformerCfg(
91 prim_path="{ENV_REGEX_NS}/Robot/base",
92 target_frames=[FrameTransformerCfg.FrameCfg(prim_path="{ENV_REGEX_NS}/Robot/.*")],
93 debug_vis=False,
94 )
95
96
97def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene):
98 """Run the simulator."""
99 # Define simulation stepping
100 sim_dt = sim.get_physics_dt()
101 sim_time = 0.0
102 count = 0
103
104 # Simulate physics
105 while simulation_app.is_running():
106 if count % 500 == 0:
107 # reset counter
108 count = 0
109 # reset the scene entities
110 # root state
111 # we offset the root state by the origin since the states are written in simulation world frame
112 # if this is not done, then the robots will be spawned at the (0, 0, 0) of the simulation world
113 root_pose = scene["robot"].data.default_root_pose.torch.clone()
114 root_pose[:, :3] += scene.env_origins
115 scene["robot"].write_root_pose_to_sim_index(root_pose=root_pose)
116 root_vel = scene["robot"].data.default_root_vel.torch.clone()
117 scene["robot"].write_root_velocity_to_sim_index(root_velocity=root_vel)
118 # set joint positions with some noise
119 joint_pos, joint_vel = (
120 scene["robot"].data.default_joint_pos.torch.clone(),
121 scene["robot"].data.default_joint_vel.torch.clone(),
122 )
123 joint_pos += torch.rand_like(joint_pos) * 0.1
124 scene["robot"].write_joint_position_to_sim_index(position=joint_pos)
125 scene["robot"].write_joint_velocity_to_sim_index(velocity=joint_vel)
126 # clear internal buffers
127 scene.reset()
128 print("[INFO]: Resetting robot state...")
129 # Apply default actions to the robot
130 # -- generate actions/commands
131 targets = scene["robot"].data.default_joint_pos.torch
132 # -- apply action to the robot
133 scene["robot"].set_joint_position_target_index(target=targets)
134 # -- write data to sim
135 scene.write_data_to_sim()
136 # perform step
137 sim.step()
138 # update sim-time
139 sim_time += sim_dt
140 count += 1
141 # update buffers
142 scene.update(sim_dt)
143
144 # print information from the sensors
145 print("-------------------------------")
146 print(scene["specific_transforms"])
147 print("relative transforms:", scene["specific_transforms"].data.target_pos_source)
148 print("relative orientations:", scene["specific_transforms"].data.target_quat_source)
149 print("-------------------------------")
150 print(scene["cube_transform"])
151 print("relative transform:", scene["cube_transform"].data.target_pos_source)
152 print("-------------------------------")
153 print(scene["robot_transforms"])
154 print("relative transforms:", scene["robot_transforms"].data.target_pos_source)
155
156
157def main():
158 """Main function."""
159
160 # Initialize the simulation context
161 sim_cfg = sim_utils.SimulationCfg(dt=0.005, device=args_cli.device)
162 sim = sim_utils.SimulationContext(sim_cfg)
163 # Set main camera
164 sim.set_camera_view(eye=[3.5, 3.5, 3.5], target=[0.0, 0.0, 0.0])
165 # design scene
166 scene_cfg = FrameTransformerSensorSceneCfg(num_envs=args_cli.num_envs, env_spacing=2.0)
167 scene = InteractiveScene(scene_cfg)
168 # Play the simulator
169 sim.reset()
170 # Now we are ready!
171 print("[INFO]: Setup complete...")
172 # Run the simulator
173 run_simulator(sim, scene)
174
175
176if __name__ == "__main__":
177 # run the main function
178 main()
179 # close sim app
180 simulation_app.close()