To start, the arm in use can be classified as the "Revolute" type due to each joint consisting of rotational motion as opposed to linear actuation. Using the standard Denavit-Hartenberg convention, a free body diagram of this arm can be created by representing each of the joints as a symbol and showing the links connecting each joint as a line. It is not necessary to include the end effector (gripper) because it is not a major factor in the mathematical process, but it has been shown in the adjoining initial FBD. Also noted are the measured lengths of each linkage as well as the designated name of each joint. It can be seen that the arm has a total of five degrees of freedom, not including the horizontal movement associated with the iRobot Create base or the gripping action of the end effector.

For the geometric representation, both the end effector and wrist rotate servos can be excluded as they do not effect the positioning of the arm. Also to be excluded from the initial system is the base rotation. These exclusions leave three joints to work with. These joints are first shown with each in its base position of zero degrees before be more generally represented.

*Positioning as Θ*_{1}, Θ_{2}, and Θ_{3} are equal to zero degrees.* *** **

This representation can be further simplified by a more generic drawing of the links and joints. In this final progression of the geomtric model, a few new variables are introduced to signify the height of the arm base (*ho*) as well as the height (*H*) and distance (*D*) from the base to an object in space.

## An Assumption Based Approach

If the end goal here was amanipulator capable of grasping any given object with the ease of a human being, this model would not be feasible, but for the practical purposes of this project, there are only two approaches that actually need to be considered. The first (and most likely) scenario is that the gripper will approach the object parallel to the ground. This means that Link L3 would also be parallel to the ground and can be omitted from the equations. The second scenario is that the gripper approaches an object from directly above such that link L3 would be perpendicular to the ground plane and can be again omitted from the equations.

At this point, a few new variables should be introduced. The coordinates (Da, Ha) represent the respective distance and height of the arm need to be positioned at the translated points while (D, H) represents the respective distance and height of the original grasping point.

## Trying Out Trigonometry

While some may view trig as a dirty word, it is an awesome tool in solving these types of math problems without needing calculus or differential equations. To start, the previous geometric representation is shown again with every mentioned simplification as well as a few new variables.

If a line is drawn from the origin (arm base) to the point *P*, two triangles are created by the links *L1* and *L2*, the imaginary line *L*, and the height *(H)* and distance *(D)* from the origin to the point *P, *as shown in the figure. The lower is a standard right angle triangle consisting of the legs L, Ha, and Da. The upper is formed by the legs L1, L2, and L. The solutions to *Φ1* and *L* are given as *Eq 3* and *Eq 4*.

The second and upper triangle is easily managed using the law of cosines.

Combining the results obtained in equations 5-7 with the variables associated with the previous figures yields a few new relationships as shown in *Eq 8* and *Eq 9*. Finally, the entire system can be solved using only the original variables by combining everything obtained thus far. The final solutions to Θ1 and Θ2 are shown as *Eq 10* and *Eq 11*.

To find the seemingly forgotten angle Θ3, a simple combination of the solved angles and a slight offset can be used. The amount of offset depends on the desired angle of the end effector with respect to ground.

Considering the ability of the wrist to rotate is not a factor in the positioning of the arm, it's function can be completely neglected in this system. The only remaining angle to be solved is the base rotation. This can also be easily solved by generating a second geometric model to represent the base as looked at from above. Now, the point *P* is not a factor of distance and height, but of the Cartesian coordinates *(X, Y)* where the center of the base is the origin point. The solution to the angle is given in *Eq 14*, and again in *Eq 15* should *Eq 14* result in a negative value. Lastly, the distance *D* is the same as that used previously.

*Θ*_{0 }= arccos(Y_{base }*/ **X*_{base}*) ***Eq 14**

*Θ*_{0 }= arccos(Y_{base }*/ **X*_{base}*) **+ **π ** ***Eq 15**

*D = | **Y*_{base }*/ sin(**Θ*_{0}*) | ***Eq 16**

## Practical Considerations

Obviously, this solution is not perfect. There are many restrictive factors which must be taken into consideration. For example, the formulas will work if a negative height Ha is used, but the arm cannot very well go below the ground plane unless the robot is on the edge of a cliff. Also, using a negative distance is not feasible given that only sensors are located in the front. However, there is a distinct possibility that a reasonable height and distance pair could lie outside of the operating range of the arm. For this reason, special precautions must be taken when determining the proper algorithms to control the arm in a useful manner.

With that, I will introduce a concept known as *reachablilty*. This is the estimated reachable area of the mounted arm, as shown in the adjacent image. While the gripper can be placed into any point in orange outlined area A, only the maroon outline area B is reachable while maintaining a ground parallel orientation in accordance to *Eq 1*.

## Object Detection

The last piece of this puzzle has to be the way in which the coordinates of the object to be manipulated are supplied to the robot. This can be accomplished in many ways, but a pair of simple forward range finding sensors are be used in this project. Once again, simple procedure encompassing geometric modeling and trigonometric calculations is used.

Each sensor will return a voltage level related to the distance to an object which can be linearized to a very precise distance in millimeters. The angle between the sensor and object is also known because the robot will be in control of the servos supplying the sensor rotation. With this, the unknown Cartesian coordinates of the point P can be found with respect to each sensor.

*X*_{LET} = D_{L}cos(Φ_{L}) **Eq 14** ; Y_{LET} = D_{L}sin(Φ_{L}) **Eq 15**

*X*_{RET} = D_{R}cos(Φ_{R}) **Eq 16** ; Y_{RET} = D_{R}sin(Φ_{R}) **Eq 17**

With these coordinates found, the position of the object with respect to the base can be solved, and the resulting coordinates can be used to position the arm through the process previously described. Because the position of the sensors with respect to the base is known (I put them there) the following final equations for the object position can be obtained quite easily.

*X*_{base}=*X*_{LET}*-X*_{diff} * ***Eq 18** ; Y_{base}=*Y*_{LET}*-Y*_{diff} * ***Eq 19**

*X*_{base}=*X*_{diff}*-**X*_{RET} * ***Eq 20** ; Y_{base}=*Y*_{diff}*+**Y*_{RET} * ***Eq 21**

Placing these results into Equations 14 -16 will yield the values for *D* and *Θ**0*. One important thing to note here is that the known value of *ho* can be used for the object height *H*. This is not because the arm is not capable of auto-positioning to any point within its reachability, but simply because the only object detection in use is not capapble of scanning vertically. Thus, the height of the object *(H)* becomes equal to the height of the sensors *(ho)*. The calculated values of *H* and *D* can then be used to find the remaining unknown joint angles, *Θ1, **Θ2,* and *Θ**3* using equations 1 - 13.