[Question] Jacobians from simulation robot.root_physx_view.get_jacobians() differs from roboticstoolbox

[YZhang001]

Question

Hello. I was simulating a Franka robot arm in Isaac Sim 2023.1.1. I was also using the roboticstoolbox package to calculate the Jacobian for each link of the robot arm, given the current joint angle. I have a question about the Jacobian from robot.root_physx_view.get_jacobians():

1. I followed the tutorial from this webpage. For my application, I need to control the Cartesian velocity of each link. During the simulation, I read the positions of each link of the robot arm with robot.data.body_state_w, and Jacobians associating the Cartesian coordinates to the joint velocity with robot.root_physx_view.get_jacobians(). As I understand the body_state_w and the root_physx_view Jacobians, the first Jacobian matrix should correspond to how the linear & angular velocity of the link 1 (defined in the following figure from the Franka webpage) is affected by the joint velocities. According to the Panda’s kinematic chain from the Franka website, link 1 and link 2 are defined right at the center of the 2nd joint, which means their Cartesian position will not move (they won't translate) with the rotation of joints. This also reflects in the read position from the simulation: at any joint configuration, only the z-coordinate of the first link is larger than 0, and is a constant robot.data.body_state_w[0, 1, 0:3] = (tensor([5.8484e-10, 9.5358e-07, 3.3300e-01], device='cuda:0'); also computed with the roboticstoolbox, the position of the link 1 is always [0.0, 0.0, 0.333]. However, with the Jacobians, the result read from the simulation with robot.root_physx_view.get_jacobians() differs from the computed result from roboticstoolbox. I picked a pre-set joint configuration panda.qr from the roboticstoolbox:

panda = rtb.models.Panda()
panda.jacob0(panda.qr, end=f'panda_link1')

and get the Jacobian for the first link expressed in the root frame:

[[**0.** 0. 0. 0. 0. 0. 0.]
 [0. 0. 0. 0. 0. 0. 0.]
 [0. 0. 0. 0. 0. 0. 0.]
 [0. 0. 0. 0. 0. 0. 0.]
 [0. 0. 0. 0. 0. 0. 0.]
 [1. 0. 0. 0. 0. 0. 0.]]

Then I tried to drive the robot arm to the same joint position target in simulation with robot.set_joint_position_target(joint_pos_des, joint_ids=robot_entity_cfg.joint_ids) (I also tried to directly write the joint position into simulation with robot.write_joint_state_to_sim(position=joint_pos_des.float(), velocity=torch.tensor([[0., 0., 0., 0., 0., 0., 0.]], device=sim.device).float(), joint_ids=robot_entity_cfg.joint_ids)). The first jacobian matrix read from robot.root_physx_view.get_jacobians()[0, 0, :, robot_entity_cfg.joint_ids] is then:

tensor([[ **3.1254e-02**,  0.0000e+00,  0.0000e+00,  0.0000e+00,  0.0000e+00,
          0.0000e+00,  0.0000e+00],
        [ 2.0865e-04,  0.0000e+00,  0.0000e+00,  0.0000e+00,  0.0000e+00,
          0.0000e+00,  0.0000e+00],
        [-1.1846e-08,  0.0000e+00,  0.0000e+00,  0.0000e+00,  0.0000e+00,
          0.0000e+00,  0.0000e+00],
        [ 3.7948e-07,  0.0000e+00,  0.0000e+00,  0.0000e+00,  0.0000e+00,
          0.0000e+00,  0.0000e+00],
        [ 2.3021e-08,  0.0000e+00,  0.0000e+00,  0.0000e+00,  0.0000e+00,
          0.0000e+00,  0.0000e+00],
        [ 1.0000e+00,  0.0000e+00,  0.0000e+00,  0.0000e+00,  0.0000e+00,
          0.0000e+00,  0.0000e+00]], device='cuda:0')

where you can see from the first entry of the matrix that 1 rad/s joint velocity of the first joint in configuration space corresponds to 0.03m/s velocity of link 1 at the x direction in Cartesian space, which I don't think should happen (link 1 shouldn't translate at all) and doesn't align with the result from roboticstoolbox. I used the Jacobian from roboticstoolbox on a real Franka arm and it worked well. I also tried to apply the Jacobian matrices from the roboticstoolbox directly into the simulation, but then the simulated robot didn't move as expected (whereas the Jacobian from robot.root_physx_view.get_jacobians() worked in simulation).

2. Furthermore, I found that at a random joint configuration (take [1., -0.3, 1., -2.2, 1., 2., 0.78539816] as an example), the end-effector pose read from the simulation with robot.data.body_state_w (then transformed to the body frame with the subtract_frame_transforms() function in the tutorial) also differs from the ee_pos calculated with roboticstoolbox: At this joint configuration, the ee_pos read from simulation is: [-0.2785778 0.4330759 0.4622494 ], but the ee_pos calculated from roboticstoolbox is [-0.34073631 0.4215214 0.37585111]. There is a large difference in the direction of x and z.

I'm writing to ask whether there is any misunderstanding such as I read Jacobian and position of the end-effector from the wrong data or the Jacobian from robot.root_physx_view.get_jacobians() doesn't associate the Cartesian velocities of the links and the joint velocities. And whether there is anything to notice regarding the difference between the result from roboticstoolbox and from the simulation.

Thank you!

[Mayankm96]

Sorry for the late reply. Would it be possible to get a script to reproduce the above issue? We'd like to incorporate it as a unit test as well.

[rthaker01]

Hi @YZhang001 - Are you still seeking assistance with this issue? and have you tried it with latest Isaac Sim and Isaac Lab release?