URDF-to-USD Joint Index Discrepancy Causing PhysX Jacobian Retrieval Failure

[Plankton_Shuu]

Isaac Sim Version

4.5.0
4.2.0
4.1.0
4.0.0
4.5.0
2023.1.1
2023.1.0-hotfix.1
Other (please specify):

Operating System

Ubuntu 22.04
Ubuntu 20.04
Windows 11
Windows 10
Other (please specify):

GPU Information

• Model: RTX4090
• Driver Version: 535.230.02

Topic Description

Detailed Description

Hi, I have been working on a loco-manipulation whole-body control project using a robot with an arm mounted on a quadruped robot dog. I encountered a problem while trying to obtain the Jacobian matrix using the Isaac Sim API (robot.root_physx_view.get_jacobians(). Specifically, the Jacobian matrix cannot be correctly retrieved for this type of mani-locomotion system and thus the IK cannot be solved.

USD and Configuration

The robot system I use was directly converted from the URDF file using the URDF-to-USD converter. However, this caused an issue: the joint indices became misaligned compared to the original URDF file here where all body and joint names are organized such that the arm parts come after the leg parts. In the resulting USD file, however, the indices for the arm and leg joints are now mixed together:

body names: ['base', 'trunk', 'wx250s_base_link', 'FL_hip', 'FR_hip', 'RL_hip', 'RR_hip', 'imu_link', 'wx250s_shoulder_link', 'FL_thigh', 'FR_thigh', 'RL_thigh', 'RR_thigh', 'wx250s_upper_arm_link', 'FL_calf', 'FR_calf', 'RL_calf', 'RR_calf', 'wx250s_upper_forearm_link', 'FL_foot', 'FR_foot', 'RL_foot', 'RR_foot', 'wx250s_lower_forearm_link', 'wx250s_wrist_link', 'wx250s_gripper_link', 'wx250s_ee_arm_link', 'wx250s_gripper_prop_link', 'wx250s_gripper_bar_link', 'wx250s_fingers_link', 'wx250s_ee_gripper_link', 'wx250s_left_finger_link', 'wx250s_right_finger_link']

joint names: ['FL_hip_joint', 'FR_hip_joint', 'RL_hip_joint', 'RR_hip_joint', 'widow_waist', 'FL_thigh_joint', 'FR_thigh_joint', 'RL_thigh_joint', 'RR_thigh_joint', 'widow_shoulder', 'FL_calf_joint', 'FR_calf_joint', 'RL_calf_joint', 'RR_calf_joint', 'widow_elbow', 'widow_forearm_roll', 'widow_wrist_angle', 'widow_wrist_rotate', 'widow_left_finger', 'widow_right_finger']

What I’ve Tried

I have studied the Isaac Lab official example here for IK and how to retrieve the Jacobian matrix. I was able to follow the example successfully and test the IK for an individual arm (the same wx250s arm mounted on the quadruped robot) by correctly setting the ee_jacobi_idx and the corresponding robot joint_ids. For a single arm, the indexing is accurate and follows the structure defined in the original URDF file.

I also tried to fix the first three joints of the arm to make the remaining index of the arm to be correct and in sequence. And the Jacobian matrix is working very well and can see the arm moving correctly. Therefore, I can confirm that the reason for this issue is because of the USD joint index discrepancy.

Error Messages

The Jacobian of the whole-body system is incorrect and the IK task failed.

Screenshots or Videos

Here is the link to the video showing the failure of IK due to incorrect Jacobian matrix.

Additional Information

Here is my USD file for the robot, the cfg for the robot, and the my modifed script from the Isaac Lab official demo. The robot system is set to fixed base for only testing the IK.

Additional Context

I can now confirm the reason for this issue. However, it seems that there is no way for users to customize the joint ordering during the URDF-to-USD conversion process? What can be done to solve this?

Thank you very much for your help.

[Plankton_Shuu]

The problem has been solved by following here. The issue is not related to USD file itself. Hope this will be helpful for future developers.

[Zhefeng_H]

I also encountered the same problem. I have a URDF file that defines joints in an order j1, j4, j2, j3, j5, j6, j7, j8, where j2 and j3 are mimicking j2. After converting the URDF file into a USD file, I loaded it into Isaac Sim and checked robot.find_joints(name_keys=[".*"], preserve_order=True), which returned joints in a new order j1, j2, j4, j3, j5, j6, j7, j8, giving URDF-USD joint order discrepancy.

Then I created a robot_entity_cfg for indexing into the Jacobian matrix and joint position vector, which has robot_entity_cfg.joint_ids = [0, 1, 4, 5, 6, 7]. I skipped index 2 and 3 since j4 and j3 are mimic joints. This index was correct for indexing into both robot.data.joint_pos.

However, it failed when I tried jacobian = jacobians[:, ee_jacobi_idx, :, robot_entity_cfg.joint_ids] and returned a wrong Jacobian. I printed out the full Jacobian: jacobians = robot.root_physx_view.get_jacobians() and found out that its 2nd and 4th column are zeros, suggesting joint at index 1 and 3 are mimic joints. Apparently, somehow the Jacobian matrix in articulation view is still following the joint order in the URDF file.

To verify that whether the URDF order j1, j4, j2, j3, j5, j6, j7, j8 presents for all properties of robot.root_physx_view, I also printed out robot.root_physx_view.get_dof_positions() to compare it with robot.data.joint_pos. It turned out they are identical, suggesting that maybe the URDF order is only kept for the Jacobian matrix in physx articulation view?

In the end I solved the problem by simply doing jacobian = jacobians[:, ee_jacobi_idx, :, [0, 2, 4, 5, 6, 7], manually correcting the indexing following the URDF joint order.