The inverse Jacobi Matrix is pose dependent. It is not constant, it is dependent on pose (joint angles or TCP coords, based on formulation) and so it characterizes a pose not a robot. By implementing it as a constant you basicly assume that the same joint velocity will always cause the same TCP velocity. In reality it is clear that joint positions play an important role in how joint velocities are transformed to the TCP.

I recommend you generate the Jacobi matrix in a symbolic form, similarly as your forward kinematics, dependend on variables (joint angles of TCP cooridnates) and write a function that calculates the value of the matrix for each current pose. Then recalculate the matrix in every iteration and it should be fine.

Furthermore, in this form, if you have a 3x3 matrix, you can easily calculate its inverse, you do not need a pseudoinverse. Later, if you have 7 DOFs you might...

The (inverse) Jacobi Matrix is pose dependent. It is not constant, it is dependent on pose (joint angles or TCP coords, based on formulation) and so it characterizes a pose not a robot. By implementing it as a constant you basicly assume that the same joint velocity will always cause the same TCP velocity. In reality it is clear that joint positions play an important role in how joint velocities are transformed to the TCP.

I recommend you generate the (inverse) Jacobi matrix in a symbolic form, similarly as your forward kinematics, dependend on variables (joint angles of TCP cooridnates) and write a function that calculates the value of the matrix for each current pose. Then recalculate the matrix in every iteration and it should be fine.

Furthermore, in this form, if you have a 3x3 matrix, you can easily calculate its inverse, you do not need a pseudoinverse. Later, if you have 7 DOFs you might...