There are some errors in the parallel forward_kinematics calculation (N=1000) when an input URDF consists of fixed joints.
For example, I have a URDF: tiago_dual_simplified.zip that has fixed joints before AND after the revolute joints in the chain. Upon using the tests for forward kinematics with this urdf (test_kinematics.py), the issues are:
Line 146 in chain.py (forward_kinematics):
trans = trans.compose(f.get_transform(th[:, cnt].view(N, 1)))
which in turn calls:
Line 176 in transform3d.py:
self._matrix = torch.eye(4, dtype=dtype, device=device).view(default_batch_size, 4, 4)
Here, default_batch_size has value equal to N and hence causes the error: RuntimeError: shape '[N, 4, 4]' is invalid for input of size 16
My fix was to change line 146 to:
self._matrix = torch.eye(4, dtype=dtype, device=device).repeat(default_batch_size, 1, 1)
Same line (146):
In the case where there are fixed joints AFTER the last revolute joint, the cnt variable goes over the maximum index of the variable th. th has size equal to the number of DOFs and hence the increment cnt+=1 will eventually cross the number of DOFs and when the value of th is accessed as th[:, cnt], it throws an error.
My fix was to just check if the joint is fixed and use th[:,0] instead (the value of th[:,0] is irrelevant for a fixed joint).
LemonPi
commented
2 years ago
There are some errors in the parallel forward_kinematics calculation (N=1000) when an input URDF consists of fixed joints. For example, I have a URDF: tiago_dual_simplified.zip that has fixed joints before AND after the revolute joints in the chain. Upon using the tests for forward kinematics with this urdf (test_kinematics.py), the issues are:
trans = trans.compose(f.get_transform(th[:, cnt].view(N, 1)))which in turn calls: Line 176 in transform3d.py:self._matrix = torch.eye(4, dtype=dtype, device=device).view(default_batch_size, 4, 4)Here,default_batch_sizehas value equal toNand hence causes the error:RuntimeError: shape '[N, 4, 4]' is invalid for input of size 16My fix was to change line 146 to:
self._matrix = torch.eye(4, dtype=dtype, device=device).repeat(default_batch_size, 1, 1)cntvariable goes over the maximum index of the variableth.thhas size equal to the number of DOFs and hence the increment cnt+=1 will eventually cross the number of DOFs and when the value ofthis accessed asth[:, cnt], it throws an error.My fix was to just check if the joint is fixed and use
th[:,0]instead (the value ofth[:,0]is irrelevant for a fixed joint).The fixes can be found in this commit: https://git.ias.informatik.tu-darmstadt.de/jauhri/ias_pytorch_kinematics/-/commit/6e95865109a5461cdb0865c00ad961c7160231b1#e952e44500e0ab8e85e2680be772fba7944093e5