Orbit DifferentialIKController for velocity control

Hello, I’m trying to use the Orbit framework to do velocity control on a Franka Panda arm end-effector. I saw this differential IK controller from the following tutorial, but the example isn’t using the provided relative mode: Using a task-space controller — orbit documentation

I tried to use the relative mode so that I could send velocity commands to the controller, instead of desired pose, and avoid dealing with quaternions. I started with adapting the example code and sending zero delta change of ee pose, but it shows in the simulation that the end-effector keeps moving although the zero delta change command was constantly sent.
Could someone maybe check my code and tell me where I got mistakes? Thank you!

Ps. As I could not find it, are there functions for the Franks arm to compute its forward and inverse kinematics, instead of a controller like DifferentialIKController? Like directly getting ee pose from the joint configuration and directly getting joint positions from the desired pose of end-effector.

from __future__ import annotations

"""Launch Isaac Sim Simulator first."""

import argparse

from omni.isaac.orbit.app import AppLauncher

# add argparse arguments
parser = argparse.ArgumentParser(description="Tutorial on using the differential IK controller.")
parser.add_argument("--robot", type=str, default="franka_panda", help="Name of the robot.")
parser.add_argument("--num_envs", type=int, default=1, help="Number of environments to spawn.")
# append AppLauncher cli args
# 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 torch
import traceback

import carb

import omni.isaac.orbit.sim as sim_utils
from omni.isaac.orbit.assets import AssetBaseCfg
from omni.isaac.orbit.controllers import DifferentialIKController, DifferentialIKControllerCfg
from omni.isaac.orbit.managers import SceneEntityCfg
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.utils import configclass
from omni.isaac.orbit.utils.assets import ISAAC_NUCLEUS_DIR
from omni.isaac.orbit.utils.math import subtract_frame_transforms

# Pre-defined configs
from omni.isaac.orbit_assets import FRANKA_PANDA_HIGH_PD_CFG, UR10_CFG  # isort:skip

