Wrong InverseKinematics results with new dVRK model

[MissingLight]

Hi,

I am trying to integrate a new dVRK PSM Si model (different with dVRK classic) into SurRoL. I basically wrote the new URDF file and re-define the psm environment. For URDF I tried to make the link order and joint arrangement similar to what is defined in the original psm classic URDF file.

However, when testing, I get stuck in the wrong or unstable solution of InverseKinematics from pybullet. I was wondering if you have faced the same issue before? Here is an example:

joint_input = psm.move_joint([0., 0., 0.2, 0., 0., 0.])
step(0.5)
pose_world = forward_kinematics(psm.body, eef_link=psm.EEF_LINK_INDEX) 
target_world = get_matrix_from_pose_2d(pose_world)
print('pose_world: ')
print(target_world)

joints_inv = p.calculateInverseKinematics(
    bodyUniqueId=psm.body,
    endEffectorLinkIndex=psm.EEF_LINK_INDEX,
    targetPosition=pose_world[0],  # inertial pose, not joint pose
    targetOrientation=pose_world[1],
    lowerLimits=psm.limits['lower'][:psm.DoF],
    upperLimits=psm.limits['upper'][:psm.DoF],
    jointRanges=psm.limits['upper'][:psm.DoF] - psm.limits['lower'][:psm.DoF],
    restPoses=[0.] * psm.DoF,
    # jointDamping = [0.5] * psm.DoF,
    # solver = p.IK_SDLS,
    residualThreshold=1e-12,  # can tune
    maxNumIterations=1000
    )
joints_inv = np.array(joints_inv)
for i in range(psm.DoF):
    if psm.JOINT_TYPES[i] == 'P':
        joints_inv[i] /= psm.scaling
joints_sol = wrap_angle(joints_inv[:psm.DoF])
print('***TEST*** joints_inv', joints_sol)

psm.move_joint(joints_sol)
step(0.5)
pose_world = forward_kinematics(psm.body, eef_link=psm.EEF_LINK_INDEX) 
print('pose_world sol: ')
print(get_matrix_from_pose_2d(pose_world))

And the output is:

pose_world: 
[[-1.96549771e-05 -8.12493362e-01 -5.82970443e-01  4.59036201e-01]
 [ 1.00000000e+00 -2.41588265e-05 -4.47552144e-08 -6.61911059e-08]
 [-1.40475185e-05 -5.82970442e-01  8.12493362e-01  4.68582630e-01]
 [ 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00]]
***TEST*** joints_inv [ 9.49841379e-07  2.40407970e-07  1.99999986e-01 -1.57079623e+00
  1.77197320e-05 -2.59519567e-06]
pose_world sol: 
[[-5.82970651e-01 -8.12493212e-01  1.63319895e-05  4.59036201e-01]
 [ 5.60809389e-06 -2.41249322e-05 -1.00000000e+00 -6.93162292e-08]
 [ 8.12493212e-01 -5.82970651e-01  1.86206656e-05  4.68582630e-01]
 [ 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00]]

The IK solved joint result joints_inv is [0 0 0.2 -1.57 0 0] while the provided input is [0 0 0.2 0 0 0]. From testing so far, I found that the position tends to be accurate but for orientation, there is always a -1.57079623(-pi/2) offset of the 4rd joint(tool roll). However, in some cases such offset is gone. So the stability of IK solver is not reliable. I cannot find any problem with either the URDF file or the environment. I feel it is more of the problem with bullet itself.

Could you please provide any hint to this problem? I am attaching the necessary scripts for your reference. I am more than happy to have a further discussion on adding more robot modules into SurRoL. Thanks.
https://drive.google.com/drive/folders/12SArtA-CraVkCvT5lst_lGiUnBx_vpZp?usp=sharing

[YonghaoLong]

Hi,

Thanks a lot for your interest in our work. Would you like to have a meeting with our developers? It can better identify the issue and further discuss about the integration. If you are interested, please feel free to reach out to me through my email: yhlong@cse.cuhk.edu.hk

Thanks!
LONG Yonghao