anygrasp_sdk
anygrasp_sdk copied to clipboard
Using anygrasp in IsaacSim
I'm having real trouble figuring out how to translate my grasp results from AnyGrasp to IsaacSim. I was able to do it fairly easily with the translation of the grip using the code below (starting off with the grasp detection example):
best_translation = gg_pick[0].translation.copy()
homogeneous_coordinates = np.append(best_translation, 1)
transformed_coordinates = trans_mat @ homogeneous_coordinates
best_translation = transformed_coordinates[:3]
best_translation[:2] /= 10
However, when I apply a similar process to the rotation, as shown below, the results are wildly off:
rotation_trans = R.from_matrix(trans_mat[:3, :3]).as_matrix()
adjusted_rotation = rotation_trans @ gg_pick[0].rotation_matrix
inverse_camera_matrix = np.array([0,-90, 0])
camera_matrix = R.from_euler('xyz', inverse_camera_matrix, degrees=True).inv().as_matrix()
best_rotation_matrix = camera_matrix @ adjusted_rotation
best_rotation = R.from_matrix(best_rotation_matrix).as_quat()
For some additional context, within IsaacSim, my camera is set directly above the object with a rotation of [0, -90, 0]. The expected result should be something like [0, -1, 0, 0], but I am getting [0.58990548, -0.53592884, 0.3752615, 0.47325533] pre-translation and [0.04431686, 0.68247612, 0.71360097, 0.15177621] post-translation.
Any help would be greatly appreciated.
I met the same problem. Any advice?