I am struggling with this problem for days. I really hope that someone could give me a hint what the problem is.

The robot consists of 5 axes. The first axis rotates around the z-axis and other 4 axes rotate around the y-axis. And the solver basically works.

Here is what I have done so far:

  1. I calculate the manipulability factor with my Jacobian matrix (only translational part, since only the position is tracked here. Actually, I also tried with a combined Jacobian matrix, so not only the translational part but also the rotational part. But jerky motion was there anyway):

  2. Then the damping factor is:

    enter image description here

  3. The damping factor is then integrated into the pseudo inverse calculation :

enter image description here

As you can see, this is just a classical pseudo inverse kinematic solver with damped least square method. The manipulability factor according the second (problem) movement is : enter image description here The manipulability drops in the beginning of the video. But why? As far as I know, this manipulability factor indicates the linear dependence of the axes. To me the axes don't seem to be linearly dependent in the beginning part.

This jerky motion drives me crazy. As you can see in the first animation, the solver seems to work properly. What am I missing here?

  • 1
    $\begingroup$ Welcome to Robotics, Joe! This is a fantastic question, but unfortunately I don't think there's quite enough detail to answer the question. When a system goes to a singularity, I would expect some axis of motion to make a wild swing, like one axis swinging 180 degrees because another one is trying to go 89 to 91 degrees (if 90 degrees were the singularity). I'm not really seeing that behavior here. I'm wondering if this is more of an implementation issue, such as using something like atan instead of atan2, etc. Could you please edit your question to post the code you're using? $\endgroup$
    – Chuck
    Commented Feb 1, 2019 at 13:59
  • 1
    $\begingroup$ @ Chuck, I edited the question so that the "wild swing" can be seen. (look at the 2. updated animation ) $\endgroup$
    – Joe
    Commented Feb 1, 2019 at 15:51

2 Answers 2


As others have already pointed out, there must be an issue with your implementation of the IK algorithm since there shall not be any singular behavior in the descriptions you gave.

Now you have two alternatives: either you start off debugging the code or you might want to exploit the fact that the problem can be easily broken down in two subproblems for which you can readily employ the most part of the code written so far.

Given the desired 3D target $\left( x_d,y_d,z_d \right)$, it's straightforward to observe that the desired value of the first joint is: $\theta_{1d}=\arctan\left( \frac{y_d}{x_d}\right)$.

The control law to drive the first joint of the manipulator to $\theta_{1d}$ can be as simple as:

$ \dot{\theta_1}=K_1 \cdot \left( \theta_{1d}-\theta_{1} \right). $

Then, let $R \in SO(3)$ be the matrix that accounts for the rotation of $\theta_{1d}$ around the $z$ axis:

$ R = \left( \begin{array}{cccc} \cos{\theta_{1d}} & -\sin{\theta_{1d}} & 0 \\ \sin{\theta_{1d}} & \cos{\theta_{1d}} & 0 \\ 0 & 0 & 1 \\ \end{array} \right). $

Through $R$, you'll get the new target $\left( x_d,0,z_d \right)_1=R^T \cdot \left( x_d,y_d,z_d \right)^T$ that will establish a new IK 2D planar problem in the $xz$ plane.

At this point, you can solve for $\left( x_d,z_d \right)_1$ by using the Jacobian of the remaining 4-DOF manipulator.

  • $\begingroup$ I think there is no need to break down the problem in subproblems. Since the declaration of the unit vector already indicates what you have just described. $\endgroup$
    – Joe
    Commented Feb 15, 2019 at 20:22
  • $\begingroup$ The break down is a mere suggestion to let you reuse the 4 DOF Jacobian, which is the part of your code working smoothly, as you reported at the beginning of your post. $\endgroup$ Commented Feb 15, 2019 at 20:30

I think you have introduced an algorithmic singularity at the first wrist axis. It appears to me that, when it reaches 90 degrees “down,” that instead of going to 91 it tries to flip back through zero to -269 degrees.

Of course this is speculative without seeing the code.


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.