Simulating a Robotic Arm
I have seen these cheap ‘6 DOF’ robot arm kits on ebay that use hobby servos to control the joints and have been curious about them for a while. I ordered one but it turns out that they include the gripper opening and closing as one of the degrees of freedom, thus making this arm truly only ‘5 DOF’ as far as positioning/orienting is concerned. Before writing firmware for it I wanted to explore the various techniques used to control robotic arms, and thus I ventured into the world of Inverse (and Forward) Kinematics
Check out the GitLab Repo here
Forward and Inverse Kinematics
Before jumping into the more interesting part I should clarify why Inverse Kinematics gets more attention. Forward Kinematics is the process of finding the pose of the robotic arm (and consequently the position and orientation of the end-effector) given the angles of the revolute joints (joints that rotate) and positions of the prismatic joints (joints that translate). This is actually not too hard given a decent background in trigonometry.
Going the other way, finding what joint angles/positions you should use to given a target position, isn’t as straight forward. This is what Inverse Kinematics attemptst to solve.
As it turns out, it has been proven that if a robotic arm has three joints that have axis of actuation that are parallel then it is possible to find a closed form solution for the inverse Kinematics problem.
Without a neat closed form solution one is left with numerical methods, and this was what I was more interested in.
Jacobian Method
The Jacobian method is a numerical method for honing in on the best angles to form the resultant position and oritentaiton. The only problem is that it requires a series of loops and isn’t guaranteed to converge on a solution (or it may converge but take very very long). It is similar to other numerical approximation methods such as gradient descent.
The Jacobian Method works by calculating the Jacobian of the position and orienation vector with respect to each of the joint paramters (angles and positons). This jacobian can then be applied to the error vector between the current position (found via forward kinematics) and the target position to get incremental changes to the joint parameters which bring the end-effector closer to the target.
Simulating
I wrote up a simple IPython Notebook which implemented the numerical method and stored the intermediate results. I noticed that although this method worked its convergance was heavily dependent on the starting position/orientation of the arm and the target position/orientation. When disregarding orientation the arm could pretty easily get to the target position but the Inverse Kinematics solver would get stuck trying to balance error in the position with error in the orientation.
To resolve this I implemented a heuristic that adjusted the target orientation after the arm went through a certain number of loops without making meaningful improvement to its position. The orientation would either be adjusted higher or lower depending on the position of the target and the orientation that the arm was getting stuck in. This allowed the arm to dynamically approcah from a new angle when it was getting stuck, trading off the impportance of approaching from the correct angle in favor of actually getting the end effector to the target position. The current heuristic still has some bugs to work out but for many targets it correctly dynamically adjusts.
In addition I implemented a method intelligentStartAngles that took the target positon/orientation and started the numerical descent at a position that was based off the target, this allowed faster calculations and eliminated some non-convergence issues when the arm always starts with angles that cause the arm to point start up.
Moving Forward
Just about the time I finished up this simulation a new Udacity Nanodegree on Robotics Software Engineering opened up. Since then I have started on the course and have have simulated a Kuka Robotic Arm in Gazebo. More on that in the future.