[Question] Quadruped Robot IK Test: Not All Legs Are Responding

[piggaycheng]

Hello,

I'm trying to apply inverse kinematics (IK) to a quadruped robot to get the target joint angles for each leg.

I've referenced the run_diff_ik.py program. I created a SceneEntityCfg for each leg of the quadruped and used a fixed command to perform a back-and-forth swing test, using a diff IK controller to calculate the target angles for each leg.

However, the results aren't what I expected. Leaving aside whether the calculated angles are correct, the most obvious issue is that not all legs are moving.

Here is my source code. Can anyone help me with this?

"""
This script demonstrates how to use the differential inverse kinematics controller with the simulator.

The differential IK controller can be configured in different modes. It uses the Jacobians computed by
PhysX. This helps perform parallelized computation of the inverse kinematics.

.. code-block:: bash

    # Usage
    ./isaaclab.sh -p scripts/tutorials/05_controllers/run_diff_ik.py

"""

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

import argparse

from isaaclab.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=128, help="Number of environments to spawn.")
# 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 torch

import isaaclab.sim as sim_utils
from isaaclab.assets import AssetBaseCfg
from isaaclab.controllers import DifferentialIKController, DifferentialIKControllerCfg
from isaaclab.managers import SceneEntityCfg
from isaaclab.markers import VisualizationMarkers
from isaaclab.markers.config import FRAME_MARKER_CFG
from isaaclab.scene import InteractiveScene, InteractiveSceneCfg
from isaaclab.utils import configclass
from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR
from isaaclab.utils.math import subtract_frame_transforms

##
# Pre-defined configs
##
from isaaclab_assets import UNITREE_GO2_CFG
from isaaclab.actuators import DCMotorCfg
from isaaclab.assets.articulation import ArticulationCfg


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

    # ground plane
    ground = AssetBaseCfg(
        prim_path="/World/defaultGroundPlane",
        spawn=sim_utils.GroundPlaneCfg(),
        init_state=AssetBaseCfg.InitialStateCfg(pos=(0.0, 0.0, 0.0)),
    )

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

    # articulation
    robot = UNITREE_GO2_CFG.replace(
        prim_path="{ENV_REGEX_NS}/Robot",
        actuators={
            "base_legs": DCMotorCfg(
                joint_names_expr=[".*_hip_joint", ".*_thigh_joint", ".*_calf_joint"],
                effort_limit=23.5,
                saturation_effort=23.5,
                velocity_limit=30.0,
                stiffness=55.0,
                damping=0.5,
                friction=0.0,
            )
        },
    )


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 controller
    fl_diff_ik_cfg = DifferentialIKControllerCfg(command_type="position", use_relative_mode=True, ik_method="dls")
    fr_diff_ik_cfg = DifferentialIKControllerCfg(command_type="position", use_relative_mode=True, ik_method="dls")
    rl_diff_ik_cfg = DifferentialIKControllerCfg(command_type="position", use_relative_mode=True, ik_method="dls")
    rr_diff_ik_cfg = DifferentialIKControllerCfg(command_type="position", use_relative_mode=True, ik_method="dls")
    fl_diff_ik_controller = DifferentialIKController(fl_diff_ik_cfg, num_envs=scene.num_envs, device=sim.device)
    fr_diff_ik_controller = DifferentialIKController(fr_diff_ik_cfg, num_envs=scene.num_envs, device=sim.device)
    rl_diff_ik_controller = DifferentialIKController(rl_diff_ik_cfg, num_envs=scene.num_envs, device=sim.device)
    rr_diff_ik_controller = DifferentialIKController(rr_diff_ik_cfg, num_envs=scene.num_envs, device=sim.device)

    # # Markers
    # 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"))

    # Define goals for the arm
    ee_goals = [
        [0.0, 0.0, 0.05],
        [0.0, 0.0, -0.05],
    ]
    ee_goals = torch.tensor(ee_goals, device=sim.device)
    # Track the given command
    current_goal_idx = 0
    # Create buffers to store actions
    ik_commands = torch.zeros(scene.num_envs, fr_diff_ik_controller.action_dim, device=robot.device)
    ik_commands[:] = ee_goals[current_goal_idx]

    # Specify robot-specific parameters
    fl_robot_entity_cfg = SceneEntityCfg("robot", joint_names=["FL_.*"], body_names=["FL_foot"])
    fr_robot_entity_cfg = SceneEntityCfg("robot", joint_names=["FR_.*"], body_names=["FR_foot"])
    rl_robot_entity_cfg = SceneEntityCfg("robot", joint_names=["RL_.*"], body_names=["RL_foot"])
    rr_robot_entity_cfg = SceneEntityCfg("robot", joint_names=["RR_.*"], body_names=["RR_foot"])

    # Resolving the scene entities
    fl_robot_entity_cfg.resolve(scene)
    fr_robot_entity_cfg.resolve(scene)
    rl_robot_entity_cfg.resolve(scene)
    rr_robot_entity_cfg.resolve(scene)

    # 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:
        fl_ee_jacobi_idx = fl_robot_entity_cfg.body_ids[0] - 1
        fr_ee_jacobi_idx = fr_robot_entity_cfg.body_ids[0] - 1
        rl_ee_jacobi_idx = rl_robot_entity_cfg.body_ids[0] - 1
        rr_ee_jacobi_idx = rr_robot_entity_cfg.body_ids[0] - 1
    else:
        fl_ee_jacobi_idx = fl_robot_entity_cfg.body_ids[0]
        fr_ee_jacobi_idx = fr_robot_entity_cfg.body_ids[0]
        rl_ee_jacobi_idx = rl_robot_entity_cfg.body_ids[0]
        rr_ee_jacobi_idx = rr_robot_entity_cfg.body_ids[0]

    # Define simulation stepping
    sim_dt = sim.get_physics_dt()
    count = 0

    joint_pos = robot.data.default_joint_pos.clone()
    joint_vel = robot.data.default_joint_vel.clone()
    robot.write_joint_state_to_sim(joint_pos, joint_vel)

    # Simulation loop
    while simulation_app.is_running():
        # FL_foot
        fl_ee_pose_w = robot.data.body_pose_w[:, fl_robot_entity_cfg.body_ids[0]]
        fl_root_pose_w = robot.data.root_pose_w
        fl_ee_pos_b, fl_ee_quat_b = subtract_frame_transforms(
            fl_root_pose_w[:, 0:3], fl_root_pose_w[:, 3:7], fl_ee_pose_w[:, 0:3], fl_ee_pose_w[:, 3:7]
        )
        # FR_foot
        fr_ee_pose_w = robot.data.body_pose_w[:, fr_robot_entity_cfg.body_ids[0]]
        fr_root_pose_w = robot.data.root_pose_w
        fr_ee_pos_b, fr_ee_quat_b = subtract_frame_transforms(
            fr_root_pose_w[:, 0:3], fr_root_pose_w[:, 3:7], fr_ee_pose_w[:, 0:3], fr_ee_pose_w[:, 3:7]
        )
        # RL_foot
        rl_ee_pose_w = robot.data.body_pose_w[:, rl_robot_entity_cfg.body_ids[0]]
        rl_root_pose_w = robot.data.root_pose_w
        rl_ee_pos_b, rl_ee_quat_b = subtract_frame_transforms(
            rl_root_pose_w[:, 0:3], rl_root_pose_w[:, 3:7], rl_ee_pose_w[:, 0:3], rl_ee_pose_w[:, 3:7]
        )
        # RR_foot
        rr_ee_pose_w = robot.data.body_pose_w[:, rr_robot_entity_cfg.body_ids[0]]
        rr_root_pose_w = robot.data.root_pose_w
        rr_ee_pos_b, rr_ee_quat_b = subtract_frame_transforms(
            rr_root_pose_w[:, 0:3], rr_root_pose_w[:, 3:7], rr_ee_pose_w[:, 0:3], rr_ee_pose_w[:, 3:7]
        )

        if count % 150 == 0:
            ik_commands[:] = ee_goals[current_goal_idx]
            fl_diff_ik_controller.reset()
            fr_diff_ik_controller.reset()
            rl_diff_ik_controller.reset()
            rr_diff_ik_controller.reset()

            count = 0
            current_goal_idx = (current_goal_idx + 1) % len(ee_goals)

        full_jacobian = robot.root_physx_view.get_jacobians()
        fl_jacobian = full_jacobian[:, fl_ee_jacobi_idx, :, fl_robot_entity_cfg.joint_ids]
        fr_jacobian = full_jacobian[:, fr_ee_jacobi_idx, :, fr_robot_entity_cfg.joint_ids]
        rl_jacobian = full_jacobian[:, rl_ee_jacobi_idx, :, rl_robot_entity_cfg.joint_ids]
        rr_jacobian = full_jacobian[:, rr_ee_jacobi_idx, :, rr_robot_entity_cfg.joint_ids]

        fl_joint_pos = robot.data.joint_pos[:, fl_robot_entity_cfg.joint_ids]
        fr_joint_pos = robot.data.joint_pos[:, fr_robot_entity_cfg.joint_ids]
        rl_joint_pos = robot.data.joint_pos[:, rl_robot_entity_cfg.joint_ids]
        rr_joint_pos = robot.data.joint_pos[:, rr_robot_entity_cfg.joint_ids]

        fl_diff_ik_controller.set_command(ik_commands, fl_ee_pos_b, fl_ee_quat_b)
        fr_diff_ik_controller.set_command(ik_commands, fr_ee_pos_b, fr_ee_quat_b)
        rl_diff_ik_controller.set_command(ik_commands, rl_ee_pos_b, rl_ee_quat_b)
        rr_diff_ik_controller.set_command(ik_commands, rr_ee_pos_b, rr_ee_quat_b)

        fl_joint_pos_des = fl_diff_ik_controller.compute(fl_ee_pos_b, fl_ee_quat_b, fl_jacobian, fl_joint_pos)
        fr_joint_pos_des = fr_diff_ik_controller.compute(fr_ee_pos_b, fr_ee_quat_b, fr_jacobian, fr_joint_pos)
        rl_joint_pos_des = rl_diff_ik_controller.compute(rl_ee_pos_b, rl_ee_quat_b, rl_jacobian, rl_joint_pos)
        rr_joint_pos_des = rr_diff_ik_controller.compute(rr_ee_pos_b, rr_ee_quat_b, rr_jacobian, rr_joint_pos)

        # apply actions
        robot.set_joint_position_target(fl_joint_pos_des, joint_ids=fl_robot_entity_cfg.joint_ids)
        robot.set_joint_position_target(fr_joint_pos_des, joint_ids=fr_robot_entity_cfg.joint_ids)
        robot.set_joint_position_target(rl_joint_pos_des, joint_ids=rl_robot_entity_cfg.joint_ids)
        robot.set_joint_position_target(rr_joint_pos_des, joint_ids=rr_robot_entity_cfg.joint_ids)
        scene.write_data_to_sim()
        # perform step
        sim.step()
        # update sim-time
        count += 1
        # update buffers
        scene.update(sim_dt)

        # 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])
        # Visualize goal marker at target position with identity quaternion orientation
        # goal_marker.visualize(
        #     ik_commands[:, :3] + scene.env_origins,
        #     torch.tensor([1.0, 0.0, 0.0, 0.0], device=ik_commands.device).expand(ik_commands.shape[0], 4)
        # )


