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
    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)
        J_t = np.column_stack((J[0],J[1],J[2],J[3]))
        pseudo_inverse = np.linalg.pinv(J_t)
        increment = target - X[-1]
        delta_Q = np.matmul(pseudo_inverse,increment)
        Q.append(Q[-1] + delta_Q)

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.

  • $\begingroup$ Can you post also forward_kinematics() ? $\endgroup$ Jan 30 at 2:01


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.