Applying action from ArticulationKinematicsSolver always leads to a deviation from the target position for Fetch Robot

I create an simple IK solver class based on Franka’s FollowTarget task.

from omni.isaac.examples.base_sample import BaseSample
from omni.isaac.core.utils.nucleus import get_assets_root_path
from omni.isaac.wheeled_robots.robots import WheeledRobot
from omni.isaac.core.utils.types import ArticulationAction
from omni.isaac.core.controllers import BaseController

from omni.isaac.core.utils.stage import add_reference_to_stage
from omni.isaac.core.robots import Robot
from omni.isaac.motion_generation import ArticulationKinematicsSolver, interface_config_loader, LulaKinematicsSolver
from omni.isaac.core.articulations import Articulation
from omni.isaac.core.objects.cuboid import VisualCuboid
from omni.isaac.core.objects import DynamicCuboid
from typing import Optional
from omni.isaac.core.utils.rotations import euler_angles_to_quat
from omni.isaac.core.utils.numpy.rotations import rot_matrices_to_quats

import lula

import carb

from omni.isaac.franka import Franka
from omni.isaac.franka.controllers import PickPlaceController

import numpy as np

class KinematicsSolver(ArticulationKinematicsSolver):
    """Kinematics Solver for Franka robot.  This class loads a LulaKinematicsSovler object

        robot_articulation (Articulation): An initialized Articulation object representing this Franka
        end_effector_frame_name (Optional[str]): The name of the Franka end effector.  If None, an end effector link will
            be automatically selected.  Defaults to None.

    def __init__(self,
                 robot_description_path: str,
                 urdf_path: str,
                 robot_articulation: Articulation,
                 end_effector_frame_name: Optional[str] = None,
                 default_joint_pos: np.ndarray = None,
                 ) -> None:
        self._kinematics = LulaKinematicsSolver(robot_description_path, urdf_path)
        all_frames = self._kinematics.get_all_frame_names()
        all_joints = self._kinematics.get_joint_names()
        carb.log_warn(f"all frames: {all_frames}\nall joints: {all_joints}")
        if end_effector_frame_name is None:
            end_effector_frame_name = "gripper_link"

        ArticulationKinematicsSolver.__init__(self, robot_articulation, self._kinematics, end_effector_frame_name)



    def set_robot_base_pose(self, robot_position: np.array, robot_orientation: np.array):
        return self._kinematics.set_robot_base_pose(robot_position=robot_position, robot_orientation=robot_orientation)

I want to control the end effector to move to a position relative to the world in the following extension. It works as expected for Franka, but for Fetch Robot, the end effector always stops at a position different from the target when the action is applied. I used a visualize cube to mark the target position in the scene.

However, according to the output of compute_end_effector_pose, the end effector of Fetch is right at the target position when it stops, which is very weird.

class HelloWorld(BaseSample):
    def __init__(self) -> None:

    def setup_scene(self):
        world = self.get_world()

        fetch_robot_path = "/fetch.usd"
        fetch_robot_prim_path = "/World/Fetch_Robot"
        add_reference_to_stage(usd_path=fetch_robot_path, prim_path=fetch_robot_prim_path)
        self._fetchbot = world.scene.add(Robot(
            position=np.array([.0, .0, 0.99]),
            orientation=euler_angles_to_quat([0, 0, 0]),
            scale=np.array([1., 1., 1.]),
            # usd_path=fetch_robot_path,
            # create_robot=True,
            # wheel_dof_names=["l_wheel_joint", "r_wheel_joint"],

        self._franka = world.scene.add(Franka(
            # position = np.array([-2., 1., 1.])
            position = np.array([0., .5, .0]),

        self._dynamic_cube = world.scene.add(DynamicCuboid(
                prim_path = "/World/random_cube", 
                name      = "fancy_cube",
                position  = np.array([ -1, -1, 1.]),
                color     = np.array([ 0, 1., 1.]),
                size      = .1,


    async def setup_post_load(self):
        self._world = self.get_world()
        self._jetbot = self._world.scene.get_object("fancy_robot")
        self._franka = self._world.scene.get_object("franka")
        self._fetchbot = self._world.scene.get_object("fetchbot")
        self._world.add_physics_callback("sending_actions", callback_fn=self.send_robot_actions)

        self._franka_controller = KinematicsSolver(
        self._franka_articulation_controller = self._franka.get_articulation_controller()
        carb.log_warn("ik controller and articulation controller initialized for franka")

        self._fetchbot_controller = KinematicsSolver(
        self._fetchbot_articulation_controller = self._fetchbot.get_articulation_controller()
        carb.log_warn("ik controller and articulation controller initialized for fetchbot")

        robot_base_translation,robot_base_orientation = self._fetchbot.get_world_pose()
        ee_position,ee_rot_mat = self._fetchbot_controller.compute_end_effector_pose()
        ee_orientation = rot_matrices_to_quats(ee_rot_mat)
        # Visulize ee pose.
        self._ee_visual_cube = VisualCuboid(

        self._franka_gripper_orientation = euler_angles_to_quat(np.array([0, np.pi, np.pi]))

        self._iterate_count = 0


    def send_robot_actions(self, step_size):
        self._iterate_count +=1

        if self._iterate_count < 10:

        # Take a target from targets based on _iterate_count.
        targets = [[0.4, -0.3, 1.4], [0.4, -0.3, 1.0], [0.4, 0, 1.0], [0.4, 0, 2.0]]
        robot_base_translation,robot_base_orientation = self._fetchbot.get_world_pose()
        target_idx = int(self._iterate_count / 500)
        if target_idx > len(targets) - 1:
            target_idx = len(targets) - 1
        target = np.array(targets[target_idx])

        # fetchbot control.
        actions_fetchbot, succ_fetchbot = self._fetchbot_controller.compute_inverse_kinematics(
        if succ_fetchbot:
            carb.log_warn(f"fetchbot: IK did not converge to a solution.  No action is being taken.")
        ee_pose = self._fetchbot_controller.compute_end_effector_pose()
        self._ee_visual_cube.set_world_pose(position=ee_pose[0], orientation=rot_matrices_to_quats(ee_pose[1]))
        if self._iterate_count % 100 == 0:
                carb.log_warn(f"fetchbot: pos is {robot_base_translation}, ori is {robot_base_orientation}")
                carb.log_warn(f"eef: current target_pos is {target}, ee_pos is {ee_pose[0]}, ee_ori={rot_matrices_to_quats(ee_pose[1])}")
                dynamic_cube_pose = self._dynamic_cube.get_world_pose()
                carb.log_warn(f"dynamic cube pose: {dynamic_cube_pose[0]}, {dynamic_cube_pose[1]}")

        # franka control.
        robot_base_translation,robot_base_orientation = self._franka.get_world_pose()
        actions_franka, succ_franka = self._franka_controller.compute_inverse_kinematics(
            target_position=np.array([.4, .4, .1]),
        if succ_franka:
            carb.log_warn("franka: IK did not converge to a solution.  No action is being taken.")

After some test, I found one thing that may be related: the Fetch Robot’s position from get_world_pose(robot_base_translation in my code) always has a non-zero z-axis offset while it is on the ground, which equals almost exactly to the deviation of its end effector from target position. I expect it has a near-zero z-axis value when it’s on the ground.

So is it the reason why the end effector is not at the target position? And how can I solve this problem

Here is the usd, urdf and description file I used: (6.1 MB)

At least some of the issues seem to be related to discrepancies between the USD and URDF.

Are you able to provide the meshes that accompanied the URDF on import so that we can import ourselves and diagnose these issues?

Thank you for your very kind reply! Here’s the meshes and all the URDF/USD files of fetch robot:
fetch.tar.gz (32.6 MB)

Upon further inspection, there is a bug that is inverting the control signals for some of the joints on this asset (e.g., setting a target of 1.2 radians instead of -1.2 radians).

Currently, there are no practical workarounds for this bug that will enable control of this particular USD asset. We aim to fix this issue in the next release of Isaac Sim (tentatively scheduled for December).

Thank you for helping us troubleshoot the issue. we may switch to another robot to implement our function first.

Out of curiosity, where exactly is this bug? In the URDF? USD? Presumably it can’t be in the python/native code because then it would apply more generally, right?

The bug is between the PhysX Tensor API and the Isaac Core Articulation interface.

Some of the joints are being inverted. If you look, e.g., at shoulder_lift_joint, you can see the issue.

On the left panel, you can see that the Articulation Inspector is setting and reading the joint value as -0.827 radians.

Meanwhile on the right, the Joint State API shows the current position as 47 degrees.

The Joint State API is what matches the URDF (that you are using as input to Lula).

Some of the joints are not inverted, and some are.

You could technically work around this issue, but it would be fairly complex. Roughly speaking you could:

  1. Identify which joints are affected.
  2. Build a wrapper around Isaac Core Articulation that inverts all impacted joint values (read position, set velocity, etc).
  3. Update any non-symmetric joint limits in USD to work with the inversion. For example, if the original limits were [-4 radians, 2 radians], for the inverted joint this would need to be changed to [-2 radians, 4 radians]

Another approach is to swap Body0 and Body1 on the property window on the right for the affected joints.

This option is shown below. In this image, the shoulder_lift_joint values in the Articulation Inspector (-0.85 radians) and the Joint State API (-49 degrees) match.

The sign convention has now been swapped such that a negative value for this joint causes the arm to point down. This means that with the swap, the USD model would no longer be compatible with the URDF. You could update the URDF by inverting joint axes and limits for impacted joints (e.g., joint axis [1,0,0] would be become [-1,0,0] and limits [-4, 2] would become [-2, 4]). This would mean that joint values no longer match the real robot, which would complicate any sim2real work.

In general, either of these methods would be fairly complicated to implement, with careful attention required to make sure that all systems (USD, URDF, Lula, Articulation, ArticulationInversionWrapper, etc) are all doing the right thing together.

It’s certainly possible to build enough workarounds to get the robot working with the current release, but I suspect that it may be more trouble than it’s worth if you have another robot suitable for testing.

I’m not sure if this is related, but I had a prismatic-joint where in the URDF the limit was -A to 0 and in Isaac Sim I had to set the joint limits to 0 to A to get it to work.

Is this the same bug? So does that mean things work right except for prismatic joints? Or otherwise how does a user know what to expect is broken?

That is most likely the same bug.

It’s not restricted to prismatic joints. The Fetch robot above is an example of bad behavior on revolute joints.

The easiest way to know it’s broken is to see if the results of the Articulation API and JointState API are the same when reading (e.g.) joint position. If they are different, then it’s broken.