def main():
    """Main function."""
    # Load kit helper
    sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device)
    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)
    # Play the simulator
    sim.reset()
    # Now we are ready!
    print("[INFO]: Setup complete...")
    # Run the simulator
    run_simulator(sim, scene)


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

[RandomOakForest]

Thank you for posting this. The most likely reason not all legs of the quadruped are moving in this script is an issue with how the joint and body names are defined in the SceneEntityCfg and subsequently resolved to indices for each leg. When using regular expressions (such as "FL_.*" for joint_names), if the regex does not uniquely match exactly the three joints per leg (hip, thigh, calf), it may be resolving to an empty list or the wrong set of joint IDs for some legs. I will move this post to our Discussions section for follow up. Let us know if you make any inroads into resolving this.

[CAI23sbP]

@piggaycheng @RandomOakForest
Hi, I have modified this code, see below.

"""
This script demonstrates how to use the differential inverse kinematics controller with the simulator.

The differential IK controller can be configured in different modes. It uses the Jacobians computed by
PhysX. This helps perform parallelized computation of the inverse kinematics.

.. code-block:: bash

    # Usage
    ./isaaclab.sh -p scripts/tutorials/05_controllers/run_diff_ik.py

"""

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

import argparse

from isaaclab.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=128, help="Number of environments to spawn.")
# 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 torch

import isaaclab.sim as sim_utils
from isaaclab.assets import AssetBaseCfg
from isaaclab.controllers import DifferentialIKController, DifferentialIKControllerCfg
from isaaclab.managers import SceneEntityCfg
from isaaclab.markers import VisualizationMarkers
from isaaclab.markers.config import FRAME_MARKER_CFG
from isaaclab.scene import InteractiveScene, InteractiveSceneCfg
from isaaclab.utils import configclass
from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR
from isaaclab.utils.math import subtract_frame_transforms

