Skip to content

K
KincoActuatorIsaacLab
Unverified Commit 620ce2b0 authored by jsmith-bdai's avatar jsmith-bdai Committed by GitHub
Browse files
Options

Fixes source frame indexing in FrameTransfomer sensor (#350) 

# Description

The FrameTransformer sensor always assumed that the source frame index
is 0 in the parsed regex expression. However, a recent issue appeared
where this is not the case, and it led to a bug due to the hard-coded
value. This MR fixes the source frame index in the sensor and also adds
a unit test to check this behavior.

Fixes: https://github.com/NVIDIA-Omniverse/Orbit/issues/170

## Type of change

- Bug fix (non-breaking change which fixes an issue)

## Checklist

- [x] I have run the [`pre-commit` checks](https://pre-commit.com/) with
`./orbit.sh --format`
- [x] I have made corresponding changes to the documentation
- [x] My changes generate no new warnings
- [x] I have added tests that prove my fix is effective or that my
feature works
- [x] I have run all the tests with `./orbit.sh --test` and they pass
- [x] I have updated the changelog and the corresponding version in the
extension's `config/extension.toml` file
- [x] I have added my name to the `CONTRIBUTORS.md` or my name already
exists there

---------
Co-authored-by: 's avatarMayank Mittal <mittalma@leggedrobotics.com>
parent ea0ee24f
Expand all Hide whitespace changes
Inline Side-by-side
Showing with 491 additions and 356 deletions
source/extensions/omni.isaac.orbit/config/extension.toml
View file @ 620ce2b0
[package]
# Note: Semantic Versioning is used: https://semver.org/
version = "0.10.11"
version = "0.10.12"
# Description
title = "ORBIT framework for Robot Learning"
......
source/extensions/omni.isaac.orbit/docs/CHANGELOG.rst
View file @ 620ce2b0
Changelog
---------
0.10.12 (2024-01-10)
~~~~~~~~~~~~~~~~~~~~
Fixed
^^^^^
* Fixed indexing of source and target frames in the :class:`omni.isaac.orbit.sensors.FrameTransformer` class.
Earlier, it always assumed that the source frame body is at index 0. Now, it uses the body index of the
source frame to compute the transformation.
Deprecated
^^^^^^^^^^
* Renamed quantities in the :class:`omni.isaac.orbit.sensors.FrameTransformerData` class to be more
consistent with the terminology used in the asset classes. The following quantities are deprecated:
* ``target_rot_w`` -> ``target_quat_w``
* ``source_rot_w`` -> ``source_quat_w``
* ``target_rot_source`` -> ``target_quat_source``
0.10.11 (2024-01-08)
~~~~~~~~~~~~~~~~~~~~
......
source/extensions/omni.isaac.orbit/omni/isaac/orbit/sensors/frame_transformer/frame_transformer_data.py
View file @ 620ce2b0
......@@ -6,6 +6,7 @@
from __future__ import annotations
import torch
import warnings
from dataclasses import dataclass
......@@ -14,11 +15,11 @@ class FrameTransformerData:
"""Data container for the frame transformer sensor."""
target_frame_names: list[str] = None
"""Target frame names (this denotes the order that frame data will be ordered).
"""Target frame names (this denotes the order in which that frame data is ordered).
The frame names are resolved from the :attr:`FrameTransformerCfg.FrameCfg.name` field.
This usually follows the order in which the frames are defined in the config. However, in the case of
regex matching, the order may be different.
This usually follows the order in which the frames are defined in the config. However, in
the case of regex matching, the order may be different.
"""
target_pos_source: torch.Tensor = None
......@@ -26,28 +27,63 @@ class FrameTransformerData:
Shape is (N, M, 3), where N is the number of environments, and M is the number of target frames.
"""
target_rot_source: torch.Tensor = None
target_quat_source: torch.Tensor = None
"""Orientation of the target frame(s) relative to source frame quaternion (w, x, y, z).
Shape is (N, M, 4), where N is the number of environments, and M is the number of target frames.
"""
target_pos_w: torch.Tensor = None
"""Position of the target frame(s) after offset (in world frame).
Shape is (N, M, 3), where N is the number of environments, and M is the number of target frames.
"""
target_rot_w: torch.Tensor = None
target_quat_w: torch.Tensor = None
"""Orientation of the target frame(s) after offset (in world frame) quaternion (w, x, y, z).
Shape is (N, M, 4), where N is the number of environments, and M is the number of target frames.
"""
source_pos_w: torch.Tensor = None
"""Position of the source frame after offset (in world frame).
Shape is (N, 3), where N is the number of environments.
"""
source_rot_w: torch.Tensor = None
source_quat_w: torch.Tensor = None
"""Orientation of the source frame after offset (in world frame) quaternion (w, x, y, z).
Shape is (N, 4), where N is the number of environments.
"""
@property
def target_rot_source(self) -> torch.Tensor:
"""Alias for :attr:`target_quat_source`.
.. deprecated:: v0.2.1
Use :attr:`target_quat_source` instead. Will be removed in v0.3.0.
"""
warnings.warn("'target_rot_source' is deprecated, use 'target_quat_source' instead.", DeprecationWarning)
return self.target_quat_source
@property
def target_rot_w(self) -> torch.Tensor:
"""Alias for :attr:`target_quat_w`.
.. deprecated:: v0.2.1
Use :attr:`target_quat_w` instead. Will be removed in v0.3.0.
"""
warnings.warn("'target_rot_w' is deprecated, use 'target_quat_w' instead.", DeprecationWarning)
return self.target_quat_w
@property
def source_rot_w(self) -> torch.Tensor:
"""Alias for :attr:`source_quat_w`.
.. deprecated:: v0.2.1
Use :attr:`source_quat_w` instead. Will be removed in v0.3.0.
"""
warnings.warn("'source_rot_w' is deprecated, use 'source_quat_w' instead.", DeprecationWarning)
return self.source_quat_w
source/extensions/omni.isaac.orbit/test/sensors/check_frame_transformer.py deleted 100644 → 0
View file @ ea0ee24f
# Copyright (c) 2022-2024, The ORBIT Project Developers.
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
"""
This script checks the FrameTransformer sensor by visualizing the frames that it creates.
"""
from __future__ import annotations
"""Launch Isaac Sim Simulator first."""
import argparse
import math
from scipy.spatial.transform import Rotation
from omni.isaac.orbit.app import AppLauncher
# add argparse arguments
parser = argparse.ArgumentParser(
description="This script checks the FrameTransformer sensor by visualizing the frames that it creates."
)
parser.add_argument(
"--mode",
type=str,
choices=["feet", "all"],
default="all",
help="Dictates the the type of frames to use for FrameTransformer.",
)
parser.add_argument(
"--visualize",
action="store_true",
help=(
"Whether to enable FrameTransformer's debug_vis (True) or visualize each frame one at a"
" time and print to console (False)."
),
)
parser.add_argument("--num_envs", type=int, default=2, help="Number of environments to spawn.")
AppLauncher.add_app_launcher_args(parser)
args_cli = parser.parse_args()
# launch omniverse app
app_launcher = AppLauncher(headless=args_cli.headless)
simulation_app = app_launcher.app
"""Rest everything follows."""
import omni.isaac.orbit.sim as sim_utils
from omni.isaac.orbit.assets import AssetBaseCfg
from omni.isaac.orbit.markers import VisualizationMarkers
from omni.isaac.orbit.markers.config import FRAME_MARKER_CFG
from omni.isaac.orbit.scene import InteractiveScene, InteractiveSceneCfg
from omni.isaac.orbit.sensors import FrameTransformerCfg, OffsetCfg
from omni.isaac.orbit.sim import SimulationContext
from omni.isaac.orbit.terrains import TerrainImporterCfg
from omni.isaac.orbit.utils import configclass
from omni.isaac.orbit.utils.timer import Timer
##
# Pre-defined configs
##
from omni.isaac.orbit_assets.anymal import ANYMAL_C_CFG # isort:skip
def quat_from_euler_rpy(roll, pitch, yaw, degrees=False):
"""Converts Euler XYZ to Quaternion (w, x, y, z)."""
quat = Rotation.from_euler("xyz", (roll, pitch, yaw), degrees=degrees).as_quat()
return tuple(quat[[3, 0, 1, 2]].tolist())
def euler_rpy_apply(rpy, xyz, degrees=False):
"""Applies rotation from Euler XYZ on position vector."""
rot = Rotation.from_euler("xyz", rpy, degrees=degrees)
return tuple(rot.apply(xyz).tolist())
@configclass
class MySceneCfg(InteractiveSceneCfg):
"""Example scene configuration."""
# terrain - flat terrain plane
terrain = TerrainImporterCfg(
prim_path="/World/ground",
terrain_type="plane",
)
# articulation - robot
robot = ANYMAL_C_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")
if args_cli.mode == "feet":
# Example where only feet position frames are created
frame_transformer = FrameTransformerCfg(
prim_path="{ENV_REGEX_NS}/Robot/base",
target_frames=[
FrameTransformerCfg.FrameCfg(
name="LF_FOOT",
prim_path="{ENV_REGEX_NS}/Robot/LF_SHANK",
offset=OffsetCfg(
pos=euler_rpy_apply(rpy=(0, 0, -math.pi / 2), xyz=(0.08795, 0.01305, -0.33797)),
rot=quat_from_euler_rpy(0, 0, -math.pi / 2),
),
),
FrameTransformerCfg.FrameCfg(
name="RF_FOOT",
prim_path="{ENV_REGEX_NS}/Robot/RF_SHANK",
offset=OffsetCfg(
pos=euler_rpy_apply(rpy=(0, 0, math.pi / 2), xyz=(0.08795, -0.01305, -0.33797)),
rot=quat_from_euler_rpy(0, 0, math.pi / 2),
),
),
FrameTransformerCfg.FrameCfg(
name="LH_FOOT",
prim_path="{ENV_REGEX_NS}/Robot/LH_SHANK",
offset=OffsetCfg(
pos=euler_rpy_apply(rpy=(0, 0, -math.pi / 2), xyz=(-0.08795, 0.01305, -0.33797)),
rot=quat_from_euler_rpy(0, 0, -math.pi / 2),
),
),
FrameTransformerCfg.FrameCfg(
name="RH_FOOT",
prim_path="{ENV_REGEX_NS}/Robot/RH_SHANK",
offset=OffsetCfg(
pos=euler_rpy_apply(rpy=(0, 0, math.pi / 2), xyz=(-0.08795, -0.01305, -0.33797)),
rot=quat_from_euler_rpy(0, 0, math.pi / 2),
),
),
],
debug_vis=args_cli.visualize,
)
elif args_cli.mode == "all":
# Example using .* to get full body + LF_FOOT to ensure these work together
frame_transformer = FrameTransformerCfg(
prim_path="{ENV_REGEX_NS}/Robot/base",
target_frames=[
FrameTransformerCfg.FrameCfg(prim_path="{ENV_REGEX_NS}/Robot/.*"),
FrameTransformerCfg.FrameCfg(
prim_path="{ENV_REGEX_NS}/Robot/LF_SHANK",
name="LF_FOOT",
offset=OffsetCfg(
pos=euler_rpy_apply(rpy=(0, 0, -math.pi / 2), xyz=(0.08795, 0.01305, -0.33797)),
rot=quat_from_euler_rpy(0, 0, -math.pi / 2),
),
),
],
debug_vis=args_cli.visualize,
)
# extras - light
light = AssetBaseCfg(
prim_path="/World/light",
spawn=sim_utils.DistantLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)),
init_state=AssetBaseCfg.InitialStateCfg(pos=(0.0, 0.0, 500.0)),
)
def main():
"""Main function."""
# Load kit helper
sim = SimulationContext(sim_utils.SimulationCfg(dt=0.005))
# Set main camera
sim.set_camera_view(eye=[5, 5, 5], target=[0.0, 0.0, 0.0])
# Spawn things into stage
with Timer("Setup scene"):
scene = InteractiveScene(MySceneCfg(num_envs=args_cli.num_envs, env_spacing=5.0, lazy_sensor_update=False))
# Play the simulator
with Timer("Time taken to play the simulator"):
sim.reset()
# Now we are ready!
print("[INFO]: Setup complete...")
# default joint targets
robot_actions = scene.articulations["robot"].data.default_joint_pos.clone()
# Define simulation stepping
sim_dt = sim.get_physics_dt()
sim_time = 0.0
count = 0
# We only want one visualization at a time. This visualizer will be used
# to step through each frame so the user can verify that the correct frame
# is being visualized as the frame names are printing to console
if not args_cli.visualize:
cfg = FRAME_MARKER_CFG.replace(prim_path="/Visuals/FrameVisualizerFromScript")
cfg.markers["frame"].scale = (0.05, 0.05, 0.05)
transform_visualizer = VisualizationMarkers(cfg)
else:
transform_visualizer = None
frame_index = 0
# 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()
continue
# # reset
if count % 50 == 0:
# reset counters
sim_time = 0.0
count = 0
# reset root state
root_state = scene.articulations["robot"].data.default_root_state.clone()
root_state[:, :3] += scene.env_origins
joint_pos = scene.articulations["robot"].data.default_joint_pos
joint_vel = scene.articulations["robot"].data.default_joint_vel
# -- set root state
# -- robot
scene.articulations["robot"].write_root_state_to_sim(root_state)
scene.articulations["robot"].write_joint_state_to_sim(joint_pos, joint_vel)
root_state[:, 1] += 1.0
# reset buffers
scene.reset()
print(">>>>>>>> Reset!")
# perform this loop at policy control freq (50 Hz)
for _ in range(4):
# set joint targets
scene.articulations["robot"].set_joint_position_target(robot_actions)
# write data to sim
scene.write_data_to_sim()
# perform step
sim.step()
# read data from sim
scene.update(sim_dt)
# Change the frame that we are visualizing to ensure that frame names
# are correctly associated with the frames
if not args_cli.visualize:
if count % 50 == 0:
frame_names = scene["frame_transformer"].data.target_frame_names
frame_name = frame_names[frame_index]
print(f"Displaying {frame_index}: {frame_name}")
frame_index += 1
frame_index = frame_index % len(frame_names)
# visualize frame
pos = scene["frame_transformer"].data.target_pos_w[:, frame_index]
rot = scene["frame_transformer"].data.target_rot_w[:, frame_index]
transform_visualizer.visualize(pos, rot)
# update sim-time
sim_time += sim_dt * 4
count += 1
if __name__ == "__main__":
# Run the main function
main()
# Close the simulator
simulation_app.close()
source/standalone/environments/state_machine/lift_cube_sm.py
View file @ 620ce2b0
......@@ -256,7 +256,7 @@ def main():
# -- end-effector frame
ee_frame_sensor = env.unwrapped.scene["ee_frame"]
tcp_rest_position = ee_frame_sensor.data.target_pos_w[..., 0, :].clone() - env.unwrapped.scene.env_origins
tcp_rest_orientation = ee_frame_sensor.data.target_rot_w[..., 0, :].clone()
tcp_rest_orientation = ee_frame_sensor.data.target_quat_w[..., 0, :].clone()
# -- object frame
object_data: RigidObjectData = env.unwrapped.scene["object"].data
object_position = object_data.root_pos_w - env.unwrapped.scene.env_origins
......
source/standalone/tutorials/04_sensors/run_frame_transformer.py
View file @ 620ce2b0
......@@ -38,6 +38,8 @@ simulation_app = app_launcher.app
import math
import torch
import omni.isaac.debug_draw._debug_draw as omni_debug_draw
import omni.isaac.orbit.sim as sim_utils
import omni.isaac.orbit.utils.math as math_utils
from omni.isaac.orbit.assets import Articulation
......@@ -65,7 +67,7 @@ def define_sensor() -> FrameTransformer:
FrameTransformerCfg.FrameCfg(prim_path="/World/Robot/.*"),
FrameTransformerCfg.FrameCfg(
prim_path="/World/Robot/LF_SHANK",
name="LF_FOOT",
name="LF_FOOT_USER",
offset=OffsetCfg(pos=tuple(pos_offset.tolist()), rot=tuple(rot_offset[0].tolist())),
),
],
......@@ -113,8 +115,11 @@ def run_simulator(sim: sim_utils.SimulationContext, scene_entities: dict):
cfg = FRAME_MARKER_CFG.replace(prim_path="/Visuals/FrameVisualizerFromScript")
cfg.markers["frame"].scale = (0.1, 0.1, 0.1)
transform_visualizer = VisualizationMarkers(cfg)
# debug drawing for lines connecting the frame
draw_interface = omni_debug_draw.acquire_debug_draw_interface()
else:
transform_visualizer = None
draw_interface = None
frame_index = 0
# Simulate physics
......@@ -143,9 +148,20 @@ def run_simulator(sim: sim_utils.SimulationContext, scene_entities: dict):
frame_index = frame_index % len(frame_names)
# visualize frame
pos = frame_transformer.data.target_pos_w[:, frame_index]
rot = frame_transformer.data.target_rot_w[:, frame_index]
transform_visualizer.visualize(pos, rot)
source_pos = frame_transformer.data.source_pos_w
source_quat = frame_transformer.data.source_quat_w
target_pos = frame_transformer.data.target_pos_w[:, frame_index]
target_quat = frame_transformer.data.target_quat_w[:, frame_index]
# draw the frames
transform_visualizer.visualize(
torch.cat([source_pos, target_pos], dim=0), torch.cat([source_quat, target_quat], dim=0)
)
# draw the line connecting the frames
draw_interface.clear_lines()
# plain color for lines
lines_colors = [[1.0, 1.0, 0.0, 1.0]] * source_pos.shape[0]
line_thicknesses = [5.0] * source_pos.shape[0]
draw_interface.draw_lines(source_pos.tolist(), target_pos.tolist(), lines_colors, line_thicknesses)
def main():
......
source/standalone/tutorials/04_sensors/run_usd_camera.py
View file @ 620ce2b0
......@@ -59,9 +59,6 @@ from omni.isaac.orbit.sensors.camera import Camera, CameraCfg
from omni.isaac.orbit.utils import convert_dict_to_backend
from omni.isaac.orbit.utils.math import project_points, transform_points, unproject_depth
# Acquire draw interface
draw_interface = omni_debug_draw.acquire_debug_draw_interface()
def define_sensor() -> Camera:
"""Defines the camera sensor to add to the scene."""
......@@ -138,6 +135,9 @@ def run_simulator(sim: sim_utils.SimulationContext, scene_entities: dict):
output_dir = os.path.join(os.path.dirname(os.path.realpath(__file__)), "output", "camera")
rep_writer = rep.BasicWriter(output_dir=output_dir, frame_padding=3)
# Acquire draw interface
draw_interface = omni_debug_draw.acquire_debug_draw_interface()
# Set pose: There are two ways to set the pose of the camera.
# -- Option-1: Set pose using view
eyes = torch.tensor([[2.5, 2.5, 2.5], [-2.5, -2.5, 2.5]], device=sim.device)
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment