I am trying to create an inverse kinematics solver for my 4-dof (all revolute joints) robot using the inverse Jacobian Method. However, it does not seem to be working as the distance between target and end-effector position keeps growing rather than converging to 0. Not sure where I am going wrong. I have pasted my code below.
def inverse_kinematics_solver(current_joint_configuration,target): n = 4 # number of joints X =  # position of end effector Q =  # joint configurations J = [0,0,0,0] # jacobian matrix iterations = 0 Q.append(current_joint_configuration) X.append(forward_kinematics(Q[-1])) print(np.linalg.norm(target-X[-1])) while (np.linalg.norm(target-X[-1]) > 5): for c in range(4): change = Q[-1].copy() change[c] = change[c] + 0.01 J[c] = X[-1] - forward_kinematics(change) print(J[c]) J_t = np.column_stack((J,J,J,J)) pseudo_inverse = np.linalg.pinv(J_t) increment = target - X[-1] delta_Q = np.matmul(pseudo_inverse,increment) Q.append(Q[-1] + delta_Q) X.append(forward_kinematics(Q[-1])) return(Q[-1])
I can confirm that my forward_kinematics function is working. Also my target vector has the following format [X,Y,Z,pitch,yaw,roll]. Also if it is helpful, this robot is part of a 6dof robot (PPRRRR) that I am designing. I wanted to confirm the kinematics for just revolute joints was working first.
Any help would be greatly appreciated. Thanks.