5. Code Documentation
The purpose of inverse kinematics is to calculate the joint angles required to achieve a desired end effector position. Many robots use a velocity-based or heuristic approach to solving this problem, but our arm design allows us to do something a little simpler. Our arm, as per competition requirements, has five degrees of motion. Three joints control position on the YZ plane, one controls rotation around the Y axis, and the final joint controls the rotation of the end effector. As such, we are able to use a trigonometric approach to calculating joint angles.
For reference, we call the first joint, which controls Y-axis rotation, the waist, then the joints controlling the XY plane are called the shoulder, elbow, and wrist, in order from closest to the base to closest to the end effector, and finally the hand is the joint controlling the rotation of the end effector, as shown in Figure 1 below.
We take two pieces of input to the function: a desired end effector position, given as an array {x, y, z, alpha_x, alpha_y, alpha_z} where x, y, z are the cartesian coordinates and alpha_x, alpha_y, alpha_z are the XYZ Euler angles representing the hand rotation, and the current pose of the robot, given as array {waist, shoulder, elbow, wrist, hand} where each index contains its joint's angle. We define the origin as the waist position.
Were our robot capable of rotating 360 degrees at every joint, there would be four ways to reach any one position, as there is always a working reflex and non-reflex angle for both the shoulder and elbow. The shoulder is unmoving, so the first step is to generate the possible elbow positions. The method for this is fairly simple. Imagine two circles centered on the shoulder and the wrist, with radii equal to the shoulder-elbow link and elbow-wrist link respectively. These two circles typically intersect in two points (sometimes they may only intersect in 1, but that is a rarity), and these two points represent the potential elbow positions, as shown below in Figure 2.
We do this through the inverseKinematicsJointAngles function. Essentially, we calculate the position of the wrist by backtracking from the known hand position across the line generated from the known angle of the hand. (Note: Euler angles do not translate to Cartesian angles, so we convert to a transformation matrix to calculate the angle.) We do a similar process for the shoulder, which is always directly above the waist at a fixed height. Then, we calculate the aforementioned circle intersections and combine these to create two arrays of joint positions, one with each elbow position.
This is then passed into the manager function inverseKinematicsAngleOptions, which only serves to organize four calls of inverseKinematicsComputeJointAngles. This function takes a hand, wrist, and elbow position as well as a parameter indicating whether the waist angle should be rotated 180 degrees. It contains the bulk of the algorithm, and takes advantage of the limited number of joints controlling each plane of motion.
Consider the arm as viewed from the side in Figure 1. On this supposed flat plane, only three joints control the end effector position: the shoulder, elbow, and wrist. We project our joint positions onto this plane defined by the line between the XZ coordinates of the waist and end effector. With the knowledge of the elbow position, we can calculate the angle of the shoulder joint using inverse sine. Draw an invisible line between the shoulder and wrist to form a triangle. As we know the length of the shoulder-elbow and elbow-wrist links and can calculate the length of this invisible line based on the coordinates of the shoulder and wrist, we can use cosine law to calculate the interior angle of the elbow. We repeat this process for the elbow, wrist, and hand to calculate the interior angle of the wrist.
Now consider the arm as viewed from the top, as below in Figure 3. We can calculate the appropriate waist angle using the inverse tangent of the end effector, since this joint is the only one capable of affecting this ratio.
Then, we use the measured "0" of each joint to adjust the calculated angles. This is where the predetermined waist angle rotation comes into play. If the given parameter is 180, we rotate the waist angle 180 degrees and assign the shoulder angle to be the calculated angle's negative, so that we continue to point along the same line. This finalized set of angles is then passed back to inverseKinematicsAngleOptions.
Once all the angle sets are collected, they are routed into legalIKPositionPicker. Although we previously assumed all joints could rotate 360 degrees, this is not the case. This function begins with a loop that checks each angle in an angle set for whether it is physically achievable and discards the set if it is not. Typically, this leaves only one position, assuming an achievable position was possible. If it produces multiple, we choose the position that is closest to the current position, and this is the final output of the function.
TL;DR we calculate the four possible arrangements of joints using cosine law and inverse tangent, then pick whichever one obeys the physical limits of the joints.