[Bug Report] Ik Absolute seems to not work properly with rotated base frame

[zoctipus]

Describe the bug

Isaac-Lift-Cube-Franka-IK-Abs-v0 with robot based rotated 90 degree, step constant action in robot base frame

The Frame indicates the visualization of target pose

Isaac-Lift-Cube-Franka-IK-Abs-v0 with no robot based rotation, step constant action in robot base frame

The Frame indicates the visualization of target pose

As you can see from the gif, the rotation seems to cause the robot unable to track the target pose stably.

Steps to reproduce

I wrote below code that reproduce the bug, uncomment the denoted section to rotate robot

python source/standalone/environments/position_follow_agent.py --task Isaac-Lift-Cube-Franka-IK-Abs-v0 --num_envs 1

import argparse

from omni.isaac.lab.app import AppLauncher
# add argparse arguments
parser = argparse.ArgumentParser(description="Random agent for Isaac Lab environments.")
parser.add_argument(
    "--disable_fabric", action="store_true", default=False, help="Disable fabric and use USD I/O operations."
)
parser.add_argument("--num_envs", type=int, default=None, help="Number of environments to simulate.")
parser.add_argument("--task", type=str, default=None, help="Name of the task.")
# append AppLauncher cli args
AppLauncher.add_app_launcher_args(parser)
# parse the arguments
args_cli = parser.parse_args()

# launch omniverse app
app_launcher = AppLauncher(args_cli)
simulation_app = app_launcher.app

"""Rest everything follows."""

import gymnasium as gym
import torch

import omni.isaac.lab_tasks  # noqa: F401
from omni.isaac.lab_tasks.utils import parse_env_cfg
from omni.isaac.lab.utils.math import combine_frame_transforms
from omni.isaac.lab.markers import VisualizationMarkers
from omni.isaac.lab.markers.config import FRAME_MARKER_CFG


def main():
    """Random actions agent with Isaac Lab environment."""
    # create environment configuration
    env_cfg = parse_env_cfg(
        args_cli.task, device=args_cli.device, num_envs=args_cli.num_envs, use_fabric=not args_cli.disable_fabric
    )
    # env_cfg.scene.robot.init_state.rot=(0.70711, 0, 0, -0.70711)  ----------------------Uncomment to rotate 90 degree
    # create environment
    env = gym.make(args_cli.task, cfg=env_cfg)
    # print info (this is vectorized environment)
    print(f"[INFO]: Gym observation space: {env.observation_space}")
    print(f"[INFO]: Gym action space: {env.action_space}")
    # reset environment
    env.reset()
    env.unwrapped.sim.step(render=True)
    env.unwrapped.scene.update(dt=env.unwrapped.physics_dt)
    cur_ee_pose = env.unwrapped.action_manager.get_term("arm_action")._compute_frame_pose()
    frame_marker_cfg = FRAME_MARKER_CFG.copy()  # type: ignore
    frame_marker_cfg.markers["frame"].scale = (0.05, 0.05, 0.05)
    goal_marker = VisualizationMarkers(frame_marker_cfg.replace(prim_path="/Visuals/ee_goal"))
    # simulate environment
    actions = torch.zeros(env.unwrapped.action_space.shape, device=env.unwrapped.device)
    while simulation_app.is_running():
        # run everything in inference mode
        with torch.inference_mode():
            # sample actions from -1 to 1
            actions[:, :7] = torch.cat(cur_ee_pose, dim=1)
            
            world_pos, world_quat = combine_frame_transforms(
                env.unwrapped.scene['robot'].data.root_pos_w, env.unwrapped.scene['robot'].data.root_quat_w, actions[:, :3], actions[:, 3:7]
            )
            goal_marker.visualize(world_pos + env.unwrapped.scene.env_origins, world_quat)
            # apply actions
            env.step(actions)

    # close the simulator
    env.close()


if __name__ == "__main__":
    # run the main function
    main()
    # close sim app
    simulation_app.close()

System Info

Describe the characteristic of your environment:

• Commit: 7452386
• Isaac Sim Version: 4.1
• OS: 20.04
• GPU: 4090
• CUDA: 12.5
• GPU Driver: 555.42.02

Additional context

I verified in the code until compute_delta_jointpos in ik_differential.py where the pos_error and quat_error are both close to [0, 0, 0] and [1, 0, 0, 0] when the environment just starts, indicating the target position is implemented correctly, but rotated franka will just drift away

Checklist

• [X] I have checked that there is no similar issue in the repo (required)
• [ ] I have checked that the issue is not in running Isaac Sim itself and is related to the repo

Acceptance Criteria

Add the criteria for which this task is considered done. If not known at issue creation time, you can add this once the issue is assigned.

• [ ] Ik Abs can track any reachable point whether or not base rotates