Error in jacobian calculation for prismatic joint
sjauhri opened this issue · comments
There is an error in the parallel jacobian calculation (N=1000) when an input URDF contains a prismatic joint.
For example, I have a URDF: tiago_dual_simplified.zip that has both revolute and a prismatic joint. Upon using the test for jacobian calculation, the issue is in line 47 in jacobian.py:
j_fl[:, :3, -cnt] = f.joint.axis.repeat(N, 1) @ cur_transform[:, :3, :3]
The matrix multiplication dimensions are wrong. For example, for N=1000, the two tensors being multiplied are of size [1000,3] and [1000,3,3] respectively. The matrix multiplication appends a dimension and broadcasts it, resulting in a [1000,1000,3] tensor. This is not correct and causes the error: RuntimeError: expand(torch.cuda.FloatTensor{[1000, 1000, 3]}, size=[1000, 3]): the number of sizes provided (2) must be greater or equal to the number of dimensions in the tensor (3)
My fix was to change line 47 to:
j_fl[:, :3, -cnt] = torch.squeeze(f.joint.axis.repeat(N, 1, 1) @ cur_transform[:, :3, :3])
The fix can be found in this commit:
https://git.ias.informatik.tu-darmstadt.de/jauhri/ias_pytorch_kinematics/-/commit/6e95865109a5461cdb0865c00ad961c7160231b1#e952e44500e0ab8e85e2680be772fba7944093e5
fixed with merge #4