# Problem with Inverse Kinematics Solver (Jacobian Inverse Method)

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.

• Can you post also forward_kinematics() ? Jan 30 at 2:01