Robot control part 6: Handling singularities

We’re back! Another exciting post about robotic control theory, but don’t worry, it’s short and ends with simulation code. The subject of today’s post is handling singularities.

What is a singularity

This came up recently when I had build this beautiful controller for a simple two link arm that would occasionally go nuts. After looking at it for a while it became obvious this was happening whenever the elbow angle reached or got close to 0 or \pi. Here’s an animation:

singularity

What’s going on here? Here’s what. The Jacobian has dropped rank and become singular (i.e. non-invertible), and when we try to calculate our mass matrix for operational space

\textbf{M}_\textbf{x}(\textbf{q}) = (\textbf{J} (\textbf{q}) \; \textbf{M}^{-1} (\textbf{q}) \; \textbf{J}^T(\textbf{q}))^{-1},

the values explode in the inverse calculation. Dropping rank means that the rows of the Jacobian are no longer linearly independent, which means that the matrix can be rotated such that it gives a matrix with a row of zeros. This row of zeros is the degenerate direction, and the problems come from trying to send forces in that direction.

To determine when the Jacobian becomes singular its determinant can be examined; if the determinant of the matrix is zero, then it is singular. Looking the Jacobian for the end-effector:

\textbf{J}(\textbf{q}) = \left[ \begin{array}{cc} -L_0 sin(q_0) - L_1 sin(q_0 + q_1) & -L_1 sin(q_0 + q_1) \\ L_0 cos(q_0) + L_1 cos(q_0 + q_1) & L_1 cos(q_0 + q_1) \end{array} \right].

When q_1 = 0 it can be that sin(q_0 + 0) = sin(q_0), so the Jacobian becomes

\textbf{J}(\textbf{q}) = \left[ \begin{array}{cc} (L_0 + L_1)(-sin(q_0)) & -L_1 sin(q_0) \\ (L_0 + L_1) cos(q_0) & L_1 cos(q_0) \end{array} \right],

which gives a determinant of

(L_0 + L_1)(-sin(q_0))(L_1)(cos(q_0)) - (L_1)(-sin(q_0))(L_0 + L_1)(cos(q_0)) = 0.

Similarly, when q_1 = \pi, where sin(q_0 + \pi) = -sin(q_0) and cos(q_0 + \pi) = -cos(q_0), the Jacobian is

\textbf{J}(\textbf{q}) = \left[ \begin{array}{cc} -(L_0 - L_1) sin(q_0) & L_1 sin(q_0) \\ (L_0 + L_1) cos(q_0) & - L_1 cos(q_0) \end{array} \right].

Calculating the determinant of this we get

(L_0 + L_1)(-sin(q_0))(L_1)(-cos(q_0)) - (L_1)(sin(q_0))(L_0 + L_1)(-cos(q_0)) = 0.

Note that while in these cases the Jacobian is a square matrix in the event that it is not a square matrix, the determinant of \textbf{J}(\textbf{q})\;\textbf{J}^T(\textbf{q}) can be found instead.

Fixing the problem

When a singularity is occurring it can be detected, but now it must be handled such that the controller behaves appropriately. This can be done by identifying the degenerate dimensions and setting the force in those directions to zero.

First the SVD decomposition of \textbf{M}_\textbf{x}^{-1}(\textbf{q}) = \textbf{V}\textbf{S}\textbf{U}^T is found. To get the inverse of this matrix (i.e. to find \textbf{M}_\textbf{x}(\textbf{q})) from the returned \textbf{V}, \textbf{S} and \textbf{U} matrices is a matter of inverting the matrix \textbf{S}:

\textbf{M}_\textbf{x}(\textbf{q}) = \textbf{V} \textbf{S}^{-1} \textbf{U}^T,

where \textbf{S} is a diagonal matrix of singular values.

Because \textbf{S} is diagonal it is very easy to find its inverse, which is calculated by taking the reciprocal of each of the diagonal elements.

Whenever the system approaches a singularity some of the values of \textbf{S} will start to get very small, and when we take the reciprocal of them we start getting huge numbers, which is where the value explosion comes from. Instead of allowing this to happen, a check for approaching the singularity can be implemented, which then sets the singular values entries smaller than the threshold equal to zero, canceling out any forces that would be sent in that direction.

Here’s the code:

Mx_inv = np.dot(JEE, np.dot(np.linalg.inv(Mq), JEE.T))
if abs(np.linalg.det(np.dot(JEE,JEE.T))) > .005**2:
    # if we're not near a singularity
    Mx = np.linalg.inv(Mx_inv)
else:
    # in the case that the robot is entering near singularity
    u,s,v = np.linalg.svd(Mx_inv)
    for i in range(len(s)):
        if s[i] < .005: s[i] = 0
        else: s[i] = 1.0/float(s[i])
    Mx = np.dot(v, np.dot(np.diag(s), u.T))

And here’s an animation of the controlled arm now that we’ve accounted for movement when near singular configurations:

fixed

As always, the code for this can be found up on my Github. The default is to run using a two link arm simulator written in Python. To run, simply download everything and run the run_this.py file.

Everything is also included required to run the MapleSim arm simulator. To do this, go into the TwoLinkArm folder, and run python setup.py build_ext -i. This should compile the arm simulation to a shared object library that Python can now access on your system. To use it, edit the run_this.py file to import from TwoLinkArm/arm_python to TwoLinkArm/arm and you should be good to go!
More details on getting the MapleSim arm to run can be found in this post.

Advertisements
Tagged , , , , ,

12 thoughts on “Robot control part 6: Handling singularities

  1. This is helpful for me QUT robotics course. I still cannot work out how to get singularity in MATLAB or even using Jacobian on paper.

  2. Vince says:

    What about the robot is insufficient or redundant? The Jacobin is not invertible at the first place

  3. […] we are now able to calculate the operational space inertia matrix. This code I’ve explicitly worked through before, and I’ll show it in the full code below, so I won’t go over it again […]

  4. Paradox.Delta says:

    The jacobians for q1=0 and q1=pi and their determinants look confusing to me. for example in the upper left field of the jacobian for q1=0 you would have -L0*sin(q0) – L1*sin(q0) wich is equal to (-L0 – L1)*sin(q0) wich is equal to -(L0 + L1)*sin(q0) but you write -(L0 – L1)*sin(q0) . Also it’s given determinant only equals 0 for q0=0 and q0=pi but it should equal 0 for all q0. Might be a calculation mistake

    • travisdewolf says:

      Hello, thanks for the catch! You’re right it should be (L0 + L1) * (-sin(q0)), I’ve fixed it in the post. And you’re right, the determinant should be 0 for all q0, I suspect a calculation mistake.

  5. Nitish Sharma says:

    Hi! You mentioned the following for non-square Jacobians.

    ‘Note that while in these cases the Jacobian is a square matrix in the event that it is not a square matrix, the determinant of \textbf{J}(\textbf{q})\;\textbf{J}^T(\textbf{q}) can be found instead.’

    Can you please refer me to a source which talks about this in detail?

  6. ezanalogy says:

    One thing I’m not getting is that you set the singular values below a certain threshold to zero, but then the inverse would still be Infinity (at least numerically) right?

    • travisdewolf says:

      Ah, almost. I’m checking for values close to zero, and instead of calculating the inverse (s_inv=1/s) just setting s_inv=0.

      • ezanalogy says:

        Oh ok thanks, I figured that should be the case, but it was just a bit unclear from your writing:

        ‘..which then sets the singular values entries smaller than the threshold equal to zero, canceling out any forces that would be sent in that direction…”

        Its written that the singular values are set to zero and not the inverse’s.

        Thank you! Nice post!

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s

%d bloggers like this: