Screw Theory Kinematics for Industrial Robot Arms

Introduction

In my previous project, I build a 5 DOF robot arm and implemented the kinematics using the D-H method and an analytical solver.

Have you ever felt the pain of crashing a servo? I have, but now I can crash much more expensive parts in a risk-free environment.

During my wild Googling, I came across a different method that involves screw theory, but it looked incredibly complex and I knew I would make many mistakes. I also came to realize that debugging math on a physical robot is incredibly inefficient (and potentially expensive).

In order to isolate the high level controls from the numerous sources of error on a physical robot, I set up a simulation in Webots with an industrial robot arm. With a "perfect" physics setup where errors such as gear backlash is not an issue, I can narrow down the scope of troubleshooting to the math and the code. Once I eventually port this method back to my physical robot arm, I'll be certain that any jitter is a hardware flaw, not a miscalculation.

Kinematics - Screw Theory

While there are definitely kinematics libraries out there, I wanted to build the kinematics solver from the ground up to understand exactly how the Jacobian and the concept of "twist" work.

To solve the forward and inverse kinematics, I followed the theory detailed in Lynch and Park's Modern Robotics book, which revolves around screw theory.

Screw theory is based on the Mozzi-Chasles Theorem, which states that any rigid body motion in 3D can be represented by a screw motion (a rotation and translation along a screw axis).

Advantages

• More robust to singularities with the Jacobian-based approach.
• Easier to set up since screw axes are much more intuitive to assign than D-H frames. No more axis alignment headaches.

If you're interested in the math, check out my technical document attached to this project..

Newton-Raphson Solver

While this method lacks an analytical solution for the inverse kinematics, the Newton-Raphson iterative method allows the solver to be robot-agnostic. If I were to add or remove a degree of freedom, I wouldn't have to manually re-solve the forward and inverse kinematics.

The idea behind the Newton-Raphson algorithm is simple. You feed it an initial guess to what all the joint angles should be, then it computes the forward kinematics to get the end-effector pose at that initial guess. If the error between the new end-effector pose and the target is not within some threshold, it moves on to the next guess and this process repeats until the end-effector is "close enough" to the target.

Position vs. Velocity Control

I initially tried controlling the arm using position control where I solve the inverse kinematics for the target joint positions. The problem with this method is that each joint moves a different amount, so they will all arrive at their targets at different times and the arm will end up swinging in an unpredictable arc rather than moving along a smooth path. One solution would be to set intermediate waypoints that lie along a straight path. Another way to do it would be to control joint velocities instead of position.

Velocity control solves this problem with a special matrix called the Jacobian. The Jacobian is a matrix that relates the end-effector's velocity to all the joint velocities. In particular, it helps us answer the question of "what joint velocities will result in the desired end-effector velocity?".

PID Control

Combining a PID control loop and acceleration/deceleration logic in the main motion method, the arm can move somewhat more smoothly rather than jerking to an initial velocity.

The jump in the middle of the motions is due to the speed being capped in the middle of an acceleration. This will need the accelerations and limits to be tuned.

To better tune and debug the PID control, I implemented a visualizer for the twist error. Notice how the errors spike up when the robot starts moving, then reduce down to 0 as the robot approaches its target.

Problem

While I was coding this up, I started wondering if PID control was even necessary. If I had acceleration control to accelerate to a certain speed, then decelerate down to 0 as the end-effector approaches the target, then what would the PID control solve? And would it fight the trapezoidal velocity profile? I'll be exploring this next by implementing a trapezoidal velocity profile, and hopefully the motions will look more realistic.

What do you guys think about this, does it make sense to combine PID control with a velocity profile?Let me know in the comments!

Support Me

The code for this project is fully open-source, so anyone can try it out or make their own improvements. It does take quite a bit of time to put together all the videos and documentation, so if you find my content interesting and/or helpful, feel free to support me by buying me a coffee!

If you have any feedback or notice any errors, please leave a comment!