| |
| |
| |
| |
|
|
| from __future__ import annotations |
|
|
| from typing import TYPE_CHECKING |
|
|
| import torch |
|
|
| from isaaclab.assets import RigidObject |
| from isaaclab.managers import SceneEntityCfg |
| from isaaclab.utils.math import subtract_frame_transforms |
|
|
| if TYPE_CHECKING: |
| from isaaclab.envs import ManagerBasedRLEnv |
|
|
|
|
| def object_position_in_robot_root_frame( |
| env: ManagerBasedRLEnv, |
| robot_cfg: SceneEntityCfg = SceneEntityCfg("robot"), |
| object_cfg: SceneEntityCfg = SceneEntityCfg("object"), |
| ) -> torch.Tensor: |
| """The position of the object in the robot's root frame.""" |
| robot: RigidObject = env.scene[robot_cfg.name] |
| object: RigidObject = env.scene[object_cfg.name] |
| object_pos_w = object.data.root_pos_w[:, :3] |
| object_pos_b, _ = subtract_frame_transforms(robot.data.root_pos_w, robot.data.root_quat_w, object_pos_w) |
| return object_pos_b |
|
|
