# Singularity problem at Inverse kinematic solver

I am struggling with this problem for days. 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, as you can see below:

The problem occurs when the goal position contains y-movement also as below:

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 :

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

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 : The manipulability drops in the beginning of the video. But why? As far as I know, this manipulability factor indicates the linear dependance of the axes. To me the axes dont seem to be linear dependant 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?

UPDATE -----------------------------

Two more animations, the first one is with very high alpha in the damping factor calculation below and the second is one is with alpha =0, which means without any damping around singularity area.

As you can see above, with very high alpha the movement stops around the singularity area (the manipulability value is in that position 0.12 ! At the home configuration the manipulability value is around 15.)

Below you can see how the robot moves, if the damping effec is equal to zero (alpha =0).

UPDATE 2 -----------------------------

I might find the error, but not sure because this leads to another error... So I changed the unit_vectors of my 2-5 axes so that they rotate around x-axis, instead of y-axis:

the problematic point can be reached now :

But the point that could be reached before is not reachable now....

To me it doesnt make sense that the unit_vectors should be around x-axis. What am I missing here?

• 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? – Chuck Feb 1 at 13:59
• @ Chuck, I edited the question so that the "wild swing" can be seen. (look at the 2. updated animation ) – Joe Feb 1 at 15:51

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.

• 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. – Joe Feb 15 at 20:22
• 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. – Ugo Feb 15 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.