##
# Pre-defined configs
##
from isaaclab_assets import UNITREE_GO2_CFG
from isaaclab.actuators import DCMotorCfg
from isaaclab.assets.articulation import ArticulationCfg


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

    # ground plane
    ground = AssetBaseCfg(
        prim_path="/World/defaultGroundPlane",
        spawn=sim_utils.GroundPlaneCfg(),
        init_state=AssetBaseCfg.InitialStateCfg(pos=(0.0, 0.0, 0.0)),
    )

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

    # articulation
    robot = UNITREE_GO2_CFG.replace(
        prim_path="{ENV_REGEX_NS}/Robot",
        actuators={
            "base_legs": DCMotorCfg(
                joint_names_expr=[".*_hip_joint", ".*_thigh_joint", ".*_calf_joint"],
                effort_limit=23.5,
                saturation_effort=23.5,
                velocity_limit=30.0,
                stiffness=55.0,
                damping=0.5,
                friction=0.0,
            )
        },
    )


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 controller
    fl_diff_ik_cfg = DifferentialIKControllerCfg(command_type="position", use_relative_mode=True, ik_method="dls")
    fr_diff_ik_cfg = DifferentialIKControllerCfg(command_type="position", use_relative_mode=True, ik_method="dls")
    rl_diff_ik_cfg = DifferentialIKControllerCfg(command_type="position", use_relative_mode=True, ik_method="dls")
    rr_diff_ik_cfg = DifferentialIKControllerCfg(command_type="position", use_relative_mode=True, ik_method="dls")
    fl_diff_ik_controller = DifferentialIKController(fl_diff_ik_cfg, num_envs=scene.num_envs, device=sim.device)
    fr_diff_ik_controller = DifferentialIKController(fr_diff_ik_cfg, num_envs=scene.num_envs, device=sim.device)
    rl_diff_ik_controller = DifferentialIKController(rl_diff_ik_cfg, num_envs=scene.num_envs, device=sim.device)
    rr_diff_ik_controller = DifferentialIKController(rr_diff_ik_cfg, num_envs=scene.num_envs, device=sim.device)
    frame_marker_cfg = FRAME_MARKER_CFG.copy()
    fl_ee_marker = VisualizationMarkers(frame_marker_cfg.replace(prim_path="/Visuals/fl_ee_current"))
    fr_ee_marker = VisualizationMarkers(frame_marker_cfg.replace(prim_path="/Visuals/fr_ee_current"))
    rl_ee_marker = VisualizationMarkers(frame_marker_cfg.replace(prim_path="/Visuals/rl_ee_current"))
    rr_ee_marker = VisualizationMarkers(frame_marker_cfg.replace(prim_path="/Visuals/rr_ee_current"))
    fl_goal_marker = VisualizationMarkers(frame_marker_cfg.replace(prim_path="/Visuals/fl_ee_goal"))
    fr_goal_marker = VisualizationMarkers(frame_marker_cfg.replace(prim_path="/Visuals/fr_ee_goal"))
    rl_goal_marker = VisualizationMarkers(frame_marker_cfg.replace(prim_path="/Visuals/rl_ee_goal"))
    rr_goal_marker = VisualizationMarkers(frame_marker_cfg.replace(prim_path="/Visuals/rr_ee_goal"))

    # Define goals for the arm
    fl_ee_goals = [[0.26, 0.1, -0.25], [0.24909090909090909, 0.1, -0.25], [0.2381818181818182, 0.1, -0.25], [0.2272727272727273, 0.1, -0.25], [0.21636363636363637, 0.1, -0.25]]
    fr_ee_goals = [[0.15090909090909094, 0.1, -0.25], [0.14, 0.1, -0.25], [0.14, 0.1, -0.25], [0.15714285714285714, 0.1, -0.2283058130441221], [0.1742857142857143, 0.1, -0.2109084258765985]]
    rl_ee_goals =  [[-0.24909090909090909, -0.1, -0.25], [-0.26, -0.1, -0.25], [-0.26, -0.1, -0.25], [-0.24285714285714288, -0.1, -0.2283058130441221], [-0.22571428571428573, -0.1, -0.2109084258765985]]
    rr_ee_goals = [[-0.14, -0.1, -0.25], [-0.15090909090909094, -0.1, -0.25], [-0.16181818181818183, -0.1, -0.25], [-0.17272727272727273, -0.1, -0.25], [-0.18363636363636365, -0.1, -0.25]]
  
    fl_ee_goals = torch.tensor(fl_ee_goals, device=sim.device)
    fr_ee_goals = torch.tensor(fr_ee_goals, device=sim.device)
    rl_ee_goals = torch.tensor(rl_ee_goals, device=sim.device)
    rr_ee_goals = torch.tensor(rr_ee_goals, device=sim.device)
    # Track the given command
    current_goal_idx = 0
    # Create buffers to store actions
    fl_ik_commands = torch.zeros(scene.num_envs, fl_diff_ik_controller.action_dim, device=robot.device)
    fl_ik_commands[:] = fl_ee_goals[current_goal_idx]
    fr_ik_commands = torch.zeros(scene.num_envs, fl_diff_ik_controller.action_dim, device=robot.device)
    fr_ik_commands[:] = fr_ee_goals[current_goal_idx]
    rl_ik_commands = torch.zeros(scene.num_envs, fl_diff_ik_controller.action_dim, device=robot.device)
    rl_ik_commands[:] = rl_ee_goals[current_goal_idx]
    rr_ik_commands = torch.zeros(scene.num_envs, fl_diff_ik_controller.action_dim, device=robot.device)
    rr_ik_commands[:] = rr_ee_goals[current_goal_idx]

    # Specify robot-specific parameters
    fl_robot_entity_cfg = SceneEntityCfg("robot", joint_names=["FL_.*"], body_names=["FL_foot"])
    fr_robot_entity_cfg = SceneEntityCfg("robot", joint_names=["FR_.*"], body_names=["FR_foot"])
    rl_robot_entity_cfg = SceneEntityCfg("robot", joint_names=["RL_.*"], body_names=["RL_foot"])
    rr_robot_entity_cfg = SceneEntityCfg("robot", joint_names=["RR_.*"], body_names=["RR_foot"])

    # Resolving the scene entities
    fl_robot_entity_cfg.resolve(scene)
    fr_robot_entity_cfg.resolve(scene)
    rl_robot_entity_cfg.resolve(scene)
    rr_robot_entity_cfg.resolve(scene)

    fl_ee_jacobi_idx = fl_robot_entity_cfg.body_ids[0]
    fr_ee_jacobi_idx = fr_robot_entity_cfg.body_ids[0]
    rl_ee_jacobi_idx = rl_robot_entity_cfg.body_ids[0]
    rr_ee_jacobi_idx = rr_robot_entity_cfg.body_ids[0]

    # Define simulation stepping
    sim_dt = sim.get_physics_dt()
    count = 0

    joint_pos = robot.data.default_joint_pos.clone()
    joint_vel = robot.data.default_joint_vel.clone()
    robot.write_joint_state_to_sim(joint_pos, joint_vel)
    count = 0

    # Simulation loop
    while simulation_app.is_running():
        if count % 150 == 0:
            count = 0
            joint_pos = robot.data.default_joint_pos.clone()
            joint_vel = robot.data.default_joint_vel.clone()
            robot.write_joint_state_to_sim(joint_pos, joint_vel)
            robot.reset()
            # reset actions
            fl_ik_commands[:] = fl_ee_goals[current_goal_idx]
            fr_ik_commands[:] = fr_ee_goals[current_goal_idx]
            rl_ik_commands[:] = rl_ee_goals[current_goal_idx]
            rr_ik_commands[:] = rr_ee_goals[current_goal_idx]
            fl_joint_pos_des = joint_pos[:, fl_robot_entity_cfg.joint_ids].clone()
            # reset controller
            fl_ee_pose_w = robot.data.body_state_w[:, fl_robot_entity_cfg.body_ids[0], 0:7]
            fl_root_pose_w = robot.data.root_state_w[:, 0:7]
            fl_ee_pos_b, fl_ee_quat_b = subtract_frame_transforms(
                fl_root_pose_w[:, 0:3], fl_root_pose_w[:, 3:7], fl_ee_pose_w[:, 0:3], fl_ee_pose_w[:, 3:7]
            )


            fr_joint_pos_des = joint_pos[:, fr_robot_entity_cfg.joint_ids].clone()
            # reset controller
            fr_ee_pose_w = robot.data.body_state_w[:, fr_robot_entity_cfg.body_ids[0], 0:7]
            fr_root_pose_w = robot.data.root_state_w[:, 0:7]
            fr_ee_pos_b, fr_ee_quat_b = subtract_frame_transforms(
                fr_root_pose_w[:, 0:3], fr_root_pose_w[:, 3:7], fr_ee_pose_w[:, 0:3], fr_ee_pose_w[:, 3:7]
            )

            rl_joint_pos_des = joint_pos[:, rl_robot_entity_cfg.joint_ids].clone()
            # reset controller
            rl_ee_pose_w = robot.data.body_state_w[:, rl_robot_entity_cfg.body_ids[0], 0:7]
            rl_root_pose_w = robot.data.root_state_w[:, 0:7]
            rl_ee_pos_b, rl_ee_quat_b = subtract_frame_transforms(
                rl_root_pose_w[:, 0:3], rl_root_pose_w[:, 3:7], rl_ee_pose_w[:, 0:3], rl_ee_pose_w[:, 3:7]
            )

            
            rr_joint_pos_des = joint_pos[:, rr_robot_entity_cfg.joint_ids].clone()
            # reset controller
            rr_ee_pose_w = robot.data.body_state_w[:, rr_robot_entity_cfg.body_ids[0], 0:7]
            rr_root_pose_w = robot.data.root_state_w[:, 0:7]
            rr_ee_pos_b, rr_ee_quat_b = subtract_frame_transforms(
                rr_root_pose_w[:, 0:3], rr_root_pose_w[:, 3:7], rr_ee_pose_w[:, 0:3], rr_ee_pose_w[:, 3:7]
            )

            fl_diff_ik_controller.reset()
            fl_diff_ik_controller.set_command(fl_ik_commands, fl_ee_pos_b, fl_ee_quat_b)
            fr_diff_ik_controller.reset()
            fr_diff_ik_controller.set_command(fr_ik_commands, fr_ee_pos_b, fr_ee_quat_b)
            rl_diff_ik_controller.reset()
            rl_diff_ik_controller.set_command(rl_ik_commands, rl_ee_pos_b, rl_ee_quat_b)
            rr_diff_ik_controller.reset()
            rr_diff_ik_controller.set_command(rr_ik_commands, rr_ee_pos_b, rr_ee_quat_b)


            # change goal
            current_goal_idx = (current_goal_idx + 1) % len(fl_ee_goals)
        else:
            # obtain quantities from simulation
            fl_jacobian = robot.root_physx_view.get_jacobians()[:, fl_ee_jacobi_idx, :, fl_robot_entity_cfg.joint_ids]
            fl_ee_pose_w = robot.data.body_state_w[:, fl_robot_entity_cfg.body_ids[0], 0:7]
            fl_root_pose_w = robot.data.root_state_w[:, 0:7]
            fl_joint_pos = robot.data.joint_pos[:, fl_robot_entity_cfg.joint_ids]
            # compute frame in root frame
            fl_ee_pos_b, fl_ee_quat_b = subtract_frame_transforms(
                fl_root_pose_w[:, 0:3], fl_root_pose_w[:, 3:7], fl_ee_pose_w[:, 0:3], fl_ee_pose_w[:, 3:7]
            )

            fr_jacobian = robot.root_physx_view.get_jacobians()[:, fr_ee_jacobi_idx, :, fr_robot_entity_cfg.joint_ids]
            fr_ee_pose_w = robot.data.body_state_w[:, fr_robot_entity_cfg.body_ids[0], 0:7]
            fr_root_pose_w = robot.data.root_state_w[:, 0:7]
            fr_joint_pos = robot.data.joint_pos[:, fr_robot_entity_cfg.joint_ids]
            # compute frame in root frame
            fr_ee_pos_b, fr_ee_quat_b = subtract_frame_transforms(
                fr_root_pose_w[:, 0:3], fr_root_pose_w[:, 3:7], fr_ee_pose_w[:, 0:3], fr_ee_pose_w[:, 3:7]
            )

            rl_jacobian = robot.root_physx_view.get_jacobians()[:, rl_ee_jacobi_idx, :, rl_robot_entity_cfg.joint_ids]
            rl_ee_pose_w = robot.data.body_state_w[:, rl_robot_entity_cfg.body_ids[0], 0:7]
            rl_root_pose_w = robot.data.root_state_w[:, 0:7]
            rl_joint_pos = robot.data.joint_pos[:, rl_robot_entity_cfg.joint_ids]
            # compute frame in root frame
            rl_ee_pos_b, rl_ee_quat_b = subtract_frame_transforms(
                rl_root_pose_w[:, 0:3], rl_root_pose_w[:, 3:7], rl_ee_pose_w[:, 0:3], rl_ee_pose_w[:, 3:7]
            )

            rr_jacobian = robot.root_physx_view.get_jacobians()[:, rr_ee_jacobi_idx, :, rr_robot_entity_cfg.joint_ids]
            rr_ee_pose_w = robot.data.body_state_w[:, rr_robot_entity_cfg.body_ids[0], 0:7]
            rr_root_pose_w = robot.data.root_state_w[:, 0:7]
            rr_joint_pos = robot.data.joint_pos[:, rr_robot_entity_cfg.joint_ids]
            # compute frame in root frame
            rr_ee_pos_b, rr_ee_quat_b = subtract_frame_transforms(
                rr_root_pose_w[:, 0:3], rr_root_pose_w[:, 3:7], rr_ee_pose_w[:, 0:3], rr_ee_pose_w[:, 3:7]
            )
            # compute the joint commands
            fl_joint_pos_des = fl_diff_ik_controller.compute(fl_ee_pos_b, fl_ee_quat_b, fl_jacobian, fl_joint_pos)

            # compute the joint commands
            fr_joint_pos_des = fr_diff_ik_controller.compute(fr_ee_pos_b, fr_ee_quat_b, fr_jacobian, fr_joint_pos)

            # compute the joint commands
            rl_joint_pos_des = rl_diff_ik_controller.compute(rl_ee_pos_b, rl_ee_quat_b, rl_jacobian, rl_joint_pos)


            # compute the joint commands
            rr_joint_pos_des = rr_diff_ik_controller.compute(rr_ee_pos_b, rr_ee_quat_b, rr_jacobian, rr_joint_pos)

        # apply actions
        robot.set_joint_position_target(fl_joint_pos_des, joint_ids=fl_robot_entity_cfg.joint_ids)
        robot.set_joint_position_target(fr_joint_pos_des, joint_ids=fr_robot_entity_cfg.joint_ids)
        robot.set_joint_position_target(rl_joint_pos_des, joint_ids=rl_robot_entity_cfg.joint_ids)
        robot.set_joint_position_target(rr_joint_pos_des, joint_ids=rr_robot_entity_cfg.joint_ids)

        scene.write_data_to_sim()
        # perform step
        sim.step()
        # update sim-time
        count += 1
        # update buffers
        scene.update(sim_dt)

        # obtain quantities from simulation
        fl_ee_pose_w = robot.data.body_state_w[:, fl_robot_entity_cfg.body_ids[0], 0:7]
        fr_ee_pose_w = robot.data.body_state_w[:, fr_robot_entity_cfg.body_ids[0], 0:7]
        rl_ee_pose_w = robot.data.body_state_w[:, rl_robot_entity_cfg.body_ids[0], 0:7]
        rr_ee_pose_w = robot.data.body_state_w[:, rr_robot_entity_cfg.body_ids[0], 0:7]
        # update marker positions
        fl_ee_marker.visualize(fl_ee_pose_w[:, 0:3], fl_ee_pose_w[:, 3:7])
        fr_ee_marker.visualize(fr_ee_pose_w[:, 0:3], fr_ee_pose_w[:, 3:7])
        rl_ee_marker.visualize(rl_ee_pose_w[:, 0:3], rl_ee_pose_w[:, 3:7])
        rr_ee_marker.visualize(rr_ee_pose_w[:, 0:3], rr_ee_pose_w[:, 3:7])
        
        fl_goal_marker.visualize(fl_ik_commands[:, 0:3] + scene.env_origins)
        fr_goal_marker.visualize(fr_ik_commands[:, 0:3] + scene.env_origins)
        rl_goal_marker.visualize(rl_ik_commands[:, 0:3] + scene.env_origins)
        rr_goal_marker.visualize(rr_ik_commands[:, 0:3] + scene.env_origins)



def main():
    """Main function."""
    # Load kit helper
    sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device)
    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=1, env_spacing=1.0)
    scene = InteractiveScene(scene_cfg)
    # Play the simulator
    sim.reset()
    # Now we are ready!
    print("[INFO]: Setup complete...")
    # Run the simulator
    run_simulator(sim, scene)


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