For my honors thesis I made a five bar robot that could play air hockey. More on that and the custom hardware required to make that happen in a future post (sneak peek of the render above!), but for now we are going to talk about the kinematics and dynamics of this robot. In this I’ll include a few diagrams and a traces of the robot motion and links to some of the papers and code I used to make this happen.

## Forward Kinematics of a Five-Bar

The forward kinematics of the five-bar can be determined using the standard rules of geometry and a healthy portion of cosine and sine. Using the diagram below, it is clear that if we assume joint 1 to be at position , the point would be just be the sum of the x and y components of each side of the five-bar.

This leaves us with the following equations for the end effector point, finishing the forward kinematic analysis.




## Inverse Kinematics of a Five-Bar

Because that was too easy, we now graduate to the inverse kinematics problem. For the five-bar robot this is both surprisingly easy and hard. Because of the redundant nature of the robot, we see that there are four solutions of angles that reach . To envision these other solutions, imagine reflecting the two bars across their respective dashed lines.

The method used here will make use of several facts about the geometry of the system and use the cosine rule. The cosine rule can effectively define , , and , so we can then solve for each.

Using these solutions, we can then solve for and . This creates four total solutions. This can be done as follows:

      

Each of those represents a possible solution of joint angles that reaches . Given a set of current angles, one of these solutions will likely be the closest and most reasonable solution to move towards. Because of this, the associated code on my GitHub determines the best solution based off your current position in terms of angles.

## Five-Bar Robot Dynamics

The code and equations for the dynamics are massive and cumbersome, but can be written in the standard form of differential equations for ridged body systems. If you want to know more about this, check out this paper someone made on this and the code I made to simulate it.

Not only did I make code to simulate the five-bar under a set of initial conditions, I also made code that simulated the five-bar following a given path with a PD controller. I include this here because I thought it was a cool demonstration on how PD gains can affect the speed and accuracy of the tracking for the robot. I also found that the robot, when tuned aggressively, had to follow the path closely or it would go out of control. This indicated to me that some form of gain scheduling based on the distance to the desired path would be needed if this control strategy was used. I have not implemented this feature yet, but it seems like something I should look into.

For each figure, the bar on the left represented the color of the line over time from time=0 seconds to time=10 seconds. The robot gets to the final position before 10 seconds, so you will not see the entire color range. You can see this color change best on the poorly performing PD controller due to the extra settling time.

On the left we can see that for a medium Kp PD controller (Kp=50 Kd=5) the five-bar robot follows the line well. On the right we can see that for a small Kp PD controller (Kp=5 Kd=5) the five-bar robot does not follow the line effectively.

For the large Kp PD controller (Kp=150 Kd=5) the five-bar robot follows the line extremely well.