class TableTopSceneCfg(InteractiveSceneCfg):
    """Configuration for a cart-pole scene."""

    # ground plane
    ground = AssetBaseCfg(
        init_state=AssetBaseCfg.InitialStateCfg(pos=(0.0, 0.0, -1.05)),

    # lights
    dome_light = AssetBaseCfg(
        prim_path="/World/Light", spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75))

    # mount
    table = AssetBaseCfg(
            usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/Stand/stand_instanceable.usd", scale=(5.0, 5.0, 2.0)
        # init_state=AssetBaseCfg.InitialStateCfg(pos=(2.0, 0.0, 0.0)),

    # articulation
    if args_cli.robot == "franka_panda":
        robot = FRANKA_PANDA_HIGH_PD_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")
    elif args_cli.robot == "ur10":
        robot = UR10_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")
        raise ValueError(f"Robot {args_cli.robot} is not supported. Valid: franka_panda, ur10")

# get robot pose in robot frame
def ee_pose_in_robot_frame(scene, robot, robot_entity_cfg) -> torch.Tensor:
    ee_pos_w, ee_quat_w = ee_pose_in_world_frame(scene, robot, robot_entity_cfg)
    root_pose_w = root_pose_in_world_frame(scene, robot, robot_entity_cfg)
    # joint_pos = robot.data.joint_pos[:, robot_entity_cfg.joint_ids]
    ee_pos_r, ee_quat_r = subtract_frame_transforms(
                    root_pose_w[:, 0:3], root_pose_w[:, 3:7], ee_pos_w, ee_quat_w
    return ee_pos_r, ee_quat_r

# get robot pose in world frame from simulation buffer
def ee_pose_in_world_frame(scene, robot, robot_entity_cfg) -> torch.Tensor:
    ee_pose_w = robot.data.body_state_w[:, robot_entity_cfg.body_ids[0], 0:7]
    ee_pos_w = ee_pose_w[:, 0:3]
    ee_quat_w = ee_pose_w[:, 3:7]
    # root_pose_w = robot.data.root_state_w[:, 0:7]
    return ee_pos_w, ee_quat_w

# get root pose in world frame from simulation buffer
def root_pose_in_world_frame(scene, robot, robot_entity_cfg) -> torch.Tensor:
    root_pose_w = robot.data.root_state_w[:, 0:7]
    return root_pose_w

def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene):
    """Runs the simulation loop."""
    # Extract scene entities
    # note: we only do this here for readability.
    robot = scene["robot"]

    # Create a differential IK controller
    diff_ik_cfg = DifferentialIKControllerCfg(command_type="pose", use_relative_mode=True, ik_method="dls")
    diff_ik_controller = DifferentialIKController(diff_ik_cfg, num_envs=scene.num_envs, device=sim.device)

    # Create a marker for the end-effector
    frame_marker_cfg = FRAME_MARKER_CFG.copy()
    frame_marker_cfg.markers["frame"].scale = (0.1, 0.1, 0.1)
    ee_marker = VisualizationMarkers(frame_marker_cfg.replace(prim_path="/Visuals/ee_current"))
    goal_marker = VisualizationMarkers(frame_marker_cfg.replace(prim_path="/Visuals/ee_goal"))

    # Specify robot-specific parameters
    if args_cli.robot == "franka_panda":
        robot_entity_cfg = SceneEntityCfg("robot", joint_names=["panda_joint.*"], body_names=["panda_hand"])
    elif args_cli.robot == "ur10":
        robot_entity_cfg = SceneEntityCfg("robot", joint_names=[".*"], body_names=["ee_link"])
        raise ValueError(f"Robot {args_cli.robot} is not supported. Valid: franka_panda, ur10") 
    # Resolving the scene entities
    # 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.
    if robot.is_fixed_base:
        ee_jacobi_idx = robot_entity_cfg.body_ids[0] - 1
        ee_jacobi_idx = robot_entity_cfg.body_ids[0]

    ik_commands = torch.tensor([[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]], device=sim.device)

    # Define simulation stepping
    sim_dt = sim.get_physics_dt()
    count = 0
    # Simulation loop
    while simulation_app.is_running():
        # reset
        if count % 150 == 0:
            # reset time
            count = 0
            # reset joint state
            joint_pos_default = robot.data.default_joint_pos.clone()
            joint_vel_default = robot.data.default_joint_vel.clone()
            robot.write_joint_state_to_sim(joint_pos_default, joint_vel_default)
            # reset actions
            # ik_commands = ee_vel_goal_zero
            joint_pos_des = joint_pos_default[:, robot_entity_cfg.joint_ids].clone()
            joint_vel_des = joint_vel_default[:, robot_entity_cfg.joint_ids].clone()
            # joint_vel_des = torch.zeros((scene.num_envs, 7), device=sim.device)
            joint_pos = robot.data.joint_pos[:, robot_entity_cfg.joint_ids]
            ee_pos_b, ee_quat_b = ee_pose_in_world_frame(scene, robot, robot_entity_cfg)
            # reset controller
            diff_ik_controller.set_command(ik_commands, ee_pos= ee_pos_b, ee_quat = ee_quat_b) # delta change of pose
            # obtain quantities from simulation
            jacobian = robot.root_physx_view.get_jacobians()[:, ee_jacobi_idx, :, robot_entity_cfg.joint_ids]
            ee_pos_b, ee_quat_b = ee_pose_in_world_frame(scene, robot, robot_entity_cfg)
            diff_ik_controller.set_command(ik_commands, ee_pos= ee_pos_b, ee_quat = ee_quat_b) # delta change of pose
            # compute the joint commands
            joint_pos = robot.data.joint_pos[:, robot_entity_cfg.joint_ids]
            joint_pos_des = diff_ik_controller.compute(ee_pos_b, ee_quat_b, jacobian, joint_pos)
            joint_vel_des = (joint_pos_des - joint_pos) / sim_dt
        # apply actions
        robot.set_joint_velocity_target(joint_vel_des, joint_ids=robot_entity_cfg.joint_ids, env_ids = list(range(scene.num_envs)))
        # perform step
        # update sim-time
        count += 1
        # update buffers
        # obtain quantities from simulation
        ee_pose_w = robot.data.body_state_w[:, robot_entity_cfg.body_ids[0], 0:7]
        # update marker positions
        ee_marker.visualize(ee_pose_w[:, 0:3], ee_pose_w[:, 3:7])
        # goal_marker.visualize(ee_pos_ref, ee_quat_ref)

def main(): 
    # Load kit helper
    sim_cfg = sim_utils.SimulationCfg(dt=0.01)
    sim = sim_utils.SimulationContext(sim_cfg)
    # Set main camera
    sim.set_camera_view([2.5, 2.5, 2.5], [0.0, 0.0, 0.0])
    # Design scene
    scene_cfg = TableTopSceneCfg(num_envs=args_cli.num_envs, env_spacing=2.0)
    scene = InteractiveScene(scene_cfg)

    robot = scene["robot"]

    # Play the simulator
    # Now we are ready!
    print("[INFO]: Setup complete...")
    # Run the simulator
    run_simulator(sim, scene)

if __name__ == "__main__":
        # run the main execution
    except Exception as err:
        #close sim app