python - Solving Denavit Hartenberg Forward Kinematics for n roboticarms with Numpy

Question

EDIT I just solved the problem with numpy's einsum function. Instead of doing T[:,:,:,0]@... I just Matmul'ed the first two with einsum, then the resulting with the next index, and so on.

---

I am currently trying to solve the Denavit Hartenberg Equation for a 5 DOF robotic arm forward kinematics. Since i need so solve this for a genetic algorithm, i need to do it simultaniously for n robotic arms.

My current approach is that I define a 4x4xnx5 matrice with all the needed transformation matrices for n robotic arms. I then itarate all n arms over a for-loop, solving each DH-Equation with matrixmultiplikation (See code sample 1 down below). This works but is obviously time consuming. A much smarter way would be to do something like code sample 2, but I then get the following error message (n=500):

"matmul: Input operand 1 has a mismatch in its core dimension 0, with gufunc signature (n?,k),(k,m?)->(n?,m?) (size 4 is different from 500)"

is there a way to do such matrixmultiplikation with numpy like demonstrated in code sample 2?

for i in range(n):
       T_res[i,:,:] = T[:,:,i,0]@T[:,:,i,1]@T[:,:,i,2]@T[:,:,i,3]@T[:,:,i,4] #CODE SAMPLE 1


T_res= T[:,:,:,0]@T[:,:,:,1]@T[:,:,:,2]@T[:,:,:,3]@T[:,:,:,4] #CODE SAMPLE 2

Answer 1

I just solved the problem with numpy's einsum function. Instead of doing T[:,:,:,0]@... I just Matmul'ed the first two with einsum, then the resulting with the next index, and so on.