task-space controller Jacobian matrix exception

[MrVBian]

I am referring to the following link: https://isaac-sim.github.io/IsaacLab/main/source/tutorials/05_controllers/run_diff_ik.html

Differences:

In my implementation, I have made two modifications to the use case:

1. The ROBOT_CFG has been replaced with a custom robotic arm.

2. The joint_names and body_names have been updated to match the components of my robotic arm using regular expressions:

robot_entity_cfg = SceneEntityCfg("zmebot", joint_names=["left_joint[1-7]"], body_names=["left_link7"])

note:

In the official example, the robotic arm is fixed, while mine is floating base articulations.

Question:

My robotic arm is unable to reach the target point. The robotic arm changes very little.
There are numerous zeros in the Jacobian matrix:

jacobian = robot.root_physx_view.get_jacobians()[:, ee_jacobi_idx, :, robot_entity_cfg.joint_ids]

jacobian: tensor([[[ 0.0000e+00,  0.0000e+00,  0.0000e+00,  0.0000e+00,  8.7464e-01,
          -3.1228e-02, -4.7513e-01],
         [ 0.0000e+00,  0.0000e+00,  0.0000e+00,  0.0000e+00, -2.7196e-02,
           2.2162e-04, -2.5771e-02],
         [ 0.0000e+00,  0.0000e+00,  0.0000e+00,  0.0000e+00, -2.6679e-02,
           2.1953e-02,  1.6447e-02],
         [ 0.0000e+00,  0.0000e+00,  0.0000e+00,  0.0000e+00, -3.0482e-02,
          -7.0937e-03,  3.4608e-02],
         [ 0.0000e+00,  0.0000e+00,  0.0000e+00,  0.0000e+00,  2.0839e-04,
          -9.9997e-01, -2.4470e-04],
         [ 0.0000e+00,  0.0000e+00,  0.0000e+00,  0.0000e+00, -9.9954e-01,
           4.4119e-06,  9.9940e-01]]], device='cuda:0')

I am unsure of the underlying cause and would appreciate any assistance in resolving this issue.

[MrVBian]

I have verified ee_jacobi_idx and robot_entity_cfg.joint_ids:

joint_name = [robot.root_physx_view.dof_paths[0][i] for i in robot_entity_cfg.joint_ids]
print(f"joint_name: {joint_name} body_name:{robot.root_physx_view.link_paths[0][ee_jacobi_idx]}")

The output is as follows, the index is ok:

joint_name: ['/World/envs/env_0/Robot/lifting_base_link/left_joint1', '/World/envs/env_0/Robot/left_link1/left_joint2', '/World/envs/env_0/Robot/left_link2/left_joint3', '/World/envs/env_0/Robot/left_link3/left_joint4', '/World/envs/env_0/Robot/left_link4/left_joint5', '/World/envs/env_0/Robot/left_link5/left_joint6', '/World/envs/env_0/Robot/left_link6/left_joint7'] body_name:/World/envs/env_0/Robot/left_link7

My biggest confusion at the moment is why the Jacobian matrix has so many zeros, which affects the subsequent solution

[MrVBian]

I am very sorry to bother you. I saw the relevant content and asked for your advice. Thanks

@lorenwel @zoctipus @Mayankm96
#1033
#967

[zoctipus]

Hi there, thanks for posting question

It's hard to pinpoint what is the issue from the post. If it is truly the issue of Jacobian, then it is the urdf or usd that has the underlining issue and robot's usd file needs to be made more carefully. I am also feeling that it could be possible that this is not an urdf or usd issue, but bug in the code, and to check it, I usually do below things:

1. Try print the target ee_des and current ee, see if ee_des is a reasonable value. One possible bug will be pass in ik goal in world frame and ee_des will be non-reasonable value.
2. Try print the target joint position and current joint position and see if current joint position is snapping toward target joint position, sometime your ik solution could be correct but robot actuator is not configured in a way such that joint moves to the target.

Hope this helps!

[MrVBian]

Thank you very much for your serious reply.

1. I controlled the real robot to obtain several sets of Cartesian coordinates and joint angles. In the simulation, the joint angles were executed, and the output ee_des position was basically consistent with the real Cartesian value. It was determined that the target ee_des was reasonable
2. I output the target joint position and current joint position, and determined that the target joint angle and the current joint angle were synchronized

The solution process in the official example includes three steps:

1. Get the Jacobian matrix
2. Compute frame in root frame
3. Compute the joint commands

When obtaining the Jacobian matrix in the first step, the Jacobian matrix value is output, and the values of the first four joint outputs are 0. This makes the solved joint_pose_des[0:3] also 0.

Thanks again for your reply!

[MrVBian]

Try

Without modifying the code, I imported the same urdf with fixed base articulations, and the robotic arm could move towards the target.

But in this case, after 17 steps, the Jacobian matrix appears:

tensor([[[nan, nan, nan, nan, nan, nan, nan],
         [nan, nan, nan, nan, nan, nan, nan],
         [nan, nan, nan, nan, nan, nan, nan],
         [nan, nan, nan, nan, nan, nan, nan],
         [nan, nan, nan, nan, nan, nan, nan],
         [nan, nan, nan, nan, nan, nan, nan]]], device='cuda:0')

Error message:

Question

1. Therefore, I speculate: Can we think that there is a problem in obtaining the Jacobian matrix of float base robot (containing a large number of zeros)?

2. What is the problem that all elements of the Jacobi matrix are nan? How can I fix this

[MrVBian]

sim_utils.ArticulationRootPropertiesCfg set is not reasonable

[YuHoChau]

Hi, I encountered a similar issue and was wondering if I could reach out to you via email/wechat to ask about possible solutions. I would really appreciate your help and thank you in advance!