Evaluation of a Three Degree of Freedom Revolute-Spherical-Revolute Joint

Configuration Parallel Manipulator

A thesis presented to

the faculty of

the Russ College of Engineering and Technology of Ohio University

In partial fulfillment

of the requirements for the degree

Master of Science

Joseph J. Feck

August 2013

© 2013 Joseph J. Feck. All Rights Reserved.

2

This thesis titled

Evaluation of a Three Degree of Freedom Revolute-Spherical-Revolute Joint

Configuration Parallel Manipulator

by

JOSEPH J. FECK

has been approved for

the Department of Mechanical Engineering

and the Russ College of Engineering and Technology by

Robert L. Williams II

Professor of Mechanical Engineering

Dennis Irwin

Dean, the Russ College of Engineering and Technology

3

Abstract

FECK, JOSEPH J., M.S., August 2013, Mechanical Engineering

Evaluation of a Three Degree of Freedom Revolute-Spherical-Revolute Joint

Configuration Parallel Manipulator (142 pp.)

Director of Thesis: Robert L. Williams II.

The purpose of this thesis is to evaluate a three degree of freedom (DOF) revolute-spherical-revolute (RSR) configured robotic manipulator for the use of controlling yaw, pitch, and roll of a model aircraft. Studies of past 3-DOF parallel manipulators were used to gain an understanding of how to design such a system without creating singularity conditions. Inverse orientation and forward orientation kinematics were derived from the vector loop closure equation of the system. This study investigated what effects would occur to the maximal rotational capabilities of the robotic system when the constant lengths of the parallel manipulator were changed. It was determined that the IOK and FOK solutions of the system were valid and that such a system would be able to attain requested angular rotations by the aircraft’s team.

However, modeling of the system led to a case were another leg configuration wanted to be used, as it proved to reduce the torque applied to the servo motors within the system.

A prototype of the new parallel manipulator was machined and future work on this project will require the derivation of the kinematics equations, such as was done with the

3-RSR orientation.

4

Acknowledgements

First and foremost, I would like to give special thanks to God for bestowing many blessings upon me and helping me shape into the man I am today. I would like to thank my advisor, Dr. Robert L. Williams II. Without him I would have never gained such a passion for robotics, and it is that passion which fueled my pursuit of this thesis. He has helped me every step of the way with great enthusiasm. I would like to thank Dr. Zhu for providing me with a project and helping me obtain the materials needed to complete this project. He has been very helpful along the way. I want to thank Dr. Choi. The knowledge I obtained in his class has helped me in my work. I would also like to thank

Dr. Chapin for his time and participation with my thesis. I would like to give a special thanks to Dean Irwin for funding me and making all of this possible. I would also like to thank Elvedin Klijuno, as his work jumpstarted this project. Finally, I would like to thank my family. It is their constant love and support that has encouraged me to always work hard and do my best in all aspects of life.

5

Table of Contents

Page

Abstract ...... 3

Acknowledgements ...... 4

List of Figures ...... 7

List of Tables ...... 10

1 Introduction ...... 11

1.1 Background ...... 11

1.2 Literature Review ...... 14

1.2.1 3-PRUR Parallel System ...... 14

1.2.2 3-UPU Parallel System ...... 18

1.2.3 3-RRUR Parallel System ...... 20

1.2.4 3-RPR Parallel Manipulator ...... 23

1.3 Project Information ...... 26

1.4 Thesis Objectives ...... 29

2 Kinematics Analysis ...... 31

2.1 Vector Loop Closure Equation ...... 31

2.2 Inverse Orientation Kinematics Solution ...... 41

2.3 Forward Orientation Kinematics Solution ...... 44

3 Kinematics Results ...... 51

3.1 Inverse Orientation Kinematics ...... 51

3.1.1 Obtaining the IOK Solution ...... 51

3.1.2 Graphical Interpretation of the IOK Solution ...... 56 6

3.1.3 IOK Results for Windmobile Project ...... 76

3.2 Forward Orientation Kinematics ...... 82

4 Discussion of Kinematics Results ...... 88

5 Modified Design 3-RSR to 3-USR ...... 94

6 Conclusion/Future Work ...... 102

References ...... 106

Appendix. MATLAB Programs...... 108

7

List of Figures

Page

Figure 1: Stewart Platform (Shao, Tang, Chen, & Wang, 2012, p. 650) ...... 11

Figure 2: 3-PRUR Parallel System (Zeng et al., 2008)...... 16

Figure 3: 3-UPU Parallel System (Di Gregorio, 2003)...... 19

Figure 4: 3-RRUR Parallel System (Deidda et al., 2009) ...... 21

Figure 5: Reference frames for analysis of 3-RRUR (Deidda et al., 2009) ...... 22

Figure 6: 3-RPR parallel manipulator (Zhang, 2012) ...... 24

Figure 7: Galah aircraft (Dibenedetto, 2012 a) ...... 27

Figure 8: Truss system conceptual design (Dibenedetto, 2012 b) ...... 28

Figure 9: Generalized model of 3-RSR system ...... 32

Figure 10: Fixed Base Platform Kinematic Details ...... 33

Figure 11: Moving Base Platform Kinematic Details ...... 34

Figure 12: Model of RSR system with virtual middle leg ...... 36

Figure 13: {L} Kinematic Detail ...... 38

Figure 14: {r} Kinematic Details ...... 39

Figure 15: {r} Kinematic Details with Rotated Servomotor ...... 40

Figure 16: General Graphical Interpretation of IOK Solution with variable α ...... 57

Figure 17: Change in α vs θ and ϕ with L = 10in ...... 59

Figure 18: Change in α vs θ and ϕ with L = 5in ...... 60

Figure 19: Change in α vs θ and ϕ with R = P = 3.5in...... 61

Figure 20: Change in α vs θ and ϕ with R = P = 1.5in...... 62

Figure 21: Change in α vs θ and ϕ with r = 2in ...... 63 8

Figure 22: Change in α vs θ and ϕ with r = 0.5in ...... 64

Figure 23: General Graphical Interpretation of IOK Solution with variable β ...... 65

Figure 24: Change in β vs θ and ϕ with L = 10in ...... 66

Figure 25: Change in β vs θ and ϕ with L = 5in ...... 66

Figure 26: Change in β vs θ and ϕ with R = P = 3.5in ...... 67

Figure 27: Change in β vs θ and ϕ with R = P = 1.5in ...... 68

Figure 28: Change in β vs θ and ϕ with r = 2in ...... 69

Figure 29: Change in β vs θ and ϕ with r = 0.5in ...... 70

Figure 30: General Graphical Interpretation of IOK Solution with variable γ ...... 71

Figure 31: Change in γ vs θ and ϕ with L = 10in ...... 72

Figure 32: Change in γ vs θ and ϕ with L = 5in ...... 73

Figure 33: Change in γ vs θ and ϕ with R = P = 3.5in ...... 74

Figure 34: Change in γ vs θ and ϕ with R = P = 1.5in ...... 74

Figure 35: Change in γ vs θ and ϕ with r = 2in ...... 75

Figure 36: Change in γ vs θ and ϕ with r = 0.5in ...... 76

Figure 37: Allowable Footprint of Robotic System ...... 77

Figure 38: Change in α vs θ and ϕ for Windmobile Project ...... 79

Figure 39: Change in β vs θ and ϕ for Windmobile Project ...... 80

Figure 40: Change in γ vs θ and ϕ for Windmobile Project...... 81

Figure 41: Investigation of Rotated Servo ...... 82

Figure 42: Initial RSR Configuration CAD Model...... 95

Figure 43: Part A ...... 97

Figure 44: Part B ...... 98 9

Figure 45: Part C ...... 99

Figure 46: Generalized 3-USR Parallel Manipulator ...... 100

Figure 47: Windmobile’s USR Configuration CAD ...... 101

10

List of Tables

Page

Table 1: Arrangement of functions Fj(X) from equations (22) ...... 46

Table 2: Arrangement of variables X in equations (22) ...... 46

Table 3: IOK_Solution_Alpha.m sample ...... 55

Table 4: Desired vs. achieved roll, pitch, and yaw angles ...... 92

11

1 Introduction

1.1 Background

Parallel manipulators are known as mechanical systems that generally have a base and a platform, which are connected together by computer controlled legs. The legs are serial kinematic chains that work together to control the platform of the system which may also be referred to as the end effector. Parallel manipulators are known to have some advantages over their serial counterparts such as higher rigidity, higher load capacity, and better positioning accuracy (Zeng, Zhen, & Wenjuan, 2008). There are many types of parallel manipulator systems; one that is well known is the Stewart

Platform. A representation of the Stewart Platform can be seen in Figure 1. This robotic system is typically comprised of six linear actuators that support a platform. This parallel manipulator is used in applications such as flight and auto simulations. It has even been used to evaluate biomechanical functions such as joint laxity (Mangan, 2010).

Figure 1: Stewart Platform (Shao, Tang, Chen, & Wang, 2012, p. 650) 12

There are many difficulties that arise in studying parallel robotic manipulators.

These problems include complex kinematic and dynamic equations. In the case of a parallel robotic kinematic analysis the inverse kinematic solution will be much easier to obtain than the forward kinematic solution. The forward kinematic solution is difficult to achieve due to the fact that it requires the solution of multiple coupled nonlinear

(transcendental) algebraic equations from the vector loop-closure equations. This, in turn, leads to multiple valid solutions for the forward kinematics problem (Deidda,

Mariani, &Ruggiu, 2009).

Another difficulty that arises is the complex control of the kinematic chains (Liu,

Lou, & Li, 2003). It is important to ensure that the linear actuators are programmed to work in conjunction with each other to provide the right motion for the platform. When examining a parallel robotic manipulator system, it can be observed that there would be a problem with the configuration space, as the system has only a limited workspace due to the set legs. The problem with this is that the configuration space is never explicitly known. According to Liu et al. (2003) “the configuration space is implicitly defined by a set of constraint functions introduced by the manipulator’s closure constraints” (p. 579).

The final difficulty with these systems is finding a way to overcome the many singularities found within the system. Singularities are found in the end-effector, the configuration space, and the actuators of the system (Liu et al., 2003). In most cases it has been found that the number of degrees of freedom (DOFs) that a parallel manipulator possesses is equal to its number of legs. Also, it has typically been found that only one joint in each leg is actuated. Interestingly, parallel robots may sometimes gain DOFs in 13 singular configurations as opposed to lose DOFs, which is what is seen in serial robotic systems (Ben-Horin, 2006).

As mentioned, the Stewart platform configuration is the most common type of parallel robotic manipulator. While researching, it was found that a lot of times when searching for a Stewart platform, a hexapod robot would appear as a search result. This is due to many people’s misconception that a hexapod is a type of Stewart platform; however this is not the case. In a Stewart platform parallel robotic manipulator, the legs are set and support a single end-effector. Hexapod robots are found to have free moving legs which allow for the whole system to move about and are not confined to a central location. This mobility of the system itself is the main difference between the parallel manipulators and the hexapods that were observed. However, it can be pointed out that in this specific study a parallel manipulator system that incorporates the use of servo motors as opposed to linear actuators is being evaluated. The hexapod robots are also found to run off of servo motors instead of linear actuators, as was found in the conventional Stewart platform. In this sense, the 3-RSR (revolute-spherical-revolute) parallel robotic manipulator being evaluated could be viewed as type of hybrid of the two systems since it uses servo motors to actuate movements within the legs, yet the legs are attached to a base platform and are not free moving.

To better understand how to evaluate and design a 3-RSR parallel manipulator it is necessary to investigate many different types of parallel manipulator systems. Keeping in mind that only three DOFs are needed in many technical applications (Di Gregorio,

2003), only three DOFs parallel systems need be examined for the current project. With that, the 3-PRUR (prismatic-revolute-universal-revolute), 3-UPU (universal-prismatic- 14 universal), 3-RRUR (revolute-revolute-universal-revolute), and 3-RPR (revolute- prismatic-revolute) systems have been investigated. Each of these systems are known three degree of freedom parallel manipulator systems and knowledge obtained by studying them will lead to a better understanding of the evaluation of a 3-RSR parallel manipulator.

1.2 Literature Review

1.2.1 3-PRUR Parallel System

In Figure 2, a 3-PRUR parallel manipulator is presented. In this configuration, the P, R, and U stand for prismatic, revolute, and universal joint respectively. The prismatic joint is the joint that is actuated. For this manipulator, there are only three translational DOFs (Zeng et al., 2008). Zeng et al. express that one of the merits of this configuration is that it is possible to use it as a micromanipulator due to the fact that it is completely decoupled at the initial position. A micromanipulator is any manipulator used to interact with objects that would be under a microscope. They require very precise movements. For the system presented by Zeng et al. the inverse kinematics involve determining the required inputs (L11, L22, L33) for a given position (xo, yo, zo) of the moving platform (MP). By solving the inverse kinematics of the 3-PRUR system, it was determined that the system “can behave like a conventional X-Y-Z Cartesian machine if any two of xo, yo, and zo are defined” (Zeng et al., 2008, p. 042307-1). As shown in equation 1, differentiating the inverse kinematics equations leads to the determination of the Jacobian matrix of the parallel manipulator which in this case was concluded to be: 15

(1)

In the design of the parallel manipulator, we can study the Jacobian matrix to avoid some singularity conditions. Studying the Jacobian Matrix of the system, it can be seen that to avoid singularities the values of xo, yo, and M-zo need to be smaller than the values of L1 and L2 (Zeng et al., 2008). This is concluded by noticing that if x0, y0, or M- z0 was greater than the values of L1 or L2 than this would cause a case where we would be trying to take the square root of a negative number. This would cause an imaginary solution, which would conclude that the system fails. It is important to make sure that the results within the Jacobian matrix produce real solutions to ensure that the system does not fail.

16

Figure 2: 3-PRUR Parallel System (Zeng et al., 2008)

Observing the 3-PRUR system, it can be seen that workspace is a major factor in its design. Generally it is found that parallel manipulators have smaller workspaces than their serial counterparts. The workspace of a parallel manipulator depends on the mechanical limits and interferences amongst the legs of the manipulator, extreme displacements of the linear actuators, and singularity constraints (Zeng et al., 2008).

Zeng et al. determined that the workspace shape for the 3-PRUR manipulator was similar to a cube, in which the side lengths of that cube were fairly equal to the input limits of the actuators. This makes sense in that it would be impossible for the workspace of the system to be any larger than what the input links of the arms will allow the system to move. Therefore, the length of the links within the system directly relates to the potential 17 workspace of the system. In order to increase the workspace, it is necessary to maintain the links of the system as long as possible.

The dexterity of a robotic system describes how much the system can move and how many ways it can perform tasks. The more dexterous a robotic system is means that it has a greater capability of performing intricate motions and performing more tasks.

The condition number of the Jacobian matrix is used to determine the dexterous regions of a manipulator’s workspace. If the condition number of the Jacobian matrix is large, then there is less dexterity within the given system. In designing a parallel manipulator, one wants the condition number to equal one; this creates an isotropic robot (Zeng et al.,

2008). The equation for isotropy is given by:

(2)

where σmax is the maximal number of singular values for matrix J and σmin is the minimal number of singular values for matrix J (Zeng et al., 2008). A challenging part of designing the 3-PRUR is determining the right balance of the systems specifications.

This is challenging for the fact that optimal design of the system would include a large workspace, and an isotropic system. However, it is found that the isotropy of the parallel manipulator is better with smaller inputs, yet smaller inputs create a smaller workspace for the system (Zeng et al., 2008). This creates a difficult task in determining the correct specifications for the manipulator design. It is important to know what works best for the particular case that the system will be used in order to balance out the conditions to meet the criteria of the task at hand. 18

1.2.2 3-UPU Parallel System

Di Gregorio (2003) researched the kinematics of a 3-UPU parallel manipulator for wrist applications. His study found that two engineers, Karouia and Hervé, sought a three DOF spherical parallel manipulator that would have a platform capable of performing elementary spherical motion. The ability to perform such a motion is one condition that guarantees that the platform can perform finite spherical motion which would make the system considered a parallel wrist (Di Gregorio, 2003). For a better understanding of what a wrist application should be used for, Hess-Coelho (2007) described that a wrist should provide spherical motion, possess a remote location of actuators, be light weight, have high accuracy, be compact in size, have low inertia, have high mechanical stiffness, and achieve a large workspace. Thus, the system that the engineers developed to perform such motions was the 3-UPU parallel manipulator.

This type of manipulator is composed of a base and end effector that are connected by three legs comprised of a UPU configuration. The U and P stand for universal and prismatic joints respectively. The prismatic joint of the leg is the joint being actuated. Figure 3 displays a 3-UPU parallel system. Before the 3-UPU configuration was introduced, only two other configurations were being used to obtain a parallel wrist, those configurations were UPS (U, P, and S standing for universal joint, prismatic joint, and spherical joint respectively) and a RRR (R standing for revolute joint)

(Di Gregorio, 2003).

19

Figure 3: 3-UPU Parallel System (Di Gregorio, 2003)

Di Gregorio’s research found that the UPU configuration, unlike the past configurations, was not overconstrained. This allowed the UPU system to overcome the drawbacks such as mechanical jamming and high internal loads that were associated with the previously overconstrained systems being used (Di Gregorio, 2003). By solving the kinematics of the 3-UPU manipulator, Di Gregorio found that if the mechanism starts moving from rest in a non-singular configuration that satisfies the wrist’s geometric conditions, then the wrist would perform an infinitesimal motion at the end-effector while point P would remain at its initial position and velocity of point P would remain zero.

Point P is the common intersection of the R pair axes that is fixed in the platform (Di

Gregorio, 2003). 20

It is necessary to determine the relationship between the platforms angular (ω)

̇ and translational ( ̇) velocities and the time derivatives ( ) of the actuated joints to determine the singularities of the 3-UPU wrist. Di Gregorio determined that rotational singularities would arise if the configuration made ω indeterminate, while translational singularities would arise if the configuration made ̇ indeterminate. The 3-UPU possesses pure translational motion if the two outer and two inner revolute joint axes are parallel to one another (Tsai, Joshi, 2000). Tsai and Joshi (2000) derived an equation that determined an isotropic point within the workspace of the 3-UPU system and also found that the global condition index is maximized in such a system when the base and moving platforms are symmetrical. Maximizing the global conditioning number creates a better chance for the system to be isotropic. The methods used to find the singularity conditions in Di Gregorio’s case could be used to determine singularity conditions in other parallel manipulator configurations. This includes systems such as the 3-RSR which will be introduced later.

1.2.3 3-RRUR Parallel System

Figure 4 presents a 3-RRUR spherical parallel manipulator. In this configuration, the R and U stand for revolute and universal joints respectively. The first revolute joint attached to the frame is the actuated joint. This configuration makes use of equivalent planar joints as opposed to equivalent spherical joints to simplify assembling issues

(Deidda et al., 2009). Many applications can be obtained through the use of a 3-RRUR manipulator. Those applications include orientation wrist for cameras, medical devices, haptic interfaces, and sensors (Deidda et al., 2009). 21

Figure 4: 3-RRUR Parallel System (Deidda et al., 2009)

Due to the potential applications of the manipulator, we must determine its kinematics. In order to begin the inverse and forward kinematics, it is necessary to determine a vector loop closure equation of the system. Figure 5 presents the reference frames for the analysis of the kinematics of the 3-RRUR manipulator. As shown in equation 3 the vector loop closure equation for this case yielded:

(3 ) where ai = AiBi, bi = BiCi, and di = CiP (Deidda et al., 2009). The inverse kinematics process involves the Euler angles (φ, γ, ψ) of the moving platform to be known, and the actuated rotations ϑ1i (i = 1, 2, 3) are determined. By using the vector loop closure equation and algebraic manipulation in the inverse kinematic analysis, the actuated 22 rotations were able to be obtained. The inverse kinematics produces four solutions for each actuated rotation (Deidda et al., 2009). The same was then done for the forward kinematics of the 3-RRUR parallel manipulator.

Figure 5: Reference frames for analysis of 3-RRUR (Deidda et al., 2009)

Deidda et al. determined that the forward kinematics has thirty-two solutions at most. As for all other parallel manipulators, singularity analysis was conducted on the 3-RRUR parallel manipulator. For this system, it was determined that direct singularities may occur when wi are coplanar and inverse singularities may occur when the first and last R pair axes of a leg are aligned or one of the legs is stretched or fully folded (Deidda et al.,

2009). For this system, wi = . The information presented on the kinematics of this 23 particular system can be used in the determination of the kinematics of other parallel manipulator systems. It is important to set up a vector loop closure equation for a given system to perform kinematic analysis on the system.

1.2.4 3-RPR Parallel Manipulator

One last 3 DOF parallel manipulator that was observed was the 3-RPR configuration. Williams and Joshi (1999) observed such a configuration and pointed out that as was seen with other parallel manipulators, the inverse kinematics is straight forward, while the forward kinematics is difficult. This configuration is interesting in that it has been seen to represent a planar model for the Stewart Platform as opposed to the conventional spatial configuration that had been previously discussed (Zhang, 2012).

The spatial Stewart platform case was comprised of six linear actuated legs, while the planar Stewart platform is comprised of three legs with a RPR, Revolute-Prismatic-

Revolute joint configuration. Williams and Joshi (1999) described that in a RPR configuration, the revolute joints are passive, while the prismatic joints of the system are being actuated. Other applications for this parallel manipulator include manufacturing or assembly, where high speed and accuracy within a relatively small workspace would be required of the system (Williams and Joshi, 1999). A simple model of a 3-RPR parallel manipulator can be seen in Figure 6.

24

Figure 6: 3-RPR parallel manipulator (Zhang, 2012)

Using the Grubler mobility equation, it is found that the RPR configuration does in fact have 3-DOF since it is found to be comprised of 8 rigid links and 9 1-DOF joints

(Williams and Joshi, 1999). As has been seen with the previous parallel manipulators, this configuration poses a concern in its limited workspace. The workspace of a parallel manipulator is complex and “is divided internally by the singularity locus of the direct kinematic problem, with its boundaries being defined by the singularities of the inverse kinematic problem” (Urízar, Petuya, Altuzarra & Hernández, 2012). With this being taken into consideration, much attention is given to the evaluation of the workspace and singularities of parallel manipulators. Observing the 3-RPR configuration, the analysis of cusp points is investigated to determine where within the workspace singularities will occur. Zein, Wenger, and Chablet showed that a nonsingular change of assembly mode is possible through the act of encircling a cusp point in the case of a 3-RPR manipulator.

A cusp is a point where two arcs of a curve intersect and the tangents of each of those 25 arcs are equal. Thus, once the locus of cusp points have been determined, one can determine how to transition between different solutions of the direct kinematics problem by making closed-loop paths that encircle any of the curves that are associated with the locus of cusp points (Urízar et al., 2012). By doing this, the transition of solutions will be nonsingular as long as the patch generated by the loop is crossed by the agreeing curve

(Urízar et al., 2012). This is one method that can be used in the direct kinematics problem to ensure no singularities in the transition of assembly modes. Other ways have been presented to avoid singularity conditions of the 3-RPR system.

Husty and Gosselin (2008) presented two different methods in determining the singularity surface of a 3-RPR system. The first method presented indicates a series of equations that leads to the determination of the Jacobian Matrix of the system. Once the

Jacobian matrix is obtained, calculating when the determinant of the Jacobian is zero indicates that the system is in a singular pose. By specializing the design parameters, the singularity surface of a chosen design is obtained (Husty and Gosselin, 2008).Husty and

Gosselin explain that the second method in determining the singularity equation is found by understanding that the 3-RPR parallel manipulator is in a singular configuration every time the three leg lines intersect. With this knowledge, one may take a point in the platform system and multiply it with a transformation matrix to obtain the coordinates in a fixed system. Then, one would compute the line joining the platform point with its equivalent point in the base (Husty and Gosselin, 2008). This leads to a set of equations that can then be placed in a 3x3 matrix. As was seen in the first method, obtaining when the determinant of this matrix is equal to zero will determine the singularity surface of a chosen design (Husty and Gosselin, 2008). All of the solutions of determining the 26 singularity conditions of a parallel manipulator must be taken into consideration. It has been seen in the case of the 3-RPR manipulator that the method of determining cusp points resulted in a method in which one could determine a path for the direct kinematics solution to take that would have the system transition through assembly modes to avoid nonsingular configurations. It was also seen that one may determine the singularity surface. By doing this, the possible configurations that may lead to singularities within the system were able to be avoided when designing the parallel manipulator.

1.3 Project Information

The purpose of this project is to investigate a parallel robotic system that could assist in the success of a project started by Professor Zhu’s team in the Avionics

Engineering Center known as the Windmobile Project. The Windmobile being developed seeks to obtain bas ic aerodynamic coefficients of UAVs with wingspan up to 8 feet, such as the Galah UAV shown in Figure 7 which will be used in the project by testing the aircraft at different angles of attack and sideslip angles. The team is conducting flight control research, and the attainment of the aerodynamic coefficients of the Galah aircraft will lead to the attainment of acceptable parameters for the autopilot controller gains and help tune the autopilot. 27

Figure 7: Galah aircraft (Dibenedetto, 2012 a)

This method proves to be more cost effective than customary methods. For instance, it would be relatively easy to test an aircraft’s aerodynamic characteristics inside of a wind tunnel. However, it is prohibitive to purchase a wind tunnel of test articles with wingspans of several feet, and it is also very expensive to pay for using a preexisting wind tunnel. Plus, the Galah aircraft cannot be directly put into a wind tunnel because the wind tunnel tests articles must be custom made to satisfy stringent stress requirements to avoid losing parts from the test article damaging the wind tunnel equipment, which would be expensive. Therefore, it was important for the team to produce an idea to test the aircraft in an affordable manner.

The solution that the team devised was to mount the test aircraft on an instrumented truss system that is mounted onto a van. An image of the conceptual design can be seen in Figure 8. The truss system is attached to the front of the van and is comprised of metal tubing. The plane will be attached to the base frame of the parallel 28 manipulator that will be inside the fuselage of the aircraft. With this, the Galah aircraft will be attached on top of the truss system, by which they will be able to drive the van down a runway and gather flight characteristics. This method will prove to be considerably affordable and more accessible than testing UAV with a conventional wind tunnel. Though it will be cheaper, it is important to note that this method will not be as accurate as a conventional wind tunnel. However, the accuracy of the test method will be satisfactory for designing the autopilot controller gains and facilitate effective and safe tuning of the autopilot.

Figure 8: Truss system conceptual design (Zhu, 2012 b)

It is here where this research is needed. The team needs a system that can attach the truss system to the UAV, and then be able to control the aircraft’s yaw, pitch, and roll orientation angles in order to set and regulate the aerodynamic angles in the presence of environmental disturbances and road surface irregularities. Implementing a 3-RSR parallel robotic system into the model aircraft, this task will be able to be accomplished 29 by adjusting the legs of the parallel manipulator. Many parallel robotic systems have been investigated in the past; however, there has never been any analysis conducted on a

3-RSR system for this type of situation. The parallel robotic system will be implanted into the model airplane where the fuel system would normally be attached. The 3-RSR system will then be able to manipulate the aircraft’s yaw, pitch, and roll by adjusting the legs of the manipulator. The system will use three servo motors that will provide three degrees of freedom. Using three servo motors would be more cost efficient than purchasing six linear actuators to control the system such as the Stewart platform.

1.4 Thesis Objectives

The objectives of this thesis are to derive vector loop closure equations for the system, perform forward and inverse kinematics analysis on the proposed system, model a 3-RSR parallel manipulator, and develop a working prototype that may be used for the

Windmobile team. It is necessary to ensure that all of the parts fit together and are able to be oriented correctly for the system to function as it is intended to function. It is also necessary to produce a valid solution of the whole system’s operation to test the limit angles of the system. Models and animations will be conducted in MATLAB.

MATLAB will provide a good source in defining the joint angle limits and orientation of parts incorporated into the 3-RSR manipulator.

Deriving vector loop closure equations of the 3-RSR manipulator will lead to the development of the forward and inverse kinematics equations needed to analyze such a manipulator. The forward and inverse kinematics analysis will need to be evaluated by at least two methods to ensure that they are accurate. Also, a circular analysis will need to be conducted to insure that the equations are valid. This analysis will be able to certify 30 that the system’s motion is confined within a specific workspace. The kinematic equations will be calculated for a general case of the proposed system. Once the general equations of the kinematic equations of the 3-RSR parallel manipulator have been completed, a working model of the system specifically for the Windmobile project can be completed by testing different variable inputs to the equations. Since a general analysis will be performed, it would then be able to easily develop 3-RSR manipulators for other applications simply by implementing new variables into the existing equations.

Implementing vector analysis for a case in which a horizontal servomotor is introduced into the equations will provide a detailed analysis on whether or not all of the servomotors should be oriented vertically or not for a greater range of motion. Finally, once all of the MATLAB solutions and equations agree for a working design, a prototype of the 3-RSR parallel manipulator can be manufactured and tested.

31

2 Kinematics Analysis

2.1 Vector Loop Closure Equation

In order to begin any analysis on the 3-RSR manipulator, vector loop closure equations must be drawn up for the proposed system. A generalized model of the system used for the equation can be seen in Figure 9. The robot consists of three Revolute-

Spherical-Revolute (RSR) jointed legs in parallel, which connect the base to the moving platform. In this model Mi represent the servo motors position, Bi represent the revolute joints that attach legs Li to leg Ri, and ri represent the arms of the servo motors. {O} is the reference frame for the base of the robotic system and {P} is the reference frame for the moving platform. Angles ϕi and ϴi represent the angles rotating about Bi and Mi respectively. 32

Figure 9: Generalized model of 3-RSR system

Looking at the model we can conclude that there are 3-DOF. This can be seen by observing N= 8 links, J1 = 6 one-DOF R joints, and J3 = 3 three-DOF S joints. Therefore, the spatial Kutzbach mobility equation produces 3-dof (Williams, 2010):

MNJJJJJ6(  1)  51  4 2  3 3  2 4  1 5 M 6(8  1)  5(6)  4(0)  3(3)  2(0) M  3

Figures 10 and 11 show the top view of reference frames {O} and {P} respectively.

From Figure 10, points Bi are located by known constant polar coordinates (Ri, δi), i = 1,

2, 3, with respect to frame {O}. Thus, looking at Figure 11 it can be observed that points

Mi are located by known constant polar coordinates (Pi, εi), i = 1,2,3, with respect to 33 moving frame {P}. In general, the equations in this document are derived for the general

3-RSR parallel robotic system. For the base testing of the system, the perfect symmetry case should lead to significant simplifications in the equations. This case is expressed by the following:

RRRR1 2  3 

LLLL1 2  3  11330 L LL 90 AB2 22 33210 PPPPR1 2  3   (4)

r1 r 2  r 3  r

The fixed base points Bi are calculated with respect to {O} coordinates as:

BRix   icos i  0     Bi  BR iy   isin i  B  0  iz    (5)

Figure 10: Fixed Base Platform Kinematic Details 34

The moving platform points Mi are calculated with respect to {P} coordinates as:

Mix   P icos i r i sin i  P     Mi   Miy    P isin i  r i cos i  M   0  iz    (6)

Figure 11: Moving Base Platform Kinematic Details

It is seen that we have an equation for vector M in reference frame {P}, but say we want to project M on reference frame {O}. In order to do this, we must use an orthonormal rotation matrix (Williams, 2010). To do this we will use a typical Z-Y-X convention. The first step of this convention is to rotate about the z-axis with an arbitrary 35 angle we will call α. Then you rotate about the y-axis an arbitary angle that we will call

β. Finally, you rotate about the x-axis with an arbitrary angle that we will call γ. This convention is also called a standard 3-2-1 convention, since in order to perform this rotation, one multiplies a three rotation matrix, with a two rotation matrix, with a one rotation matrix. With this the orthonormal rotation matrix giving the orientation of the moving platform frame {P} with respect to the fixed base frame {O} is:

rrr11 12 13   cc sccss    sscsc    0             (7) P R rrr21 22 23   sc ccsss csssc 

r31 r 32 r 33    s c  s  c c  

From Figure 9 it can be observed that the closed loop vector equation for this system

0 0 0 0 0 would be: PP   Mi    r i    Bi    Li  i 1,2,3

(8)

We would like all of the vectors to be in reference frame {O}, however, we have vectors

Mi and ri projected on frame {P}. Thus, to put them into frame {O}, we simply multiply vector Mi in frame {P} with the rotation matrix that we calculated in equation 7. We do the same thing with vector ri. Thus, the equation ends up being:

0  0 PP 0  0  0 PPP  R   Mi   P R   r i   Bi   Li 

(9)

To simplify the vector loop closure equation, a virtual center leg of the system was introduced with a passive spherical joint. Figure 12 shows an example of this situation. 36

Figure 12: Model of RSR system with virtual middle leg

In this figure, LA and LB represent the bottom half and top half of the virtual center leg respectively. From a vector loop-closure equation of the virtual central leg of the robot

0 with the passive spherical joint,PP  is calculated:

00        0 0 0   0 PLLP  A  B  00 P R       LLAB    0   ( s s  c  s  c  ) L  r L     BB 13  0    ( c s   s  s  c  ) LBB  r23 L  L   c c L  r L L  A   B  33 BA  (10) 37

0 Looking at Figure 13, vector Li  can be derived, where i are the three fixed

and given base angles (see Figure 10), and i are the three variable and unknown passive intermediate angles. That vector is determined to be:

L cos cos i i i 0    Li  Licos i sin i i 1,2,3 (11) L sin ii

P With the aid of Figures 14, vector ri  can be derived, where i are the three

fixed and given platform angles (see Figure 11), and i are the three variable and active joint angles, controlled by the servomotors. That vector is determined to be:

 ricos i cos i  2 ricos i sin i   Pr r cos sin    r cos cos  i   i i i   i i i  (12)  2    riisin r sin ii  38

Figure 13: {L} Kinematic Detail 39

Figure 14: {r} Kinematic Details

The Windmobile project’s 3-RSR manipulator would like to investigate the rotation of one or more of the servomotors by 90 degrees. This is proposed in an attempt 40 to determine if rotating the motors could be beneficial towards obtaining greater yaw

th P angles. If the j servomotor is rotated by 90 degrees, its vector rj  can be derived by referring to Figure 15. Looking at this, it is found that with the servomotor rotated the vector becomes:

    rrjcos j     j  jcos j   j      P     rj   r jsin i     j     rjsin j   j      00     (13)

Figure 15: {r} Kinematic Details with Rotated Servomotor

Substituting all of the appropriate parameters and derived terms into the vector loop- closure equations (9) yields (for no rotated servomotors):

41

rL13 B rrrM  rrr  rcs   B Lcc     11 12 13 ix  11 12 13 i i i  ix   i i i              rL23 B   rrrM21 22 23 iy   rrr21 22 23   rcci i i   B iy   Lcsi i i              r33 LBA L r31 r 32 r 33 00 r31 r 32 r 33  rsi i  Li s i 

i 1,2,3 (14)

r L  r M r M rrcs  rrcc    rrs    B Lcc  13 B 11ix 12 iy 11 i i i12 i i i13 i i ix i i i      rL23 B   rM21ix  rM 22 iy    rrcs21 i i  i  rrcc22 i  i  i  rrs23 i  i   B iy L i c i s i        r L L  rrcs  rrcc    rrs   33 BA  r31 Mix r 32 M iy  31 i i i32 i i i33 i i  Lsii

(15)

Finally, the vector loop-closure equation yields three scalar equations for each of the three in-parallel legs. Many terms are shared amongst all nine scalar equations, notably the Euler angles , ,  included in the rij terms of (15), defined from (7).

rL13B rM 11 ix  rM 12 iy  rrcs 11 iii   rrcc12 iii    rrs13 ii   B ixiii  Lcc    0 rL rM  rM  rrcs   rrcc    rrs   B  Lcs    0 23B 21 ix 22 iy 21 iii22 iii23 ii iyiii rL33 BA L  rM31 ix  rM 32 iy  rrcs 31 iii   rrcc32 iii    rrs33 iiii   Ls   0

(16)

2.2 Inverse Orientation Kinematics Solution

Now that a vector loop closure equation has been determined for the system, a solution for the inverse orientation kinematics (IOK) may be obtained. The IOK problem

0 is as followed: Given the desired platform Cartesian orientation P R specified by the

T Euler angles α, β, γ, calculate the three active servomotor anglesΘ  1  2  3  .

This IOK solution is useful in controls in that one may specify what the platform orientation should be for all time and then the inverse orientation kinematics solution calculates the three servo motor angles at each time instant to achieve the given attitudes. 42

The inverse orientation kinematics can be solved for each leg independently. Given this, equations (16) for each leg i are overconstrained. This can be seen by observing that there are nine scalar equations and only six unknowns (θi, ϕi) for i = 1, 2, and 3. To obtain a solution geometrically, the inverse solution for each leg requires the intersection of two circles in different planes. One circle of radius Li from fixed center Bi and the other circle of radius ri from known center Mi which was derived from the given α, β, γ.

We may obtain the IOK solution in closed form as follows. Being that the same steps apply to all three legs independently – the i notation may be dropped for clarity in the following equations. Selecting the Y and Z components of the vector-loop closure equations (16), we have:

LABCcos cos   sin   (17) LDEFsin cos   sin  

where:

r r c r s  A  22 21 s rr B  23 s

By r23 L B  r 21 M x  r 22 M y C  s

D r r31 s r 32 c 

E r33 r

F r33 LB  L A  r31 M x  r 32 M y

Squaring and adding both equations in (17) will eliminate the unknown ϕ. This will result in a single equation that contains the one unknown θ. The result of this process is given in (18). 43

acos22 a sin   a cos  sin a cos   a sin   a  0 (18) 1 2 3 4 5 6 where:

22 a1  A D 22 a2  B E

a3 2 AB DE 

a4 2 AC DF 

a5 2 BC EF  2 2 2 a6  C F L

Equation (18) can be solved for the unknown θ by using the well-known tangent half-angle substitution, which is stated as:

 1t 2 2t If t  tan  then cos  and sin  2 1 t 2 1 t 2

The tangent half-angle substitution applied to (18) yields the following.

A t4 A t 3  A t 2  At  A  0 (19) 4 3 2 1 0 where:

A4 a 1  a 4  a 6

A3 2(  a3  a 5 )     A2 2( a1 2 a 2 a 6 )

A12( a 3 a 5 )

A0 a 1  a 4  a 6

Equation (19) is solved for the four roots for dummy substitution variable t. Then the solution to the IOK problem, for leg i, is found by inverting the tangent half-angle definition:

  2tan1 (t ) (20)

Then the solution for eliminated passive intermediate variable ϕ is:

44

  atan2DEFABC cos sin   , cos   sin    (21) where atan2 is the quadrant-specific inverse tangent function. Since (19) is a quartic polynomial, there will be four possible solutions for θ and ϕ, for each of the three legs i 1,2,3.

2.3 Forward Orientation Kinematics Solution

The forward orientation kinematics (FOK) problem is stated as: Given the three

T active servomotor anglesΘ  1  2  3  , calculate the platform Cartesian

0 orientation P R . This FOK solution is useful for parallel manipulator simulation. It is also useful for certain control algorithms. In general, the FOK solution for parallel manipulators is extremely difficult. The FOK requires the solution of multiple coupled nonlinear (transcendental) algebraic equations, from the vector loop-closure equations.

With this, multiple valid solutions generally result. As opposed to the inverse orientation kinematics problem, the forward orientation kinematics problem cannot be solved independently for each leg. It requires the equations (16) for all three legs i = 1, 2, 3 to be considered together. This set of equations is overconstrained in observing that there are nine scalar equations yet only six unknowns α, β, γ, ϕ1, ϕ2, ϕ3.

It is possible for one to start the FOK solution from equation (18) from the IOK solution process, where unknown ϕi has already been eliminated. However, there is an alternate method for the FOK solution that uses the Y and Z components of the vector- loop closure equations (16) that will be used. Using the Y and Z components of the vector-loop closure equation and writing the set of equations three times, once for each leg i = 1, 2, 3, yields six equations that are coupled with the six unknowns α, β, γ, ϕ1, ϕ2, 45

ϕ3. Rewritten below are the Y and Z components of equations (16). The set of equations are rewritten with the i subscript back in place and also showing the α, β, γ dependence explicitly.

Y:

(cs  sscL)()(B  scM ix  cc   sssM)()iy  scrcs i i i

(cc  sssrcc)(i i i   cs   sscrs) i i B iy  Lcs i  i i  0

Z:

()()()()ccLL  BA   sMix  csM  iy   srcs i  i  i

()()csrcc i  i  i  ccrs  i  i  Ls i  i  0

i = 1, 2, 3 (22)

Using the Newton-Raphson iterative approach, one would be able to numerically solve these six coupled nonlinear algebraic equations for the six unknowns α, β, γ, ϕ1, ϕ2,

ϕ3. The Newton-Raphson Method involves numerical iteration to solve coupled sets of n nonlinear equations in n unknowns. Varedi, Daniali, and Ganji (2009) explained that the

Newton-Raphson method has an efficient convergence speed, however is sometimes difficult to use due to the fact that it requires a good initial guess value. For this system, the Newton-Raphson Method is an adequate method for solving the FOK, since we will be able to obtain a good initial guess from the IOK of the 3-RSR system. It is also known that this method will provide a sufficient convergence speed which is desirable in the evaluation. When using the Newton-Raphson Method, it is generally found that the form of the given functions to solve is stated as F() X 0. For the 3-RSR parallel robot, the six functions to solve simultaneously are 46

T FXXXXXXX()()()()()()() FFFFFF1 2 3 4 5 6  . The following tables show the arrangement of these functions Fk(X), and the six variables are

T X     1  2  3 .

Table 1: Arrangement of functions Fj(X) from equations (22)

k row index i leg index (22) component

1 1 Y

2 1 Z

3 2 Y

4 2 Z

5 3 Y

6 3 Z

Table 2: Arrangement of variables X in equations (22)

j column index variable

1 

2 

3 

4 1

5 2

6 3 47

In order to derive the Newton-Raphson iteration for the system, a Taylor Series expansion of F about X has to be performed. This produces the following equation:

6 F   k    2 FFk ()()XXXk  xj O()X k 1,2, ,6 (23) j1 x j

F ()X JJX() k is the 6x6 Newton-Raphson Jacobian matrix. This matrix is a NR NR  x j multi-dimensional form of the derivative and function of X. Looking at equation (23) it can be determined that if X is small, the higher-order terms in the Taylor Series expansion can be neglected. In order to obtain a solution for the forward orientation kinematics, it is required that:

Fk (XX ) 0 (24)

Considering that the higher-order terms in the Taylor Series expansion are negligible due to being small, our equation becomes:

6 F   k     FFk ()()XXXk  xj F k ()X JNR X 0 in 1,2, , (25) j1 x j

Knowing this it is important to determine the required correction factor at each solution iteration step. In order to calculate this, must be solved from

F() X JNR X 0which leads to:

 XJFX 1 () (26) NR Observing equation (26), it would be best to obtain a solution through Gaussian

elimination on JXFXNR  () since this route would be more robust and efficient than having to do matrix inversion. 48

Thus, there are essentially four steps in the Newton-Raphson iteration method for solving the FOK. One thing that needs to be done before beginning the method is to establish the functions and variables to solve for F(X) = 0. The first step of the Newton-

Raphson method is to make an initial guess to the solution, X0. Then one must solve

JXXFXNR()() m m  m forXm , where m is the iteration counter. Once that has been

done, the third step is to update the current best guess for the solution, XXXm1  m m .

The fourth and final step is to iterate until Xm  , where the Euclidean norm is used and is a small scalar solution tolerance that is defined by the user. It is important also to terminate the iteration if the number of steps becomes too high. If this happens the solution is diverging. If the initial guess to the solution X0 is suitably close to the actual solution, quadratic convergence using the Newton-Raphson method is guaranteed.

As mentioned the Newton-Raphson method for parallel manipulators such as the

3-RSR manipulator being evaluated requires a good initial guess in order to ensure convergence and produce only one of the multiple solutions. Avoiding a bad initial guess in this situation is easily avoidable since the existing pose configuration is already known. This configuration makes for an excellent initial guess for the next solution step as long as the control rate is high, since the robot will not be able to move too far from this initial guess. This is also a great initial guess as it will most likely be found that the one resulting solution is the solution of the actual configuration of the robot since it will be close to the last configuration. The only time that this would not be the case would be if there were singularities in the configuration that would allow for multiple branches to converge. 49

In the case of the 3-RSR parallel manipulator, the FOK Newton-Raphson

Jacobian matrix is:

FFFFFF111111()()()()()()XXXXXX            1 2 3

FFFFFF222222()()()()()()XXXXXX            1 2 3

FFFFFF333333()()()()()()XXXXXX  F ()X       1  2  3 (27) JX()k  NR  x j FFF444()()()XXXFFF444()()()XXX    1  2  3 FFFFFF()()()()()()XXXXXX 555555       1  2  3 FFFFFF()()()()()()XXXXXX 666666       1  2  3

Luckily, for the first three columns it is only necessary to derive the first two rows of the matrix, for leg i = 1. The first three columns of the middle two and last two rows follow the same pattern, for legs i = 2, 3, respectively. Thus, those derivations are calculated to be:

F X  21i ss  cscL () ccM    sc   cssM  () ccrcs   B ix iy i i i

 sc  cssrcc i  i i   ss   cscrs  i  i

F X  2i  0 

i 1,2,3 (28)

50

F X  21i  ()()()sccL    ssM    scsM     B ix iy

()()() ss rcsi  i i  scsrcc i  i i  sccrs i  i

F X  2i ()()() s c  L  L   c M   s  s  M  BA ix iy

()()() crcsi  i  i   ssrcc  i  i  i   scrs  i  i

i 1,2,3 (29)

F X  21i  cc  sssL    cs   sscM   cs   sscrcc   B iy i i i          c c s s s rsii

F X  2i ()()()() csL  ccM   ccrcc   csrs   B iy i i i i i

(30)

The last three columns of the Jacobian matrix are simple to derive from equations

(22) and have been given in full detail as follows:

Ls1 1s 1 0 0   Lc11 00 0 Lss 0 JX() 2 2 2 (31) NR4,5,6 00Lc 22 0 0 Lss 3 3 3 00Lc33

51

3 Kinematics Results

3.1 Inverse Orientation Kinematics

The first results obtained were those of the inverse orientation kinematics of the 3-

RSR robotic manipulator. The following sections show the process of obtaining an acceptable model for the robotic system based on the results obtained from the inverse orientation kinematics.

3.1.1 Obtaining the IOK Solution

Before any of the kinematic solutions were tested, as was done in the derivation of the equations, a vector loop closure equation program was created so that the solutions obtained in the kinematics testing could then be verified. However, for this to work out properly, the vector loop closure equation program had to first be tested to ensure that it ran properly and produced the correct results. For the vector loop closure equation, we know that:

0  0 PP 0  0  0 PPP  R   Mi   P R   r i   Bi   Li  in order for the system to be considered closed. With this in mind, the MATLAB program for the vector loop closure equation tests that the left side of (9) equals the right side of (9). Plugging in known values that should result in a closed system would test that the program runs as designed. With this in mind, the first leg of the 3- RSR system’s starting orientation was tested were θ = 0, ϕ = 90, ϵ = 330, δ = 330, α = 0, β = 0, and γ =

0. This resulted in a solution were the right side of equation (9) did in fact equal the left side of equation (9). Once the vector loop closure equation program was functional, the 52 kinematic problems were able to be programmed and tested. In each of the programs, the general case numbers (4) were used for testing of the 3-RSR system.

Acquisition of numerical results for the kinematics problems were obtained through

MATLAB. In the case of the IOK solution, the first program written was designed to take in a single variable input for α, β, and γ which would be used in the rotation matrix to obtain the θ and ϕ outputs of the solution for all three legs. As was mentioned previously, since (19) is a quartic polynomial, there will be four possible solutions for each leg in the IOK solution. However, it is known that the IOK solution is obtained geometrically through the intersection of two circles, thus there can only effectively be two real solutions for each leg, one referred to as the open branch solution, and one referred to as the crossed branch solution. With this in mind, of the four possible solutions obtained, two of them will always result in imaginary solutions. This had to be accounted for in the programming of the IOK solution. Also, if it is found that all four possible solutions for a leg result in imaginary solutions, this indicates that the system cannot obtain the desired rotation set by the variable inputs and therefore should be noted as an unattainable rotation by the system. As was observed through the vector loop closure equation, when α, β, and γ are equal to zero, θ = 0 and ϕ = 90 for legs i = 1, 2, 3.

Thus, this should effectively be the solution for the IOK solution if one were to use zero for the variable inputs α, β, and γ in the IOK solution program. This was tested and verified.

One result that should be noted is that when testing the IOK solution to the vector loop closure equation, the y and z components of the vector loop closure equation zero out when the correct answer from the IOK solution is inputted into the vector loop 53 closure program, however, it is found that the x component does not zero out as the y and z components do. An example of this follows: when observing the general case and using variable inputs of α = 10°, β = 0°, and γ = 0°, it was found that the IOK solution for leg one of the system obtained results of 1.588° and 84.9838° for θ and ϕ respectively.

Inputting these results from the IOK solution into the vector loop closure equation gave a

.591  solution of 0 when the right side of the equation was subtracted from the left side.  0

As can be seen, the y component and the z component of the solution both zeroed out, yet the x component did not. This is due to the fact that when deriving the IOK solution from the vector loop closure equation, the x component of equation (16) was ignored, while the y and z components of equation (16) were squared and added together to obtain the final solution for the IOK problem. For this reason, it will be found that when doing a circular check of the IOK solution with the vector loop closure equation program, the y and z component should zero out, while the x component of the solution can be ignored.

Once the IOK solution had been tested in MATLAB for single variable inputs of α,

β, and γ, it was necessary to create a program that would be able to test multiple inputs at one time to determine what the rotational capabilities of the system were in a fast and effective manner. For this reason, a program was first created to test the rotational capabilities of α for the system, while having fixed β and γ at zero degrees. The program

IOK_Solution_Alpha.m tests the system from α = 0° to α = 360° with a change of one degree per test. It is found that each test produces four possible solutions per leg of the system, as was seen with the single input program. Once the program has run through its 54 full cycle, a text file containing the results was created which was then able to easily be converted into an excel spreadsheet to view the results. Two other programs,

IOK_Solution_Beta.m and IOK_Solution_Gamma.m, were created similar to this one for the testing of rotation about β and γ respectively.

Looking at the results, if the input produced a solution containing all imaginary results for the four possible solutions, this input could be considered to be unattainable by the system. If, on the other hand, two of the solutions were found to be real solutions, this could be considered an attainable angular rotation for the system. An example of the results observed can be seen in Table 1. This is just a small sample of the resulting excel spreadsheet, as the information within the spreadsheet is extensive to ensure full detail about the system could be observed in a short timeframe. The full spreadsheet includes the results for all three legs of the system, and also does the cases for rotation about β with fixed α and γ angles and the case of rotation about γ with fixed α and β angles.

Obtaining the solutions in this fashion proved to be a great timesaver, as one would have had to use the single variable input program for the IOK solution, and check each angle one at a time to see if that rotational angle was obtainable or not by the robotic system.

55

Table 3: IOK_Solution_Alpha.m sample

The above sample is obtained by the MATLAB program IOK_Solution_Alpha.m which can be found in Appendix A. In this example, vector lengths L, R, P, and r were set to 6 in., 2.5 in., 2.5 in., and 1 in. respectively. To clarify, th1, th2, th3, and th4 within the sample represent the four possible θ solutions to the IOK problem for the given inputs and ph1, ph2, ph3 and ph4 show the four possible ϕ solutions. Also, it can be noted that th1 would result in the solution of ph1; th2 would result in the solution of ph2, and so on.

As can be seen for the general case, it was found that setting β and γ to zero and rotating

α, it is inferred that the system could obtain a yaw rotation of 27° since there were two possible solutions for θ and ϕ up to this point. However, once the system tried to rotate about α past this, it was found that all four possible solutions for the IOK problem were imaginary solutions, or as MATLAB has specified, not a number (NAN). The system, for the general constants, can only obtain a yaw rotation of up to 27° before it fails.

This, of course, is only considering the first leg of the three legged system. Thus, one must check all three legs and observe the allowable rotation for each leg. One leg of 56 the system may suggest that that particular leg can reach a yaw angle of 40°, but if one of the other legs results show that it can only obtain a yaw angle of 27°, than that is in fact the maximal rotation that may be obtained by the system as a whole. This is also the case when observing roll and pitch rotation. All three legs must have real solutions for that particular rotation angle to be considered attainable by the robotic system as a whole.

3.1.2 Graphical Interpretation of the IOK Solution

The excel file method is an effective method for reading all of the results and observing at exactly what angle the robotic system fails. However, this method lacks a quick visual aid for a large audience. It requires thoroughly investigating the spreadsheet to obtain an understanding of the system and its yaw, pitch, and roll capabilities.

Therefore, it is necessary to provide a graphical interpretation of the IOK solution. The graphical results of the IOK solution for the general case model are provided within this section. Using graphical interpretation of the solution provides for an effective method to gaining a general understanding of the robotic systems rotational capabilities and understanding how specific changes of the robotic system would affect the rotational capabilities of the system. With this in mind, the final set of programs written in

MATLAB for the IOK solution allowed for the visual representation of the rotational capabilities of yaw, pitch and roll of the robotic system obtained by the IOK solution.

Figure 16 demonstrates a general case scenario of the system where lengths L, R, P, and r of the system are equal to 6 in., 2.5 in., 2.5 in., and 1 in. respectively.

57

Figure 16: General Graphical Interpretation of IOK Solution with variable α

As can be observed, there are two possible solutions within each plot, one representing the crossed-branch solutions and the other representing the open-branch solutions. As can be seen in the first sub-plot, as was seen in the sample of the excel spreadsheet, there are real solutions for θ and ϕ when α has a rotation between 0° - 27°.

After this there is a blank section in the plot, this section should be noted as the unattainable rotation sections for that leg of the system. Still looking at the same subplot, from 173° - 360° it can be observed that there are real solutions for the first leg of the system. This information suggests that from the starting position, for the first leg the system can rotate in the opposite direction about α until it reaches -187°, any rotation after this in that direction, the system will fail. 58

As was mentioned previously, the system as a whole can only obtain rotations that are fitting for all three legs. Looking at the graphical representation of the IOK solution gives a good understanding as to how much rotation can take place before the system would reach a singularity condition. Note that leg 3 shows much greater potential for rotation about α than leg 1. However, since leg 1 can never attain the rotation of leg 3, the system is limited to leg 1’s rotational capabilities. If one were to rotate about α in the negative direction, it can be observed that for this case leg 1 has greater potential rotational capabilities than leg 3 does, but for this case the system would be limited to leg

3’s rotational capabilities in this direction. This will be the case for all graphical interpretations. For any rotational capabilities in question, the system will have the potential to rotate to whatever angle the leg with the most restriction in that direction will allow.

Now that there is a general solution for the IOK problem, the constant vector lengths L, R, P, and r should be changed to observe how this affects the rotational capabilities of the system. Since all of the examples thus far have been for rotation about the z-axis, which is effectively the yaw rotation of the system and is denoted by α, we will observe first how changes in each of the vector length constants affect this rotation.

In Figure 17, the input length L has been changed from 6 inches to 10 inches. As can be seen in the new plots, there has been an increase in the attainable angles of the system for each leg. For example, leg 1 of the system has went from having an attainable angle of

0° - 27° and 173° - 360° to having attainable angles of 0° - 37° and 164° - 360°.

Observing this, the attainable angles for yaw of the system have increased with an increase in the input length L. To be sure of this, one must also test whether or not 59 shortening length L will decrease the attainable angles for yaw of the system. As can be seen from Figure 18, this is the case. A decrease in the length of L did in fact cause a decrease in the attainable angles for yaw of the system.

Figure 17: Change in α vs θ and ϕ with L = 10in

60

Figure 18: Change in α vs θ and ϕ with L = 5in

Now changing constant lengths R and P will be tested to observe how this affects the attainable angles for yaw of the system. Figure 19 shows an increase in the constant lengths R and P from 2.5 inches to 3.5 inches. For this case, constant length L has been reset to 6 inches. This will ensure that only results due to the changes in lengths R and P will be observed in the plots.

61

Figure 19: Change in α vs θ and ϕ with R = P = 3.5in

Observing Figure 19 will lead to the discovery of real solutions within the unattainable rotation region of the plots. It needs to be understood that this does not mean that the system can attain these positions, as the system would have reached singularity conditions before it was ever able to obtain these orientations. Therefore, in future cases were floating real solutions occur within the unattainable region of the plots, these solutions should be and will be ignored. Therefore, looking at leg 1 in Figure 19, the correct way to read a plot such as this is to realize that leg one has attainable angles of 0°

- 19° and 307° - 360°. The system will not be able to attain yaw angles of 189° -270° as the floating real values may suggest. With that in mind, viewing of the plots in Figure 19 compared to those of Figure 17 shows that increasing the constant lengths R and P have caused a decrease in the attainable yaw angles of the system. Figure 20 shows a case in 62 which constant lengths R and P have been decreased from their starting length of 2.5 inches to 1.5 inches. Observations of these plots conclude that a decrease of constant vector lengths R and P cause an increase in the attainable yaw angles of the system as a whole.

Figure 20: Change in α vs θ and ϕ with R = P = 1.5in

In fact, observation of leg 2 in Figure 20 shows for the first time a case in which the leg would be able to rotate the full 360 degrees without reaching any singularity conditions.

Obviously this would not be able to happen due to the restrictions set by leg 1 and 3; however this is still an interesting case.

The last constant length for the robotic system to check is length r. Originally length r was set to one inch. Figure 21 shows a case in which constant length r has been 63 changed from one inch to two inches and Figure 22 shows a case in which constant length r is set to half of an inch.

Figure 21: Change in α vs θ and ϕ with r = 2in

As can be seen from Figure 21, increasing constant vector length r from 1 inch to 2 inches has caused the attainable yaw angle of leg 1 to increase from 0° – 27° and 173° –

360° to 0° – 33° and 147° – 360°. Checking Figure 22 verifies that while an increase in length r increases the attainable yaw angles, a decrease in length r will cause a decrease in the attainable yaw angles of the robotic system. For leg one in Figure 22, the attainable angles for yaw dropped to 0° – 21° and 197° - 360°.

64

Figure 22: Change in α vs θ and ϕ with r = 0.5in

Now that a general understanding of what the effects are of changing the constant vector lengths of L, R, P, and r do to the system in relation to the attainable yaw angles of the system, it is important to also investigate how such changes would affect the relation of attainable angles in pitch and roll. The first of these two rotations that will be investigated is the pitch angle. Figure 23 shows a graphical solution for rotation about the y-axis, or the pitch rotation and is denoted by β. Figure 24 demonstrates the case for which vector length L has been increased from 6 inches to 10 inches. Figure 25 demonstrates a case for which vector length L has been decreased to 5 inches. In the general case seen in Figure 23, the attainable pitch angles found for the system range from 0° - 23° and from 336° - 360°. Thus, it is observed through Figure 24 that by increasing the constant vector length L, one decreases the maximum attainable pitch 65 angles of the system as a whole. This is deduced by noting that in Figure 24, the maximum attainable pitch angles of the system are found to range from only 0° – 18° and from 334° - 360°. It is important to note that in these plots a vertical line is fond when θ is -180 or 180 degrees. This is because the program jumps from plotting the negative solution for θ and instead starts plotting its positive angular counterpart i.e. -181° is the same as 179°.

Figure 23: General Graphical Interpretation of IOK Solution with variable β 66

Figure 24: Change in β vs θ and ϕ with L = 10in

Figure 25: Change in β vs θ and ϕ with L = 5in

67

Investigating Figure 25 further demonstrates that with an increase in vector length L, there is a decrease in the attainable pitch angle of the system. The maximum attainable pitch angles obtained when L = 5 inches ranged from 0° - 26° and 338° - 360° which was a larger range than that found for the general case where L = 6 inches.

Figure 26 shows the case where vector lengths R and P have been increased from

2.5 inches to 3.5 inches. In this case it is noted that the ability of the system to rotate about the y-axis decreases when there is an increase in the lengths of vectors R and P.

Looking at Figure 26, the plot suggests that the attainable pitch angle of the system is limited to leg 1, which presents a case for the system to rotate from 0° – 19° and from

345° - 360°. This is a 4° drop in the positive directional rotation of the system and a 14° drop for the system if the servo arm were to be rotating in the negative direction.

Figure 26: Change in β vs θ and ϕ with R = P = 3.5in 68

Figure 27: Change in β vs θ and ϕ with R = P = 1.5in

Observation of Figure 27, which shows the case in which vector lengths R and P have been decreased to 1.5 in., shows that decreasing these lengths leads to an increase in the attainable pitch angle of the system. Figure 27 suggests an attainable pitch range of the system of 328° - 28°. This is an increase in the positive rotation of 5° and an increase of

8° in the negative direction for the entire system. Thus, this information further demonstrates that an increase in the lengths of vectors R and P cause a decrease in the attainable pitch angles of the robotic system as a whole.

The next observation of the system was determining what would happen to the maximum attainable pitch angles of the system if the length of vector r was changed.

Figures 28 and 29 demonstrate this particular case by presenting the attainable angles for the system when vector r is set to 2 in. and 0.5 in. respectively. 69

Figure 28: Change in β vs θ and ϕ with r = 2in

Looking at Figure 28, one can observe that leg 1 has almost no limitation of rotation for pitch and leg 3 is in fact able to rotate from 0° - 360° mathematically with no singularity conditions when vector r is equal to 2 in. Therefore, the system for this particular case is limited to leg 2 of the system, which suggests maximum rotation about the y-axis from

320° - 107°. These results show a significant increase in the system’s attainable pitch angle from when the system had a length of 1 in. for vector r. Looking at Figure 29, one can observe that the system becomes more limited in its ability to rotate about the y-axis when vector r is decreased to a length of 0.5 in. In this case, it is observed that the plots suggest only a pitch range of 348° – 11° for the system as a whole. The results obtained from these two plots strongly demonstrate that an increase in the length of vector r leads to the increase in the overall maximum pitch angle that may be obtained by the robotic system. 70

Figure 29: Change in β vs θ and ϕ with r = 0.5in

Now that a general understanding of what the effects are of changing the constant vector lengths of L, R, P, and r do to the system in relation to the attainable yaw and pitch angles, we may now investigate what these changes would do for the attainable roll angles of the robotic system. Figure 30 shows a general graphical representation of the

IOK solution with variable γ inputs. This plot will be used to as a comparison to the other plots obtained in this section to determine how each length change affects the capabilities of roll angles within of the system. In order to compare the other results that will be obtained once the vector lengths are changed, it must be noted that the general vector lengths obtained a maximum range of rotation for roll of the system of 340° - 25°.

As observed, this is mainly due to the limitations found within leg 2 of the system. These results are observed in Figure 30. 71

Figure 30: General Graphical Interpretation of IOK Solution with variable γ

Figure 31 presents the case in which the vector length L has been increased from

6 in. to 10 in. When this is the case, it was found that there is an increase in the robotic systems attainable roll angles. Investigating the results of the graphical solution showed a roll angle range of 341° - 30°. Even though it is observed that there was a decrease in the system’s ability to rotate in the negative direction by one degree, there was an increase in the positive direction of rotation of five degrees, thus giving a net gain in the range of rotation of four degrees for this case. However, this was not as significant of a difference in rotational capabilities as has been seen in the other cases observed thus far.

Furthermore, looking at the results obtained from Figure 32, which demonstrated the case in which vector L was decreased from 6 in. to 5 in., produced absolutely no change in the overall system’s roll rotation capabilities. Both Figure 30 and 32 were found to have an 72 overall system roll rotation range of 340° – 25°. This information suggests that an increase of vector length L may produce an increase in the attainable roll rotation of the robotic system; however it is not that significant of an increase.

Figure 31: Change in γ vs θ and ϕ with L = 10in 73

Figure 32: Change in γ vs θ and ϕ with L = 5in

The next test conducted observed the results of the system’s roll rotation with the change of vector lengths R and P. Observing Figure 33, which shows the results for the system with vector lengths R and P equal to 3.5 in., it can be deduced that an increase in vector R and P’s lengths produce a decrease in the systems overall capabilities of rolling.

The plot demonstrates that the system went from being able to rotate from 340° – 25° to only being able to rotate from 345° - 17°. This is an overall decrease of 13° of roll rotation for the robotic system with this change in the length of R and P. Observation of

Figure 34, which shows the results for the system with vector lengths R and P equal to

1.5 in., show that a decrease in the lengths of vectors R and P produce results in which there is an increase in the robotic system’s rotational capabilities in roll. There was an observed rotational range for roll of 331° – 98° for this case. This shows an overall increase in the system’s ability to roll by 82°. 74

Figure 33: Change in γ vs θ and ϕ with R = P = 3.5in

Figure 34: Change in γ vs θ and ϕ with R = P = 1.5in 75

Last result to investigate was to determine what a change in the length of vector r would do to the roll rotational capabilities of the system. Looking at Figure 35, we can observe the resulting solutions when vector length r was changed from 1 in. to 2 in. In this case, the roll rotation showed an increase in the system’s roll rotational range from

340° – 25° to 323° – 57°. This was an overall increase in the rolling capabilities of the system by 49°. Then looking at Figure 36, it can be observed that the system as a whole became more limited in its ability to roll when vector r was shortened from 1 in. to 0.5 in.

The results of this test showed an overall range in roll of 350° – 12° which gave the system an overall decrease of 23° in rotational capabilities for roll of the robotic system.

The results of these two plots result in the conclusion that an increase in the length of vector r produces an increase in the roll rotational capabilities of the robotic system.

Figure 35: Change in γ vs θ and ϕ with r = 2in 76

Figure 36: Change in γ vs θ and ϕ with r = 0.5in

3.1.3 IOK Results for Windmobile Project

Designing an acceptable model for the Windmobile project proved to be a difficult case in that there was a very limited workspace for the robotic system, therefore the footprint of the servo motors had to be minimal while still maintaining acceptable constant lengths that would produce the desired limits for roll, pitch, and yaw proposed.

The metal board that the whole system must fit on in order to be orientated within the fuselage of the Galah aircraft has basically a 7” by 7.5” footprint. A model of the footprint that the robotic system must fit on can be seen in Figure 37. After testing of many different constant lengths, it was determined that acceptable lengths for vectors L,

R, P, and r would be 6.15 in., 1.7 in., 1.7 in., and 0.85 in. respectively. Using these specified lengths for each vector created a system that was able to be orientated onto the 77 acceptable footprint of the Galah aircraft, while producing acceptable solutions for yaw, pitch, and roll.

Figure 37: Allowable Footprint of Robotic System

Also, it was determined that in order for the robotic system to fit onto the acceptable footprint, the servo motors would need to be oriented at ε and δ equal to 316°,

44°, and 180° for legs 1, 2, and 3 respectively. However, trying to test this in MATLAB proved to be problematic in that this produced a situation where an algorithmic singularity manifested. That is, the leg set at 180°, when plugged into the IOK solution, would produce imaginary solutions for all rotations since it produced a case in which sin(180°) was in the denominator of the equations. Of course it is known that one cannot divide by zero, which is exactly what this specific orientation was trying to accomplish. 78

To avoid this algorithmic singularity, the coordinate system was rotated by 90°. By doing this, the servo motors were now essentially located at ε and δ were equal to 226°,

314°, and 90° for legs 1, 2, and 3 respectively. In doing this, the x-axis and y-axis of the coordinate system were essentially switched. Thus, it can be deduced that the results found for the rotation about the x-axis, in this case, showed the allowable rotation of roll for the system instead of pitch. Also, the rotation about the y-axis, instead of representing the allowable roll rotation of the system, represents the allowable pitch rotation of the system. Since rotation of the system had no effect on the z-axis, rotation about this axis still modeled the allowable yaw rotation of the robotic system.

Figure 38 models the graphical representation of the allowable yaw rotation of the

Windmobile system. As can be seen from the plots, the minimal positive rotation is found in leg 2 of the robot and was found to be at 62°. Looking at the negative rotation of the system, the minimal negative rotation is found in leg 1 of the robot and was found to be at 253°. Therefore, the attainable yaw angles for the system obtained through the

IOK solution were found to be 253° - 62°.

79

Figure 38: Change in α vs θ and ϕ for Windmobile Project

Figure 39 models the graphical representation of the allowable roll rotation of the

Windmobile system. As can be seen from the plots, the minimal positive rotation is found in leg 2 of the robot and was found to be at 21°. Looking at the negative rotation of the system, the minimal negative rotation is found in leg 3 of the robot and was found to be at 330°. Therefore, the attainable roll angles for the system obtained through the

IOK solution were found to be 330° - 21°. 80

Figure 39: Change in β vs θ and ϕ for Windmobile Project

Figure 40 models the graphical representation of the allowable pitch rotation of the Windmobile system. As can be seen from the plots, the minimal positive rotation is found in leg 3 of the robot and was found to be at 39°. Looking at the negative rotation of the system, the minimal negative rotation was also found in leg 3 of the robot and was found to be at 337°. Therefore, the attainable pitch angles for the system obtained through the IOK solution were found to be 337° - 39°. Of course, it should be noted that all results found at this point have been produced through the IOK solution. These results do show the joint limitations of the arms in coordination with each other; however, they do not account for joint limitations of the workspace itself, such as the servo arm would eventually collide with the metal board of the system when being rotated in the negative 81 and positive directions. Actual testing of the system within the workspace must be conducted to verify the attainable solutions of the robotic system.

Figure 40: Change in γ vs θ and ϕ for Windmobile Project

The Windmobile project wanted to also investigate a case in which one of the servo motors was flipped 90°. Equation (13) was implemented into the IOK graphical

MATLAB program and one of the resulting plots of this investigation can be observed in

Figure 46. This figure shows the rotational capabilities for pitch of the system with the third leg of the system rotated 90°. As can be seen from Figure 41, leg 3 is incapable of rotating in neither the positive nor negative direction. There is observed a section of real solutions, however they do not appear until the middle of the plot, which suggests that the system would never reach such an orientation. These results may suggest that attempting 82 to rotate one of the servo motors by 90° would result in a situation where the legs of the system would be unable to rotate or split apart from the system if they were forced to rotate. The same type of results seen for the attainable pitch angles in Figure 41 were found when investigating the attainable angles for yaw and roll with a flipped servo motor. In each case, there were no real results found in the positive or negative rotational directions, thus inferring that the system would be unable to rotate with this particular design.

Figure 41: Investigation of Rotated Servo

3.2 Forward Orientation Kinematics

Once the inverse orientation kinematics solution was obtained and tested, the forward orientation kinematics problem was investigated. Just as the IOK solution, the 83

FOK solution needed to be implemented and tested in MATLAB. Equation (26) had to

0 be solved within the program in order to obtain the platform Cartesian orientation P R

T from the given three active servomotor anglesΘ  1  2  3  . Equations (27) –

(31) were used in order to obtain the Jacobian matrix of the system. With all of the correct equations for the forward orientation kinematics implemented into the program, the program was then able to take the inputs, and using the Newton-Raphson method, obtain a solution to the FOK problem. For the problem, the variable inputs were θ for legs i = 1, 2, 3. Then an initial guess for α, β, γ, and ϕ for legs i = 1, 2, 3 had to be input into MATLAB.

For manipulator forward pose problems, the Newton -Raphson technique requires a good initial guess to ensure convergence of the solution and yields only one of the multiple solutions. As was mentioned earlier when describing the FOK, this does not present any difficulty since the existing known pose configuration makes an excellent initial guess for the next solution step assuming that there is a high control rate of the system which will prevent the robot from moving too far from the known initial guess.

The one resulting solution that is closest to the initial guess is generally the solution that is desired, as it is likely the actual configuration of the real robot. The time when this would not be the case would be if there was a singularity case that would cause the multiple solution branches to converge. With this in mind, the forward orientation kinematics program was compared to the results obtained from the inverse orientation kinematics program to ensure that the solution was correct. 84

First test that was conducted with the forward orientation kinematics solution was to present a case in which we are certain of what the solution is supposed to be with the given parameters. Therefore, θ for legs i = 1, 2, 3 was set to 0°, while ϕ for legs i = 1, 2,

3 was guessed to be 90°, and α, β, and γ were all guessed to be 0°. This is the general vector closure case and therefore should converge and give the same solution if the forward orientation kinematics problem was derived correctly. For the general tests of the FOK solution, the constant lengths for L, R, P, and r were set to 6 in., 2.5 in., 2.5 in., and 1 in. as has been seen for the testing of the general case for the IOK solution. When these values were plugged into the equation, the results were as expected and produced the same results that were guessed of the system within one iteration.

Once this had been done, a comparison of the IOK and FOK solution had to be done to perform a circular check of the equations to verify that both of them had been derived correctly. Two rotations for yaw, pitch, and roll were plugged into the IOK variable input program and the results were than used to gain a good initial guess for the

FOK problem. In these results θ1, θ2, and θ3 will denote the resulting θ solutions for legs

1, 2, and 3 of the system respectively. This same denomination holds true for the results of ϕ.

First, inputs α, β, and γ were set to 10°, 0°, and 0° respectively in the IOK problem, which suggest that the system had obtained a yaw angle of 10°. This resulted in θ1, θ2, θ3,

ϕ1, ϕ2, and ϕ3 equaling 3.0133°, 0.0069°, 2.4527°, 82.410°, 89.6373°, and 96.847° respectively. The results of θ1, θ2, θ3 were then put in as the input variables for the FOK problem and an initial guess of 10°, 0°, 0°, 82.4°, 89.6°, and 96.8° were used for α, β, γ,

ϕ1, ϕ2, and ϕ3 respectively. Given these inputs, the Newton-Raphson method was able to 85 calculate the results of the solution within one iteration and obtained solutions for α, β, γ,

ϕ1, ϕ2, and ϕ3 respectively as 10°, 0°, 0°, 82.4099°, 89.6373°, and 96.8475°. This was great in that all of the solutions obtained from the FOK problem were found to be within one-hundredth of a degree of the result obtained in the IOK solution. The second yaw angle tested was 20°. For this inputs α, β, and γ were set to 20°, 0°, and 0° respectively in the IOK problem. This resulted in θ1, θ2, θ3, ϕ1, ϕ2, and ϕ3 equaling 14.0747°, 0.1086°,

8.7873°, 73.6315°, 88.5601°, and 102.9569° respectively. The results of θ1, θ2, θ3 were then put in as the input variables for the FOK problem and an initial guess of 20°, 0°, 0°,

73.6°, 88.6°, and 102.9° were used for α, β, γ, ϕ1, ϕ2, and ϕ3 respectively. Given these inputs, the Newton-Raphson method was able to calculate the results of the solution within two iterations. The forward orientation kinematics program obtained solutions for

α, β, γ, ϕ1, ϕ2, and ϕ3 respectively as 20°, 0°, 0°, 73.6314°, 88.56°, and 102.9571°. Again, as was seen in the first test for yaw rotation the FOK problem was able to obtain the correct solution without conducting many iterations and was accurate to the one- hundredth degree.

The next two tests were conducted to verify that the forward orientation kinematics solution functioned correctly in the case were the pitch angle of the system was changed.

Therefore, the variable inputs α, β, and γ were set to 0°, 10°, and 0° respectively in the

IOK problem. This resulted in θ1, θ2, θ3, ϕ1, ϕ2, and ϕ3 equaling -25.7399°, -2.6418°,

19.3483°, 88.3586°, 90°, and 90.9342° respectively. The results of θ1, θ2, θ3 were then put in as the input variables for the FOK problem and an initial guess of 0°, 10°, 0°,

88.3°, 90°, and 90.9° were used for α, β, γ, ϕ1, ϕ2, and ϕ3 respectively. Given these inputs, the Newton-Raphson method was able to calculate the results of the solution within two 86 iterations and obtained solutions for α, β, γ, ϕ1, ϕ2, and ϕ3 respectively as 0°, 10°, 0°,

88.3583°, 90°, and 90.9344°. The second test for pitch angle tested the system if it were trying to obtain a pitch angle of 20°. Therefore the variable inputs α, β, and γ of the IOK solution were respectively set to 0°, 20°, and 0°. This resulted in θ1, θ2, θ3, ϕ1, ϕ2, and ϕ3 equaling -75.1997°, -10.7294°, 34.8487°, 77.5884°, 90°, and 92.9675° respectively. The results of θ1, θ2, θ3 were then put in as the input variables for the FOK problem and an initial guess of 0°, 20°, 0°, 77.5°, 90°, and 92.9° were used for α, β, γ, ϕ1, ϕ2, and ϕ3 respectively. Given these inputs, the Newton-Raphson method was able to calculate the results of the solution within two iterations and obtained solutions for α, β, γ, ϕ1, ϕ2, and

ϕ3 respectively as 0°, 20°, 0°, 77.5883°, 90°, and 92.9676°. After obtaining these results it was found that both of the tests conducted where the obtainable pitch angle was tested for the FOK solution both produced results that were obtained within two iterations of the

Newton-Raphson method and were within one-hundredth of a degree from the solutions obtained by the IOK solution.

The last two tests were conducted to verify that the forward orientation kinematics solution functioned correctly in the case were the roll angle of the system was changed.

Therefore, the variable inputs α, β, and γ were set to 0°, 0°, and 10° respectively in the

IOK problem. This resulted in θ1, θ2, θ3, ϕ1, ϕ2, and ϕ3 equaling -9.9327°, -24.4962°, -

9.6788°, 99.9647°, 85.3451°, and 100.4336° respectively. The results of θ1, θ2, θ3 were then put in as the input variables for the FOK problem and an initial guess of 0°, 0°, 10°,

99.9°, 85.3°, and 100.4° were used for α, β, γ, ϕ1, ϕ2, and ϕ3 respectively. Given these inputs, the Newton-Raphson method was able to calculate the results of the solution within two iterations. The forward orientation kinematics program obtained solutions for 87

α, β, γ, ϕ1, ϕ2, and ϕ3 respectively as 0°, 0°, 10°, 99.9647°, 85.3451°, and 100.4336°. The second test for roll angle tested the system if it were trying to obtain a roll angle of 20°.

Therefore the variable inputs α, β, and γ of the IOK solution were respectively set to 0°,

0°, and 20°. This resulted in θ1, θ2, θ3, ϕ1, ϕ2, and ϕ3 equaling -15.2056°, 52.2482°, -

14.2181°, 109.7123°, 81.3112°, and 110.6834° respectively. The results of θ1, θ2, θ3 were then put in as the input variables for the FOK problem and an initial guess of 0°, 0°, 20°,

109.7°, 81.3°, and 110.6° were used for α, β, γ, ϕ1, ϕ2, and ϕ3 respectively. Given these inputs, the Newton-Raphson method was able to calculate the results of the solution within two iterations and obtained solutions for α, β, γ, ϕ1, ϕ2, and ϕ3 respectively as 0°,

0°, 20°, 109.7124°, 81.3112°, and 110.6833°. As had been seen in the case of obtaining specific yaw and pitch rotation, it was found that both of the tests conducted where the obtainable roll angle was tested for the FOK solution produced results that were within one-hundredth of a degree of the solutions obtained through the IOK solution. It was also observed that both of these tests also were able to produce final results after just two iterations of the Newton-Raphson method.

88

4 Discussion of Kinematics Results

Comparing the vector loop closure equation with the inverse orientation kinematics in MATLAB determined that the inverse orientation kinematics equation had been derived correctly and could therefore be used to obtain results of the 3-RSR robotic system. It was observed that since the IOK problem contained a quartic polynomial, there would be a total of four possible solutions. Knowing that there could only be two possible real solutions for each leg within the system, an open-branch solution and a crossed-branch solution, two of the solutions obtained by the IOK program would always result in imaginary numbers. If all four of the possible solutions obtained by the quartic polynomial within the inverse orientation kinematics problem were found, this concluded that the requested input variables lead to an unattainable orientation of the robotic system.

Inputting the IOK solution into a text file format made the acquisition of the attainable yaw, pitch, and roll angles of the system quickly and easily accessible. Once this format had been implemented into MATLAB, converting the program to display a graphical representation of the solution was able to give a clear model of the attainable angles of the system to a large audience.

Observations of the graphical representation of the IOK solution lead to a better understanding of how specific types of changes to the constants in the equation would change to range of attainable angles for yaw, pitch, and roll. The first results observed were those for the yaw angles of the 3-RSR robot. It was known that for the general case solution, rotation about the z-axis described the yaw angle of the robotic system. It was found that by increasing the length of vector L within the system caused an increase in the rotational capabilities for yaw of the robot. This result makes sense in that increasing 89 the distance from the base platform and the moving platform would allow for more room for the system to rotate about itself on the z-plane. Shortening that length in the legs would limit the system’s ability to rotate about the z-axis, thus effectively decreasing its yaw rotational capabilities.

It was found that increasing vector lengths R and P cause a decrease in the rotational capabilities for yaw in the 3-RSR robot. If one were to increase the length of R and P, this would create a longer arm. Increasing this length causes a decrease in the effectiveness of the servomotor to rotate the system about the z-axis. The closer link L is to the center of the system, the more of an effect the rotation of the servo motor arm, r, will have on the rotation of the system as a whole around the z-axis. This effectively would increase the range of attainable yaw angles for the system. For this reason, increasing vector lengths R and P create a decrease in the maximum attainable yaw angles of the robot. Lastly, it was observed that increasing vector length r caused an increase in the maximum attainable yaw angles of the robotic system. This is because vector r is contains the active joint of the robotic system. Thinking back to the 3-PRUR robotic manipulator, it was discussed that increasing the input link of the system would increase the overall workspace of the robotic manipulator. This is pretty much the case when vector link r is increased. Increasing this link would increase the workspace of the system as a whole, and would thus effectively increase the rotational capabilities of the system.

Next, the attainable pitch angles of the system were investigated. It was observed that an increase in vector length L would cause a decrease in the pitch angle of the system. Thinking about the relation of link L to link r makes this result understandable. 90

If link r is held constant, a shorter link L would cause for a greater change in the pitch angle of the system for the same amount of rotation of the servo motor. If link L is longer, this would cause a greater distance from the base frame to the moving platform, and rotation of the servo motor arm, r, would not has as great of an effect on the system’s ability to rotate about the y-axis. Increasing vector lengths R and P were found to also decrease the overall rotation for pitch of the system. This is due to the same reason as was seen for the attainable yaw rotation. Increasing these vectors makes the rotation of the servo motor arm less effective at rotating the entire system about the y-axis. Lastly, it was found for the case of attainable pitch angles that an increase in the length of vector r causes an increase in the system’s ability to rotate about the y-axis. This is expected as it increases the input link of the system. If we were to think about link r at 90°, it is understood that a longer servo motor arm would make the system be taller at this orientation. With that in mind, the pitch angle would increase with a longer vector r, in that you would be able to reach greater heights for each servo motor.

Lastly, the attainable roll angles were investigated for the general robotic system by observing the graphical solutions of the IOK problem while changing the constant lengths of the system. It was found that an increase in vector length L may have caused a slight increase in the mechanisms ability to achieve greater roll angles, but nothing too significant. It was found that an increase in the vector lengths R and P caused a decrease in the mechanisms ability to rotate about the x-axis, just as was seen for the y- and z- axes. Increasing these vectors increases the arm length that has to be raised by the system to generate rotation, thus effectively decreasing the amount of rotation that may be obtained by the system. Finally, the change in vector length r was tested to see what 91 type of effect this would have on the 3-RSR robotic manipulators ability to obtain roll angles. It was found in this case that an increase in length of vector r created an increase in the systems overall attainable roll angles. This was seen in all three cases; yaw, pitch, and roll. It was determined that an increase in the length of the actuated link of the system cased an overall greater workspace for the system, and therefore effectively increased the attainable angles for all three degrees of freedom.

With these results in mind, a model for the Windmobile was able to be introduced.

It was determined that the system would have a minimal workspace, therefore all of the constant vectors had to be designed to be able to fit within the confines of the fuselage of the aircraft, while also being able to obtain the desired rotational outputs of the robotic system. The project team had decided that they wanted the robotic system to be able to attain the following rotations:

-5° to 30° of Pitch

15° of Roll

15° of Yaw

Using this, a model of the project specific system was created. It was determined that the allowable footprint of the system was one that matches the dimensions of Figure 37.

With this in mind, the orientation of the servo motors which allowed for the most symmetry of the system while still fitting on the board was determined to be when the servos were located at 44°, 180°, and 316°. However, this cased a case in which when attempting to perform inverse orientation kinematics on the mechanism, algorithmic singularities disallowed the ability to successfully test the leg set at 180°. Therefore, the coordinate system was rotated by 90°, which now placed the servo motors at 90°, 226°, 92 and 314°. This changed what was the x-axis of the system into the y-axis, and the y-axis into the x-axis. Thus, the rotations about the x-axis in this case would determine the allowable rotation for pitch of the system, and the rotations about the y-axis in this case would determine the allowable rotation for roll.

A final table of results i.e. desired vs. achieved roll, pitch, and yaw angles can be seen in Table 4. Studies of the plots created by the IOK solution for the specified system determined that mathematically, the mechanism would have an allowable rotation for yaw of -107° to 62°. The project group wanted the system to be able to rotate plus or minus 15° in this direction. Therefore, the designed system has met the parameters set by the Windmobile team. Second, the plots created by the IOK solution determined that the robotic system would have attainable roll angles of -30° to 21°. The team had specified that they would like for the system to be able to have a plus and minus rotation of 15° in this direction. The -30° to 21° exceeds these requirements set by the team. Lastly, the team specified that they desire the system to be able to reach pitch angles expanding from

-5° to 30°. Studying the plots created in MATLAB for the IOK solution determined that the legs of the system would allow for a pitch rotation range of -23° to 39°. These parameters also exceeded the Windmobile team’s expectations of the system.

Table 4: Desired vs. achieved roll, pitch, and yaw angles

Rotation Desired Attainable Rotational direction Negative Positive Negative Positive Yaw -15 15 -107 62 Pitch -5 30 -23 39 Roll -15 15 -30 21

93

According to all of the numbers obtained through the IOK solution derived for the project, the Windmobile should be able to obtain and exceed the required rotations needed to acquire flight characteristics of the Galah aircraft at different angles of attack.

Something that should be noted is that these results are for the joint limitations of the legs themselves. Other constraints introduced by the workspace itself could limit the attainable rotations of the robotic system, such as the servo arm could run into the metal board if it were to rotate too far in the negative direction.

The forward orientation kinematics was conducted through MATLAB to determine that the equations had been derived correctly and so that they could also be used in the programming of the 3-RSR robotic manipulator later. The Newton-Raphson method used for the forward orientation kinematics requires a good initial guess.

Luckily, obtaining a good initial guess came easy in that the inverse orientation kinematics problem had already been solved. Therefore, using the results obtained by the

IOK solution proved to be a good method in testing the FOK solution. The desired rotation of the system was implemented into the IOK solution as the variable inputs.

Then, once the answers were obtained, numbers close to the actual results of the IOK solution were plugged into the FOK problem as initial guesses. The Newton-Raphson method was able to iterate all of the solutions within one or two iterations and was always accurate to the one-thousandth degree as can be seen in the kinematic results section.

Therefore, it was determined that the FOK solution would be an acceptable method to programming the servo motors in the future in order to obtain the desired angular rotations of the 3-RSR robotic system.

94

5 Modified Design 3-RSR to 3-USR

Modeling of the system was done in Solid Edge. Initially, when modeling the 3-

RSR parallel robotic system, the design looked like that which can be seen in Figure 42.

This system is a very generic representation of what the team of the Windmobile project were looking for to operate their system. This particular mockup was designed to be perfectly symmetrical to make for standard equations that could be solved more easily.

The legs are all oriented 120 degrees from each other rotated around the base platform of the system. The model was originally designed with three, vertically spinning servomotors. It also can be observed that the initial mockup of the system has bulky, rigid links as the legs, and bulky revolute joints attaching the base platform to the legs.

The legs joints contain a revolute joint for the servo motor rotation, a spherical joint where the servo motor arm and link L connect, and the revolute joint attaching links L and R as the vector loop closure equation suggested. Collaboration with the team led to the understanding that it would be beneficial for the Windmobile’s particular case to try to use materials in house. 95

Figure 42: Initial RSR Configuration CAD Model

Further modeling of the system led to the understanding that a 3-RSR orientation would cause the actuated legs to bear the load of most of the system. This would require relatively large holding torques on the servo motors, as the static load would have to be carried by the three actuated legs. The team wanted to model a system that would introduce a middle leg to carry the load of the system so that the actuated legs would not have to bear that weight. Therefore, the team wanted a model of a 3-USR system to be created. It was determined by using the spatial Kutzbach mobility equation that this configuration would produce the same mobility as the 3-RSR orientation: 96

MNJJJJJ6(  1)  51  4 2  3 3  2 4  1 5 M 6(9  1)  5(4)  4(4)  3(3)  2(0) M  3

In this design, the middle leg is comprised of a revolute joint and a universal joint for the purpose of giving the same mobility as a spherical joint for the middle linkage.

This causes the overall system to have 9 rigid links, 4 1-DOF revolute joints, 4 2-DOF universal joints and 3 3-DOF spherical joints which leads to the 3 degrees of freedom for the overall system that was found in the Kutzbach mobility equation above.

The middle section of the robotic system is made up of the load cell, universal joint, and three additional parts that are being called Parts A, B, and C and can be seen in

Figures 43, 44, and 45 respectively. These three parts create the core of the middle assembly of the 3-USR robotic system. Part A is at the top of the core and connects the load cell and the universal joint together. Part B fits onto the opposite side of the universal joint that Part A sits on and is what part the ball bearings fit around. Part C is the housing of Part B and the ball bearings. Part C is fixed and it can be concluded that the wider section is considered the base platform of the 3-USR system, while the long, skinner end of Part C connects to the truss system on the front of the Windmobile attaching the two systems together.

97

Figure 43: Part A 98

Figure 44: Part B 99

Figure 45: Part C

After collaboration with the Windmobile team, a better model of the generalized

3-USR parallel manipulator was able to be designed. A CAD model of the generalized 3-

USR parallel manipulator can be seen in Figure 46. The servomotors are designed to were r is horizontal when θ = 0. The middle assembly now has the load cell, Part A, Part

B, Part C, and Universal Joint all incorporated into the CAD Design. This is the design that the base tests of the IOK and FOK solutions should be based, just as the 3- RSR case was done. Also, as was seen with the 3- RSR orientation, the perfect symmetry case should lead to significant simplifications in the equations. Therefore, a perfect symmetry case should be tested first to ensure that the equations of the 3-USR orientation are 100 derived correctly. The model shows that the joint connecting link L and R has been switched from a revolute joint to a universal joint.

Figure 46: Generalized 3-USR Parallel Manipulator

Once the generalized variables have been tested for IOK and FOK solutions, a system will need to be modeled specifically for the Windmobile team. When modeling, it was determined that something such as the design which can be seen in Figure 47 would need to be incorporated in the case of the Windmobile project. The space that the robotic system has to fit within the Galah’s fuselage is limited. Therefore this model was designed. The servomotors are oriented as to fit onto the 7” by 7.5” allowable footprint.

The system’s legs are comprised of push rods and ball end joints which can be configured 101 to fit the USR specification of the system. The ball end joint can act as a spherical joint as is, or if one were to implant a set screw one of the degrees of freedom could be constrained which would successfully change the spherical joint to a universal joint. As it can be observed, the legs are much thinner than those in the original designs. It was determined that the legs would not need to be bulky, as the amount of weight the system will need to support is minimal thanks to the middle leg that has been incorporated.

Figure 47: Windmobile's USR Configuration CAD

102

6 Conclusion/Future Work

In conclusion, a 3-DOF parallel manipulator was needed by the Windmobile team in order to control the yaw, pitch, and roll of their Galah model aircraft in order to obtain flight characteristics from different angles of attack and sideslip of the aircraft. To gain an understanding of 3-DOF parallel manipulators, many of them were reviewed such as the 3-PRUR, 3-UPU, 3-RRUR, and 3-RPR robotic manipulators. This review led to the final design of a 3-DOF robotic system for the Windmobile project known as a 3-RSR configuration.

There were four main thesis objectives. The first was to develop a model of the

3-RSR system that could be used for the Windmobile project. The second was to derive the vector loop closure equation which would ultimately allow for the derivation of the inverse and forward orientation kinematics equations. The third was to then perform inverse and forward orientation kinematics for the 3-RSR robotic manipulator. Lastly, a working model of the parallel robotic manipulator system that would be used by the

Windmobile team was to be manufactured.

A model of the system was able to be developed and from that the vector loop closure equation was derived. From the vector loop closure equation, the inverse orientation kinematic solution was derived by squaring and adding the y and z portions of the vector loop closure equation in (16). Three different MATLAB programs were developed to gain an understanding of the inverse orientation kinematics. The first program developed took a look at a single orientation of the robotic system by using one input variable for α, β, and γ. This allowed for a circular check between the vector loop closure equation and the inverse orientation kinematics solution to take place. This 103 program was also able to be used to take a look at a specific orientation of the 3-RSR system when needed. The second program developed for the IOK solution created a text file that contained the solutions for every degree of rotation for α, β, and γ. This created a way to easily access the allowable rotation for yaw, pitch, and roll of the system. The last program developed was simply turned the solutions from a text file type format into a graphical interpretation of the systems allowable rotations for yaw, pitch, and roll for each leg. It was deduced that in order for the system to reach a certain degree of rotation, all three legs had to suggest that such an angle were obtainable; otherwise, the systems joint limits would have been reached.

From the graphical representation of the IOK solution, an investigation of the effects of changing the constant lengths of the robotic system was able to be conducted.

From this, it was concluded that an increase in the vector length L would lead to an increase in the maximum attainable yaw angles of the 3-RSR manipulator; however, this increase in vector length L would cause a decrease in the maximum attainable pitch angles. The results obtained from this experiment did not suggest any significant change would occur to the attainable roll angles of the system when vector L’s length had been changed. Vectors R and P were always kept at the same length in order to maintain symmetry within the system. It was observed that increasing these vector lengths produced a decrease in yaw, pitch, and roll. It was also concluded that an increase in the servo motor arm length, or vector r, always produced a case were the attainable rotations of yaw, pitch, and roll were increased.

From these results, a design was specifically made for the Windmobile team in order to maximize rotation for yaw, pitch, and roll of the system while ensuring that the 104 system would be able to fit into the allowable workspace produced by the fuselage of the

Galah aircraft. It was determined that such a model could be obtained if the three legs were oriented around the center of the system at ϵ and δ equal to 90°, 224°, and 316°.

The constant vector lengths that would work for the specific robotic system were found to be 6.15 in., 1.7 in., and 0.85 in. for L, R and P, and r respectively. The Windmobile team wanted a system that would be able to attain yaw, pitch, and roll angles of -5° to 30°, +/-

15°, and +/- 15° respectively. The inverse orientation kinematics, with the given parameters, suggests that the 3-RSR system will be able to exceed all of those required angles. Specifically, the IOK solution suggested that the attainable rotations of yaw, pitch, and roll could be -107° to 62°, -23° to 39°, and -30° to 21° respectively. That is the legs will allow for this much rotation, the workspace of the system may introduce new joint limits which could cause this much rotation to be unattainable.

Forward orientation kinematics was conducted for the 3-RSR robotic manipulator in MATLAB. In order to check that the derived equation worked properly for yaw, pitch, and roll, many tests were compared to the IOK solution. The desired rotation of the system was implemented into the IOK solution as the variable inputs. Then, once the answers were obtained, numbers close to the actual results of the IOK solution were plugged into the FOK problem as initial guesses. With these initial guesses, the Newton-

Raphson method was able to iterate all of the solutions within one or two iterations. It was also found that all of the solutions obtained from the FOK program were accurate to the one-thousandth degree. Therefore, it was determined that the FOK solution would be an acceptable method to programming the servo motors in the future in order to obtain the desired angular rotations of the 3-RSR robotic system. 105

Although the modeled RSR manipulator system suggests that the angles of yaw, pitch, and roll that the Windmobile team desired could be met, this specific orientation would cause direct load bearing on the actuated legs of the robotic system. Therefore, it is the Windmobile team’s wishes to test out a 3-USR orientation design for their Galah aircraft. A prototype of this system has been machined and initial tests are positive that such a system would produce results like the 3-RSR system while taking the direct load off of the actuated legs and placing it onto the middle leg of the new system. Future work of this project would be to create a vector loop closure equation for the 3-USR robotic manipulator and then derive and test the inverse and forward orientation kinematics, as was done for the 3-RSR orientation.

106

References

Ben-Horin, P., &Shomam, M. (2006).Singularity condition of six-degree-of-freedom

three-legged parallel robots based on grassmann-cayleyalgebra.IEEE

Transactions on Robotics, 22(4), 577-589.

Deidda, R., Mariani, A., &Ruggiu, M. (2010).On the kinematics of the 3-rrur spherical

parallel manipulator.Robotica, 28, 821-832.

Dibenedetto, J. (2012a). Galah aircraft. [Photo]

Di Gregorio, R. (2003). Kinematics of the 3-upu wrist.Mechanism and Machine Theory,

38, 253-263.

Husty, M., &Gosselin, C. (2008).On the singularity surface of planar 3-rpr parallel

mechanisms.Mechanics Based Design of Structures and Machines, 36, 411-425.

Liu, G., Lou, Y., & Li, Z. (2003). Singularities of parallel manipulators: A geometric

treatment. IEEE Transactions on Robotics and Automation, 19(4), 579-594.

Mangan, B. (2010). Application of robotic technology in biomechanics to study joint

laxity. Journal of Medical Engineering and Technology, 34(7), 399-407.

Merlet, J-P.(1996). Direct kinematics of planar parallel manipulators.

IEEE Int. Conf. on Robotics and Automation, Minneapolis, Minn.

pp. 3744–3749.

Shao, Z., Tang, X., Chen, X., & Wang, L. (2012).Research on the inertia matching of the

Stewart parallel manipulator.Robotics and Computer-Integrated Manufacturing,

28, 649-659.

Urízar, M., Petuya, V., Altuzarra, O., & Hernández, A. (2012).Assembly mode changing

in the cuspidal analytic 3-RPR.IEEE Transactions on Robotics, 28(2), 506-513. 107

Varedi, S. M., Daniali, H. M., & Ganji, D. D. (2009).Kinematics of an offset 3-upu

translational parallel manipulator by the homotopy continuation

method.Nonlinear Analysis: Real World Applications, 10, 1767-1774.

Williams II, R. L., & Joshi, A. R. (1999, December).Planar parallel 3-rpr manipulator.

Proceedings of the sixth conference on applied mechanisms and robotics

Zhang, G. (2012). Classification of direct kinematics to planar generalized stewart

platforms. Computational Geometry: Theory and Applications, 45, 458-473.

Zhu, J. (2012b). Truss system conceptual design. [Image]

Zeng, D., Zhen, H., &Wenjuan, L. (2008).Performance analysis and optimal design of a

3-dof 3-prur parallel mechanism.Journal of Mechanical Design, 130(042307), 1-

9.

Zein, M., Wenger, P., and Chablat, D., 2007, “Singular Curves in the Joint

Space and Cusp Points of 3-RPR Parallel Manipulators,” Robotica, 25(6), pp.

717–724.

108

Appendix: MATLAB Programs

This section includes MATLAB programs that were used for the testing of the vector loop closure equation, inverse orientation solutions, and forward orientation solutions of the 3-RSR parallel manipulator.

1. Program for the Vector Loop Closure Equation

%------% Vector Loop Closer Equation % Programmer: Joey Feck % December 19th 2012 %------clc; clear; DR = pi/180; %------Inputs ------I = input('Enter [th phi del eps alpha beta gamma]: '); th = I(1)*DR; phi = I(2)*DR; del = I(3)*DR; eps = I(4)*DR; alpha = I(5)*DR; beta = I(6)*DR; gamma = I(7)*DR; LL = 5.75; RR = 1.7; PP = RR; rr = .85; la = (LL)/2; lb = (LL)/2; %------% ------Vector B M and L ------B = [RR*cos(del); RR*sin(del); 0] M = [PP*cos(eps) - rr*sin(eps); PP*sin(eps) + rr*cos(eps); 0] L = [-LL*cos(phi)*cos(del); -LL*cos(phi)*sin(del); LL*sin(phi)] %------% ------Orthonormal Rotation Matrix Defined ------[r11,r12,r13,r21,r22,r23,r31,r32,r33,R] = rotationjf(alpha,beta,gamma); % ------Vector P and r ------La = [0;0;la] Lb = R*[0;0;lb] P = La + Lb

r = [ rr*cos(th)*sin(eps); -rr*cos(th)*cos(eps); 109

-rr*sin(th)] % ------Check that the Left and Right are Equal ------Left = P + R*M + R*r Right = B + L

Check = P + R*M + R*r - B - L %Should zero out

110

2. Function used to determine rotation matrix function [r11,r12,r13,r21,r22,r23,r31,r32,r33,R] = rotationjf( a,b,g ) % Function that defines the orthonormal rotation matrix % used in the vector loop closed equation r11 = cos(a)*cos(b); r12 = -sin(a)*cos(g)+cos(a)*sin(b)*sin(g); r13 = sin(a)*sin(g)+cos(a)*sin(b)*cos(g); r21 = sin(a)*cos(b); r22 = cos(a)*cos(g)+sin(a)*sin(b)*sin(g); r23 = -cos(a)*sin(g)+sin(a)*sin(b)*cos(g); r31 = -sin(b); r32 = cos(b)*sin(g); r33 = cos(b)*cos(g);

R = [r11,r12,r13; r21,r22,r23; r31,r32,r33] end

111

3. Program for Constants used throughout IOK and FOK Solutions

%------% Constants for Robotic System % Joey Feck %------DR = pi/180; RD = 180/pi;

RR = 1.7; LL = 6.15; PP = RR; rr = .85;

LA = LL/2; LB = LL/2;

%------Case of Full Symmetry ------R1 = RR; R2 = RR; R3 = RR; L1 = LL; L2 = LL; L3 = LL; P1 = PP; P2 = PP; P3 = PP; r1 = rr; r2 = rr; r3 = rr; del1 = 226*DR; del2 = 314*DR; del3 = 90*DR; eps1 = 226*DR; eps2 = 314*DR; eps3 = 90*DR;

% Base Bi and moving platform Pi points, fixed in {0} and {P} respectively

B1x = R1*cos(del1); B1y = R1*sin(del1); B2x = R2*cos(del2); B2y = R2*sin(del2); B3x = R3*cos(del3); B3y = R3*sin(del3);

M1x = P1*cos(eps1) - r1*sin(eps1); M1y = P1*sin(eps1) + r1*cos(eps1); M2x = P2*cos(eps2) - r2*sin(eps2); M2y = P2*sin(eps2) + r2*cos(eps2); M3x = P3*cos(eps3) - r3*sin(eps3); M3y = P3*sin(eps3) + r3*cos(eps3);

112

4. Program to solve the Inverse Orientation Kinematics Problem

%------% IOK_Solution.m % Programmer: Joey Feck % Inverse Orientation Kinematics for 3-RSR System %------clear; clc; Constants; % ------% ------Inputs ------I = input('Enter [alpha beta gamma]; '); a = I(1)*DR; b = I(2)*DR; g = I(3)*DR; % ------% ------Orthonormal Rotation Matrix ------r11 = cos(a)*cos(b); r12 = -sin(a)*cos(g)+cos(a)*sin(b)*sin(g); r13 = sin(a)*sin(g)+cos(a)*sin(b)*cos(g); r21 = sin(a)*cos(b); r22 = cos(a)*cos(g)+sin(a)*sin(b)*sin(g); r23 = -cos(a)*sin(g)+sin(a)*sin(b)*cos(g); r31 = -sin(b); r32 = cos(b)*sin(g); r33 = cos(b)*cos(g);

RotMatx = [r11,r12,r13; r21,r22,r23; r31,r32,r33] % ------% ------Leg 1 ------By = B1y; Mx = M1x; My = M1y; R = R1; L = L1; P = P1; r = r1; eps = eps1; del = del1; disp(' Leg 1'); IOKSolve;

% ------% ------Leg 2 ------By = B2y; Mx = M2x; My = M2y; R = R2; L = L2; P = P2; r = r2; eps = eps2; del = del2; disp(' Leg 2'); IOKSolve;

113

% ------% ------Leg 3 ------By = B3y; Mx = M3x; My = M3y; R = R3; L = L3; P = P3; r = r3; eps = eps3; del = del3; disp(' Leg 3'); IOKSolve;

114

5. Program for the In-line Function called by IOK_Solution.m

%------% IOKSolve.m % Programmer: Joey Feck % In-line function called for IOK solution by IOK.m %------% A - F terms A = (r*(-r21*sin(eps) + r22*cos(eps)))/sin(del); B = (r23*r)/sin(del); C = (-r23*LB - r21*Mx - r22*My + By)/sin(del); D = r*(r31*sin(eps) - r32*cos(eps)); E = -r33*r; F = r33*LB + LA +r31*Mx + r32*My;

% a1 - a6 terms a1 = A^2 + D^2; a2 = B^2 + E^2; a3 = 2*(A*B + D*E); a4 = 2*(A*C + D*F); a5 = 2*(B*C + E*F); a6 = C^2 + F^2 - L^2;

% A4 - A0 quartic polynomial coefficients A4 = a1 - a4 + a6; A3 = -2*(a3 - a5); A2 = -2*(a1 - 2*a2 - a6); A1 = 2*(a3 + a5); A0 = a1 + a4 + a6; tt = roots([A4 A3 A2 A1 A0]); % ------% ------Calculation of possible theta and phi ------if imag(tt(1)) == 0 Theta1 = 2*atan(tt(1)); Theta1d = 2*atan(tt(1))*RD phi1 = atan2(D*cos(Theta1)+E*sin(Theta1)+F,A*cos(Theta1)+B*sin(Theta1)+C); phi1d = (atan2(D*cos(Theta1)+E*sin(Theta1)+F,A*cos(Theta1)+B*sin(Theta1)+C))*RD else Theta1d = NaN phi1d = NaN end %------if imag(tt(2)) == 0 115

Theta2 = 2*atan(tt(2)); Theta2d = 2*atan(tt(2))*RD phi2 = atan2(D*cos(Theta2)+E*sin(Theta2)+F,A*cos(Theta2)+B*sin(Theta2)+C); phi2d = (atan2(D*cos(Theta2)+E*sin(Theta2)+F,A*cos(Theta2)+B*sin(Theta2)+C))*RD else Theta2d = NaN phi2d = NaN end %------if imag(tt(3)) == 0 Theta3 = 2*atan(tt(3)); Theta3d = 2*atan(tt(3))*RD phi3 = atan2(D*cos(Theta3)+E*sin(Theta3)+F,A*cos(Theta3)+B*sin(Theta3)+C); phi3d = (atan2(D*cos(Theta3)+E*sin(Theta3)+F,A*cos(Theta3)+B*sin(Theta3)+C))*RD else Theta3d = NaN phi3d = NaN end %------if imag(tt(4)) == 0 Theta4 = 2*atan(tt(4)); Theta4d = 2*atan(tt(4))*RD phi4 = atan2(D*cos(Theta4)+E*sin(Theta4)+F,A*cos(Theta4)+B*sin(Theta4)+C); phi4d = (atan2(D*cos(Theta4)+E*sin(Theta4)+F,A*cos(Theta4)+B*sin(Theta4)+C))*RD else Theta4d = NaN phi4d = NaN end

116

6. Program for IOK Solution with variable Alpha inputs

%------% IOK_Solution_Alpha.m % Programmer: Joe Feck % Inverse Orientation Kinematics for 3-RSR System % Creates a solution file with changing alpha inputs %------clear; clc;

Constants; f_ = fopen('IOK_RESULTS.txt','a+'); % ------% ------Inputs ------I = input('Enter [beta gamma]; '); b = I(1)*DR; g = I(2)*DR; % ------% ------For Loop ------a0 = 0; da = 1; af = 360; a = [a0:da:af]*DR; N = (af - a0) /da + 1; for i = 1:N, % ------% ------Orthonormal Rotation Matrix ------r11 = cos(a(i))*cos(b); r12 = -sin(a(i))*cos(g)+cos(a(i))*sin(b)*sin(g); r13 = sin(a(i))*sin(g)+cos(a(i))*sin(b)*cos(g); r21 = sin(a(i))*cos(b); r22 = cos(a(i))*cos(g)+sin(a(i))*sin(b)*sin(g); r23 = -cos(a(i))*sin(g)+sin(a(i))*sin(b)*cos(g); r31 = -sin(b); r32 = cos(b)*sin(g); r33 = cos(b)*cos(g);

RotMatx = [r11,r12,r13; r21,r22,r23; r31,r32,r33] % ------% ------Leg 1 ------By = B1y; Mx = M1x; My = M1y; R = R1; L = L1; P = P1; r = r1; eps = eps1; del = del1; disp(' Leg 1'); IOKSolve_Alpha;

117

Results = fprintf(f_,'%.2g\t%.2g\t%.2g\t%.2g\t%.2g\t%.2g\t%.2g\t%.2g\t',Theta1d,phi1d,Theta2d,p hi2d,Theta3d,phi3d,Theta4d,phi4d) % ------% ------Leg 2 ------By = B2y; Mx = M2x; My = M2y; R = R2; L = L2; P = P2; r = r2; eps = eps2; del = del2; disp(' Leg 2'); IOKSolve_Alpha;

Results = fprintf(f_,'%.2g\t%.2g\t%.2g\t%.2g\t%.2g\t%.2g\t%.2g\t%.2g\t',Theta1d,phi1d,Theta2d,p hi2d,Theta3d,phi3d,Theta4d,phi4d) % ------% ------Leg 3 ------By = B3y; Mx = M3x; My = M3y; R = R3; L = L3; P = P3; r = r3; eps = eps3; del = del3; disp(' Leg 3'); IOKSolve_Alpha;

Results = fprintf(f_,'%.2g\t%.2g\t%.2g\t%.2g\t%.2g\t%.2g\t%.2g\t%.2g\n',Theta1d,phi1d,Theta2d,p hi2d,Theta3d,phi3d,Theta4d,phi4d) end fclose(f_)

118

7. Program for IOK Solution with variable Beta inputs

%------% IOKSolve_Beta.m % Programmer: Joey Feck % In-line function called for IOK_Solution_Beta.m %------clear; clc;

Constants; f_ = fopen('IOK_RESULTS.txt','a+'); % ------% ------Inputs ------I = input('Enter [alpha gamma]; '); a = I(1)*DR; g = I(2)*DR; % ------% ------For Loop ------b0 = 0; db = 1; bf = 360; b = [b0:db:bf]*DR; N = (bf - b0) /db + 1; for i = 1:N, % ------% ------Orthonormal Rotation Matrix ------r11 = cos(a)*cos(b(i)); r12 = -sin(a)*cos(g)+cos(a)*sin(b(i))*sin(g); r13 = sin(a)*sin(g)+cos(a)*sin(b(i))*cos(g); r21 = sin(a)*cos(b(i)); r22 = cos(a)*cos(g)+sin(a)*sin(b(i))*sin(g); r23 = -cos(a)*sin(g)+sin(a)*sin(b(i))*cos(g); r31 = -sin(b(i)); r32 = cos(b(i))*sin(g); r33 = cos(b(i))*cos(g);

RotMatx = [r11,r12,r13; r21,r22,r23; r31,r32,r33] % ------% ------Leg 1 ------By = B1y; Mx = M1x; My = M1y; R = R1; L = L1; P = P1; r = r1; eps = eps1; del = del1; disp(' Leg 1'); IOKSolve_Beta; %phi1 = ph(1); 119

Results = fprintf(f_,'%.2g\t%.2g\t%.2g\t%.2g\t%.2g\t%.2g\t%.2g\t%.2g\t',Theta1d,phi1d,Theta2d,p hi2d,Theta3d,phi3d,Theta4d,phi4d) % ------% ------Leg 2 ------By = B2y; Mx = M2x; My = M2y; R = R2; L = L2; P = P2; r = r2; eps = eps2; del = del2; disp(' Leg 2'); IOKSolve_Beta; %phi2 = ph(4); Results = fprintf(f_,'%.2g\t%.2g\t%.2g\t%.2g\t%.2g\t%.2g\t%.2g\t%.2g\t',Theta1d,phi1d,Theta2d,p hi2d,Theta3d,phi3d,Theta4d,phi4d) % ------% ------Leg 3 ------By = B3y; Mx = M3x; My = M3y; R = R3; L = L3; P = P3; r = r3; eps = eps3; del = del3; disp(' Leg 3'); IOKSolve_Beta; %phi3 = ph(4); Results = fprintf(f_,'%.2g\t%.2g\t%.2g\t%.2g\t%.2g\t%.2g\t%.2g\t%.2g\n',Theta1d,phi1d,Theta2d,p hi2d,Theta3d,phi3d,Theta4d,phi4d) end fclose(f_)

120

8. Program for IOK Solution with variable Gamma inputs

%------% IOKSolve_Gamma.m % Programmer: Joey Feck % In-line function called for IOK_Solution_Gamma.m %------clear; clc;

Constants; f_ = fopen('IOK_RESULTS.txt','a+'); % ------% ------Inputs ------I = input('Enter [alpha beta]; '); a = I(1)*DR; b = I(2)*DR; % ------% ------For Loop ------g0 = 0; dg = 1; gf = 360; g = [g0:dg:gf]*DR; N = (gf - g0) /dg + 1; for i = 1:N, % ------% ------Orthonormal Rotation Matrix ------r11 = cos(a)*cos(b); r12 = -sin(a)*cos(g(i))+cos(a)*sin(b)*sin(g(i)); r13 = sin(a)*sin(g(i))+cos(a)*sin(b)*cos(g(i)); r21 = sin(a)*cos(b); r22 = cos(a)*cos(g(i))+sin(a)*sin(b)*sin(g(i)); r23 = -cos(a)*sin(g(i))+sin(a)*sin(b)*cos(g(i)); r31 = -sin(b); r32 = cos(b)*sin(g(i)); r33 = cos(b)*cos(g(i));

RotMatx = [r11,r12,r13; r21,r22,r23; r31,r32,r33] % ------% ------Leg 1 ------By = B1y; Mx = M1x; My = M1y; R = R1; L = L1; P = P1; r = r1; eps = eps1; del = del1; disp(' Leg 1'); IOKSolve_Gamma; %phi1 = ph(1); 121

Results = fprintf(f_,'%.2g\t%.2g\t%.2g\t%.2g\t%.2g\t%.2g\t%.2g\t%.2g\t',Theta1d,phi1d,Theta2d,p hi2d,Theta3d,phi3d,Theta4d,phi4d) % ------% ------Leg 2 ------By = B2y; Mx = M2x; My = M2y; R = R2; L = L2; P = P2; r = r2; eps = eps2; del = del2; disp(' Leg 2'); IOKSolve_Gamma; %phi2 = ph(4); Results = fprintf(f_,'%.2g\t%.2g\t%.2g\t%.2g\t%.2g\t%.2g\t%.2g\t%.2g\t',Theta1d,phi1d,Theta2d,p hi2d,Theta3d,phi3d,Theta4d,phi4d) % ------% ------Leg 3 ------By = B3y; Mx = M3x; My = M3y; R = R3; L = L3; P = P3; r = r3; eps = eps3; del = del3; disp(' Leg 3'); IOKSolve_Gamma; %phi3 = ph(4); Results = fprintf(f_,'%.2g\t%.2g\t%.2g\t%.2g\t%.2g\t%.2g\t%.2g\t%.2g\n',Theta1d,phi1d,Theta2d,p hi2d,Theta3d,phi3d,Theta4d,phi4d) end fclose(f_)

122

9. Program for Graphical Interpretation of IOK Solution with variable Alpha

%------% IOK_Solution_Alpha_Graphical % Programmer: Joey Feck % Inverse Orientation Kinematics for 3-RSR System % Graphical Interpratation of IOK Solution with variable input alpha %------clear; clc; Constants; % ------% ------Inputs ------I = input('Enter [beta gamma]; '); b = I(1)*DR; g = I(2)*DR; % ------% ------Alpha Array ------a0 = 0; da = 1; af = 360; a = [a0:da:af]*DR; N = (af - a0) /da + 1; for i = 1:N, % ------% ------Orthonormal Rotation Matrix ------r11 = cos(a(i))*cos(b); r12 = -sin(a(i))*cos(g)+cos(a(i))*sin(b)*sin(g); r13 = sin(a(i))*sin(g)+cos(a(i))*sin(b)*cos(g); r21 = sin(a(i))*cos(b); r22 = cos(a(i))*cos(g)+sin(a(i))*sin(b)*sin(g); r23 = -cos(a(i))*sin(g)+sin(a(i))*sin(b)*cos(g); r31 = -sin(b); r32 = cos(b)*sin(g); r33 = cos(b)*cos(g); RotMatx = [r11,r12,r13; r21,r22,r23; r31,r32,r33] % ------% ------Leg 1 ------By = B1y; Mx = M1x; My = M1y; R = R1; L = L1; P = P1; r = r1; eps = eps1; del = del1; disp(' Leg 1'); IOKSolve_Graphical; end figure; subplot(321) plot(a/DR,Theta1d,'r',a/DR,Theta2d,'b',a/DR,Theta3d,'r',a/DR,Theta4d,'b'); 123 grid; set(gca,'Fontsize',18); axis([0 360 -Inf Inf]); title('Change in \alpha vs \theta for leg 1'); subplot(322) plot(a/DR,phi1d,'r',a/DR,phi2d,'b',a/DR,phi3d,'r',a/DR,phi4d,'b'); grid; set(gca,'Fontsize',18); axis([0 360 -Inf Inf]); title('Change in \alpha vs \phi for leg 1'); for i = 1:N, % ------% ------Orthonormal Rotation Matrix ------r11 = cos(a(i))*cos(b); r12 = -sin(a(i))*cos(g)+cos(a(i))*sin(b)*sin(g); r13 = sin(a(i))*sin(g)+cos(a(i))*sin(b)*cos(g); r21 = sin(a(i))*cos(b); r22 = cos(a(i))*cos(g)+sin(a(i))*sin(b)*sin(g); r23 = -cos(a(i))*sin(g)+sin(a(i))*sin(b)*cos(g); r31 = -sin(b); r32 = cos(b)*sin(g); r33 = cos(b)*cos(g); RotMatx = [r11,r12,r13; r21,r22,r23; r31,r32,r33] % ------% ------Leg 2 ------By = B2y; Mx = M2x; My = M2y; R = R2; L = L2; P = P2; r = r2; eps = eps2; del = del2; disp(' Leg 2'); IOKSolve_Graphical; end subplot(323) plot(a/DR,Theta1d,'r',a/DR,Theta2d,'b',a/DR,Theta3d,'r',a/DR,Theta4d,'b'); grid; set(gca,'Fontsize',18); axis([0 360 -Inf Inf]); title('Change in \alpha vs \theta for leg 2'); legend('Open-Branch','Crossed-Branch'); ylabel('\theta'); subplot(324) plot(a/DR,phi1d,'r',a/DR,phi2d,'b',a/DR,phi3d,'r',a/DR,phi4d,'b'); grid; set(gca,'Fontsize',18); axis([0 360 -Inf Inf]); title('Change in \alpha vs \phi for leg 2'); ylabel('\phi'); for i = 1:N, % ------% ------Orthonormal Rotation Matrix ------124 r11 = cos(a(i))*cos(b); r12 = -sin(a(i))*cos(g)+cos(a(i))*sin(b)*sin(g); r13 = sin(a(i))*sin(g)+cos(a(i))*sin(b)*cos(g); r21 = sin(a(i))*cos(b); r22 = cos(a(i))*cos(g)+sin(a(i))*sin(b)*sin(g); r23 = -cos(a(i))*sin(g)+sin(a(i))*sin(b)*cos(g); r31 = -sin(b); r32 = cos(b)*sin(g); r33 = cos(b)*cos(g); RotMatx = [r11,r12,r13; r21,r22,r23; r31,r32,r33] % ------% ------Leg 3 ------By = B3y; Mx = M3x; My = M3y; R = R3; L = L3; P = P3; r = r3; eps = eps3; del = del3; disp(' Leg 3'); IOKSolve_Graphical; end subplot(325) plot(a/DR,Theta1d,'r',a/DR,Theta2d,'b',a/DR,Theta3d,'r',a/DR,Theta4d,'b'); grid; set(gca,'Fontsize',18); axis([0 360 -Inf Inf]); title('Change in \alpha vs \theta for leg 3'); xlabel('\alpha'); subplot(326) plot(a/DR,phi1d,'r',a/DR,phi2d,'b',a/DR,phi3d,'r',a/DR,phi4d,'b'); grid; set(gca,'Fontsize',18); axis([0 360 -Inf Inf]); title('Change in \alpha vs \phi for leg 3'); xlabel('\alpha');

125

10. Program for Graphical Interpretation of IOK Solution with variable Beta

%------% IOK_Solution_Beta_Graphical % Programmer: Joey Feck % Inverse Orientation Kinematics for 3-RSR System % Graphical Interpratation of IOK Solution with variable input beta %------clear; clc; Constants; % ------% ------Inputs ------I = input('Enter [alpha gamma]; '); a = I(1)*DR; g = I(2)*DR; % ------% ------Alpha Array ------b0 = 0; db = 1; bf = 360; b = [b0:db:bf]*DR; N = (bf - b0) /db + 1; for i = 1:N, % ------% ------Orthonormal Rotation Matrix ------r11 = cos(a)*cos(b(i)); r12 = -sin(a)*cos(g)+cos(a)*sin(b(i))*sin(g); r13 = sin(a)*sin(g)+cos(a)*sin(b(i))*cos(g); r21 = sin(a)*cos(b(i)); r22 = cos(a)*cos(g)+sin(a)*sin(b(i))*sin(g); r23 = -cos(a)*sin(g)+sin(a)*sin(b(i))*cos(g); r31 = -sin(b(i)); r32 = cos(b(i))*sin(g); r33 = cos(b(i))*cos(g);

RotMatx = [r11,r12,r13; r21,r22,r23; r31,r32,r33] % ------% ------Leg 1 ------By = B1y; Mx = M1x; My = M1y; R = R1; L = L1; P = P1; r = r1; eps = eps1; del = del1; disp(' Leg 1'); IOKSolve_Graphical; end figure; subplot(321) 126 plot(b/DR,Theta1d,'r',b/DR,Theta2d,'b',b/DR,Theta3d,'r',b/DR,Theta4d,'b'); grid; set(gca,'Fontsize',18); axis([0 360 -Inf Inf]); title('Change in \beta vs \theta for leg 1');

subplot(322) plot(b/DR,phi1d,'r',b/DR,phi2d,'b',b/DR,phi3d,'r',b/DR,phi4d,'b'); grid; set(gca,'Fontsize',18); axis([0 360 -Inf Inf]); title('Change in \beta vs \phi for leg 1');

for i = 1:N, % ------% ------Orthonormal Rotation Matrix ------r11 = cos(a)*cos(b(i)); r12 = -sin(a)*cos(g)+cos(a)*sin(b(i))*sin(g); r13 = sin(a)*sin(g)+cos(a)*sin(b(i))*cos(g); r21 = sin(a)*cos(b(i)); r22 = cos(a)*cos(g)+sin(a)*sin(b(i))*sin(g); r23 = -cos(a)*sin(g)+sin(a)*sin(b(i))*cos(g); r31 = -sin(b(i)); r32 = cos(b(i))*sin(g); r33 = cos(b(i))*cos(g);

RotMatx = [r11,r12,r13; r21,r22,r23; r31,r32,r33] % ------% ------Leg 2 ------By = B2y; Mx = M2x; My = M2y; R = R2; L = L2; P = P2; r = r2; eps = eps2; del = del2; disp(' Leg 2'); IOKSolve_Graphical; end subplot(323) plot(b/DR,Theta1d,'r',b/DR,Theta2d,'b',b/DR,Theta3d,'r',b/DR,Theta4d,'b'); grid; set(gca,'Fontsize',18); axis([0 360 -Inf Inf]); title('Change in \beta vs \theta for leg 2'); legend('Open-Branch','Crossed-Branch'); ylabel('\theta'); subplot(324) plot(b/DR,phi1d,'r',b/DR,phi2d,'b',b/DR,phi3d,'r',b/DR,phi4d,'b'); 127 grid; set(gca,'Fontsize',18); axis([0 360 -Inf Inf]); title('Change in \beta vs \phi for leg 2'); ylabel('\phi');

for i = 1:N, % ------% ------Orthonormal Rotation Matrix ------r11 = cos(a)*cos(b(i)); r12 = -sin(a)*cos(g)+cos(a)*sin(b(i))*sin(g); r13 = sin(a)*sin(g)+cos(a)*sin(b(i))*cos(g); r21 = sin(a)*cos(b(i)); r22 = cos(a)*cos(g)+sin(a)*sin(b(i))*sin(g); r23 = -cos(a)*sin(g)+sin(a)*sin(b(i))*cos(g); r31 = -sin(b(i)); r32 = cos(b(i))*sin(g); r33 = cos(b(i))*cos(g);

RotMatx = [r11,r12,r13; r21,r22,r23; r31,r32,r33] % ------% ------Leg 3 ------By = B3y; Mx = M3x; My = M3y; R = R3; L = L3; P = P3; r = r3; eps = eps3; del = del3; disp(' Leg 3'); IOKSolve_Graphical; end subplot(325) plot(b/DR,Theta1d,'r',b/DR,Theta2d,'b',b/DR,Theta3d,'r',b/DR,Theta4d,'b'); grid; set(gca,'Fontsize',18); axis([0 360 -Inf Inf]); title('Change in \beta vs \theta for leg 3'); xlabel('\beta'); subplot(326) plot(b/DR,phi1d,'r',b/DR,phi2d,'b',b/DR,phi3d,'r',b/DR,phi4d,'b'); grid; set(gca,'Fontsize',18); axis([0 360 -Inf Inf]); title('Change in \beta vs \phi for leg 3'); xlabel('\beta');

128

11. Program for Graphical Interpretation of IOK Solution with variable Gamma

%------% IOK_Solution_Gamma_Graphical % Programmer: Joey Feck % Inverse Orientation Kinematics for 3-RSR System % Graphical Interpratation of IOK Solution with variable input gamma %------clear; clc; Constants; % ------% ------Inputs ------I = input('Enter [alpha beta]; '); a = I(1)*DR; b = I(2)*DR; % ------% ------Alpha Array ------g0 = 0; dg = 0.5; gf = 360; g = [g0:dg:gf]*DR; N = (gf - g0) /dg + 1; for i = 1:N, % ------% ------Orthonormal Rotation Matrix ------r11 = cos(a)*cos(b); r12 = -sin(a)*cos(g(i))+cos(a)*sin(b)*sin(g(i)); r13 = sin(a)*sin(g(i))+cos(a)*sin(b)*cos(g(i)); r21 = sin(a)*cos(b); r22 = cos(a)*cos(g(i))+sin(a)*sin(b)*sin(g(i)); r23 = -cos(a)*sin(g(i))+sin(a)*sin(b)*cos(g(i)); r31 = -sin(b); r32 = cos(b)*sin(g(i)); r33 = cos(b)*cos(g(i));

RotMatx = [r11,r12,r13; r21,r22,r23; r31,r32,r33] % ------% ------Leg 1 ------By = B1y; Mx = M1x; My = M1y; R = R1; L = L1; P = P1; r = r1; eps = eps1; del = del1; disp(' Leg 1'); IOKSolve_Graphical; end figure; subplot(321) 129 plot(g/DR,Theta1d,'r',g/DR,Theta2d,'b',g/DR,Theta3d,'r',g/DR,Theta4d,'b'); grid; set(gca,'Fontsize',18); axis([0 360 -Inf Inf]); title('Change in \gamma vs \theta for leg 1'); subplot(322) plot(g/DR,phi1d,'r',g/DR,phi2d,'b',g/DR,phi3d,'r',g/DR,phi4d,'b'); grid; set(gca,'Fontsize',18); axis([0 360 -Inf Inf]); title('Change in \gamma vs \phi for leg 1'); for i = 1:N, % ------% ------Orthonormal Rotation Matrix ------r11 = cos(a)*cos(b); r12 = -sin(a)*cos(g(i))+cos(a)*sin(b)*sin(g(i)); r13 = sin(a)*sin(g(i))+cos(a)*sin(b)*cos(g(i)); r21 = sin(a)*cos(b); r22 = cos(a)*cos(g(i))+sin(a)*sin(b)*sin(g(i)); r23 = -cos(a)*sin(g(i))+sin(a)*sin(b)*cos(g(i)); r31 = -sin(b); r32 = cos(b)*sin(g(i)); r33 = cos(b)*cos(g(i));

RotMatx = [r11,r12,r13; r21,r22,r23; r31,r32,r33] % ------% ------Leg 2 ------By = B2y; Mx = M2x; My = M2y; R = R2; L = L2; P = P2; r = r2; eps = eps2; del = del2; disp(' Leg 2'); IOKSolve_Graphical; end subplot(323) plot(g/DR,Theta1d,'r',g/DR,Theta2d,'b',g/DR,Theta3d,'r',g/DR,Theta4d,'b'); grid; set(gca,'Fontsize',18); axis([0 360 -Inf Inf]); title('Change in \gamma vs \theta for leg 2'); legend('Open-Branch','Crossed-Branch'); ylabel('\theta'); subplot(324) plot(g/DR,phi1d,'r',g/DR,phi2d,'b',g/DR,phi3d,'r',g/DR,phi4d,'b'); grid; set(gca,'Fontsize',18); axis([0 360 -Inf Inf]); title('Change in \gamma vs \phi for leg 2'); ylabel('\phi'); 130

for i = 1:N, % ------% ------Orthonormal Rotation Matrix ------r11 = cos(a)*cos(b); r12 = -sin(a)*cos(g(i))+cos(a)*sin(b)*sin(g(i)); r13 = sin(a)*sin(g(i))+cos(a)*sin(b)*cos(g(i)); r21 = sin(a)*cos(b); r22 = cos(a)*cos(g(i))+sin(a)*sin(b)*sin(g(i)); r23 = -cos(a)*sin(g(i))+sin(a)*sin(b)*cos(g(i)); r31 = -sin(b); r32 = cos(b)*sin(g(i)); r33 = cos(b)*cos(g(i));

RotMatx = [r11,r12,r13; r21,r22,r23; r31,r32,r33] % ------% ------Leg 3 ------By = B3y; Mx = M3x; My = M3y; R = R3; L = L3; P = P3; r = r3; eps = eps3; del = del3; disp(' Leg 3'); IOKSolve_Graphical; end subplot(325) plot(g/DR,Theta1d,'r',g/DR,Theta2d,'b',g/DR,Theta3d,'r',g/DR,Theta4d,'b'); grid; set(gca,'Fontsize',18); axis([0 360 -Inf Inf]); title('Change in \gamma vs \theta for leg 3'); xlabel('\gamma'); subplot(326) plot(g/DR,phi1d,'r',g/DR,phi2d,'b',g/DR,phi3d,'r',g/DR,phi4d,'b'); grid; set(gca,'Fontsize',18); axis([0 360 -Inf Inf]); title('Change in \gamma vs \phi for leg 3'); xlabel('\gamma');

131

12. Program for In-line Function called by IOK_Solution_Alpha.m

%------% IOKSolve_Alpha.m % Programmer: Joey Feck % In-line function called for IOK_Solution_Alpha.m %------% A - F terms A = (r*(-r21*sin(eps) + r22*cos(eps)))/sin(del); B = (r23*r)/sin(del); C = (-r23*LB - r21*Mx - r22*My + By)/sin(del); D = r*(r31*sin(eps) - r32*cos(eps)); E = -r33*r; F = r33*LB + LA +r31*Mx + r32*My;

% a1 - a6 terms a1 = A^2 + D^2; a2 = B^2 + E^2; a3 = 2*(A*B + D*E); a4 = 2*(A*C + D*F); a5 = 2*(B*C + E*F); a6 = C^2 + F^2 - L^2;

% A4 - A0 quartic polynomial coefficients A4 = a1 - a4 + a6; A3 = -2*(a3 - a5); A2 = -2*(a1 - 2*a2 - a6); A1 = 2*(a3 + a5); A0 = a1 + a4 + a6; tt = roots([A4 A3 A2 A1 A0]); % ------% ------Writing External File------f_rd = fopen('IOK_RESULTS.txt','a+'); al = a(i)*RD; be = b*RD; ga = g*RD; de = del*RD; ep = eps*RD; ini_value = fprintf(f_rd,'%g\t%g\t%g\t%g\t%g\t',al,be,ga,de,ep) % ------% ------Calculation of possible theta and phi ------if imag(tt(1)) == 0 Theta1 = 2*atan(tt(1)); Theta1d = 2*atan(tt(1))*RD phi1 = atan2(D*cos(Theta1)+E*sin(Theta1)+F,A*cos(Theta1)+B*sin(Theta1)+C); 132

phi1d = (atan2(D*cos(Theta1)+E*sin(Theta1)+F,A*cos(Theta1)+B*sin(Theta1)+C))*RD else Theta1d = NaN; phi1d = NaN; end %------if imag(tt(2)) == 0 Theta2 = 2*atan(tt(2)); Theta2d = 2*atan(tt(2))*RD phi2 = atan2(D*cos(Theta2)+E*sin(Theta2)+F,A*cos(Theta2)+B*sin(Theta2)+C); phi2d = (atan2(D*cos(Theta2)+E*sin(Theta2)+F,A*cos(Theta2)+B*sin(Theta2)+C))*RD else Theta2d = NaN; phi2d = NaN; end %------if imag(tt(3)) == 0 Theta3 = 2*atan(tt(3)); Theta3d = 2*atan(tt(3))*RD phi3 = atan2(D*cos(Theta3)+E*sin(Theta3)+F,A*cos(Theta3)+B*sin(Theta3)+C); phi3d = (atan2(D*cos(Theta3)+E*sin(Theta3)+F,A*cos(Theta3)+B*sin(Theta3)+C))*RD else Theta3d = NaN; phi3d = NaN; end %------if imag(tt(4)) == 0 Theta4 = 2*atan(tt(4)); Theta4d = 2*atan(tt(4))*RD phi4 = atan2(D*cos(Theta4)+E*sin(Theta4)+F,A*cos(Theta4)+B*sin(Theta4)+C); phi4d = (atan2(D*cos(Theta4)+E*sin(Theta4)+F,A*cos(Theta4)+B*sin(Theta4)+C))*RD else Theta4d = NaN; phi4d = NaN; end fclose(f_rd)

133

13. Program for In-line Function called by IOK_Solution_Beta.m

%------% IOKSolve_Beta.m % Programmer: Joey Feck % In-line function called for IOK_Solution_Beta.m %------% A - F terms A = (r*(-r21*sin(eps) + r22*cos(eps)))/sin(del); B = (r23*r)/sin(del); C = (-r23*LB - r21*Mx - r22*My + By)/sin(del); D = r*(r31*sin(eps) - r32*cos(eps)); E = -r33*r; F = r33*LB + LA+r31*Mx + r32*My;

% a1 - a6 terms a1 = A^2 + D^2; a2 = B^2 + E^2; a3 = 2*(A*B + D*E); a4 = 2*(A*C + D*F); a5 = 2*(B*C + E*F); a6 = C^2 + F^2 - L^2;

% A4 - A0 quartic polynomial coefficients A4 = a1 - a4 + a6; A3 = -2*(a3 - a5); A2 = -2*(a1 - 2*a2 - a6); A1 = 2*(a3 + a5); A0 = a1 + a4 + a6; tt = roots([A4 A3 A2 A1 A0]);

% ------% ------Writing External File------f_rd = fopen('IOK_RESULTS.txt','a+'); al = a*RD; be = b(i)*RD; ga = g*RD; de = del*RD; ep = eps*RD; ini_value = fprintf(f_rd,'%g\t%g\t%g\t%g\t%g\t',al,be,ga,de,ep) % ------% ------Calculation of possible theta and phi ------if imag(tt(1)) == 0 134

Theta1 = 2*atan(tt(1)); Theta1d = 2*atan(tt(1))*RD phi1 = atan2(D*cos(Theta1)+E*sin(Theta1)+F,A*cos(Theta1)+B*sin(Theta1)+C); phi1d = (atan2(D*cos(Theta1)+E*sin(Theta1)+F,A*cos(Theta1)+B*sin(Theta1)+C))*RD else Theta1d = NaN; phi1d = NaN; end %------if imag(tt(2)) == 0 Theta2 = 2*atan(tt(2)); Theta2d = 2*atan(tt(2))*RD phi2 = atan2(D*cos(Theta2)+E*sin(Theta2)+F,A*cos(Theta2)+B*sin(Theta2)+C); phi2d = (atan2(D*cos(Theta2)+E*sin(Theta2)+F,A*cos(Theta2)+B*sin(Theta2)+C))*RD else Theta2d = NaN; phi2d = NaN; end %------if imag(tt(3)) == 0 Theta3 = 2*atan(tt(3)); Theta3d = 2*atan(tt(3))*RD phi3 = atan2(D*cos(Theta3)+E*sin(Theta3)+F,A*cos(Theta3)+B*sin(Theta3)+C); phi3d = (atan2(D*cos(Theta3)+E*sin(Theta3)+F,A*cos(Theta3)+B*sin(Theta3)+C))*RD else Theta3d = NaN; phi3d = NaN; end %------if imag(tt(4)) == 0 Theta4 = 2*atan(tt(4)); Theta4d = 2*atan(tt(4))*RD phi4 = atan2(D*cos(Theta4)+E*sin(Theta4)+F,A*cos(Theta4)+B*sin(Theta4)+C); phi4d = (atan2(D*cos(Theta4)+E*sin(Theta4)+F,A*cos(Theta4)+B*sin(Theta4)+C))*RD else Theta4d = NaN; phi4d = NaN; end fclose(f_rd) 135

14. Program for In-line Function called by IOK_Solution_Gamma.m

%------% IOKSolve_Gamma.m % Programmer: Joey Feck % In-line function called for IOK_Solution_Gamma.m %------% A - F terms A = (r*(-r21*sin(eps) + r22*cos(eps)))/sin(del); B = (r23*r)/sin(del); C = (-r23*LB - r21*Mx - r22*My + By)/sin(del); D = r*(r31*sin(eps) - r32*cos(eps)); E = -r33*r; F = r33*LB + LA+r31*Mx + r32*My;

% a1 - a6 terms a1 = A^2 + D^2; a2 = B^2 + E^2; a3 = 2*(A*B + D*E); a4 = 2*(A*C + D*F); a5 = 2*(B*C + E*F); a6 = C^2 + F^2 - L^2;

% A4 - A0 quartic polynomial coefficients A4 = a1 - a4 + a6; A3 = -2*(a3 - a5); A2 = -2*(a1 - 2*a2 - a6); A1 = 2*(a3 + a5); A0 = a1 + a4 + a6; tt = roots([A4 A3 A2 A1 A0]);

% ------% ------Writing External File------f_rd = fopen('IOK_RESULTS.txt','a+'); al = a*RD; be = b*RD; ga = g(i)*RD; de = del*RD; ep = eps*RD; ini_value = fprintf(f_rd,'%g\t%g\t%g\t%g\t%g\t',al,be,ga,de,ep) % ------% ------Calculation of possible theta and phi ------if imag(tt(1)) == 0 136

Theta1 = 2*atan(tt(1)); Theta1d = 2*atan(tt(1))*RD phi1 = atan2(D*cos(Theta1)+E*sin(Theta1)+F,A*cos(Theta1)+B*sin(Theta1)+C); phi1d = (atan2(D*cos(Theta1)+E*sin(Theta1)+F,A*cos(Theta1)+B*sin(Theta1)+C))*RD else Theta1d = NaN; phi1d = NaN; end %------if imag(tt(2)) == 0 Theta2 = 2*atan(tt(2)); Theta2d = 2*atan(tt(2))*RD phi2 = atan2(D*cos(Theta2)+E*sin(Theta2)+F,A*cos(Theta2)+B*sin(Theta2)+C); phi2d = (atan2(D*cos(Theta2)+E*sin(Theta2)+F,A*cos(Theta2)+B*sin(Theta2)+C))*RD else Theta2d = NaN; phi2d = NaN; end %------if imag(tt(3)) == 0 Theta3 = 2*atan(tt(3)); Theta3d = 2*atan(tt(3))*RD phi3 = atan2(D*cos(Theta3)+E*sin(Theta3)+F,A*cos(Theta3)+B*sin(Theta3)+C); phi3d = (atan2(D*cos(Theta3)+E*sin(Theta3)+F,A*cos(Theta3)+B*sin(Theta3)+C))*RD else Theta3d = NaN; phi3d = NaN; end %------if imag(tt(4)) == 0 Theta4 = 2*atan(tt(4)); Theta4d = 2*atan(tt(4))*RD phi4 = atan2(D*cos(Theta4)+E*sin(Theta4)+F,A*cos(Theta4)+B*sin(Theta4)+C); phi4d = (atan2(D*cos(Theta4)+E*sin(Theta4)+F,A*cos(Theta4)+B*sin(Theta4)+C))*RD else Theta4d = NaN; phi4d = NaN; end fclose(f_rd) 137

15. In-line Function called by Graphical Interpretation Programs for the IOK

Solution

%------% IOKSolve_Graphical.m % Programmer: Joey Feck % In-line Function called for IOK_Solution_Alpha_Graphical.m %------% A - F terms A = (r*(-r21*sin(eps) + r22*cos(eps)))/sin(del); B = (r23*r)/sin(del); C = (-r23*LB - r21*Mx - r22*My + By)/sin(del); D = r*(r31*sin(eps) - r32*cos(eps)); E = -r33*r; F = r33*LB + LA+r31*Mx + r32*My;

% a1 - a6 terms a1 = A^2 + D^2; a2 = B^2 + E^2; a3 = 2*(A*B + D*E); a4 = 2*(A*C + D*F); a5 = 2*(B*C + E*F); a6 = C^2 + F^2 - L^2;

% A4 - A0 quartic polynomial coefficients A4 = a1 - a4 + a6; A3 = -2*(a3 - a5); A2 = -2*(a1 - 2*a2 - a6); A1 = 2*(a3 + a5); A0 = a1 + a4 + a6; tt = roots([A4 A3 A2 A1 A0]); % ------% ------Calculation of possible theta and phi ------if imag(tt(1)) == 0 Theta1(i) = 2*atan(tt(1)); Theta1d(i) = 2*atan(tt(1))*RD phi1(i) = atan2(D*cos(Theta1(i))+E*sin(Theta1(i))+F,A*cos(Theta1(i))+B*sin(Theta1(i))+C); phi1d(i) = (atan2(D*cos(Theta1(i))+E*sin(Theta1(i))+F,A*cos(Theta1(i))+B*sin(Theta1(i))+C))*R D else Theta1d(i) = NaN; phi1d(i) = NaN; 138 end %------if imag(tt(2)) == 0 Theta2(i) = 2*atan(tt(2)); Theta2d(i) = 2*atan(tt(2))*RD phi2(i) = atan2(D*cos(Theta2(i))+E*sin(Theta2(i))+F,A*cos(Theta2(i))+B*sin(Theta2(i))+C); phi2d(i) = (atan2(D*cos(Theta2(i))+E*sin(Theta2(i))+F,A*cos(Theta2(i))+B*sin(Theta2(i))+C))*R D else Theta2d(i) = NaN; phi2d(i) = NaN; end %------if imag(tt(3)) == 0 Theta3(i) = 2*atan(tt(3)); Theta3d(i) = 2*atan(tt(3))*RD phi3(i) = atan2(D*cos(Theta3(i))+E*sin(Theta3(i))+F,A*cos(Theta3(i))+B*sin(Theta3(i))+C); phi3d(i) = (atan2(D*cos(Theta3(i))+E*sin(Theta3(i))+F,A*cos(Theta3(i))+B*sin(Theta3(i))+C))*R D else Theta3d(i) = NaN; phi3d(i) = NaN; end %------if imag(tt(4)) == 0 Theta4(i) = 2*atan(tt(4)); Theta4d(i) = 2*atan(tt(4))*RD phi4(i) = atan2(D*cos(Theta4(i))+E*sin(Theta4(i))+F,A*cos(Theta4(i))+B*sin(Theta4(i))+C); phi4d(i) = (atan2(D*cos(Theta4(i))+E*sin(Theta4(i))+F,A*cos(Theta4(i))+B*sin(Theta4(i))+C))*R D else Theta4d(i) = NaN; phi4d(i) = NaN; end

139

16. Program for the Forward Orientation Kinematics Solution

%------% Forward Orientation Kinematics for 3-RSR Parallel Robot % Programmer: Joey Feck %------clc; clear; DR = pi/180; RD = 180/pi; % ------Inputs ------I =input('Enter [th1 th2 th3]:'); th1 = I(1)*DR; th2 = I(2)*DR; th3 = I(3)*DR; Constants; X = input('Enter Initial Guess [a b g phi1 phi2 phi3] '); a = X(1)*DR; b = X(2)*DR; g = X(3)*DR; phi1 = X(4)*DR; phi2 = X(5)*DR; phi3 = X(6)*DR; eps = 0.001; % Convergence tolerance for del X MAX = 100; % MAX iterations del_X = [999;999;999]; % Initialize error vector counts = 0; % Initialize iteration counter while (norm(del_X) > eps & counts < MAX) % ------F Equations ------sa = sin(a); ca = cos(a); sb = sin(b); cb = cos(b); sg = sin(g); cg = cos(g);

F1 = (-ca*sg+sa*sb*cg)*LB + (sa*cb)*M1x + (ca*cg+sa*sb*sg)*M1y + (sa*cb)*r1*cos(th1)*sin(eps1) - (ca*cg+sa*sb*sg)*r1*cos(th1)*cos(eps1) - (- ca*sg+sa*sb*cg)*r1*sin(th1) - B1y + L1*cos(phi1)*sin(del1); F2 = (cb*cg)*LB + LA + (-sb)*M1x + (cb*sg)*M1y + (-sb)*r1*cos(th1)*sin(eps1) - (cb*sg)*r1*cos(th1)*cos(eps1) - (cb*cg)*r1*sin(th1) - L1*sin(phi1); F3 = (-ca*sg+sa*sb*cg)*LB + (sa*cb)*M2x + (ca*cg+sa*sb*sg)*M2y + (sa*cb)*r2*cos(th2)*sin(eps2) - (ca*cg+sa*sb*sg)*r2*cos(th2)*cos(eps2) - (- ca*sg+sa*sb*cg)*r2*sin(th2) - B2y + L2*cos(phi2)*sin(del2); F4 = (cb*cg)*LB + LA + (-sb)*M2x + (cb*sg)*M2y + (-sb)*r2*cos(th2)*sin(eps2) - (cb*sg)*r2*cos(th2)*cos(eps2) - (cb*cg)*r2*sin(th2) - L2*sin(phi2); F5 = (-ca*sg+sa*sb*cg)*LB + (sa*cb)*M3x + (ca*cg+sa*sb*sg)*M3y + (sa*cb)*r3*cos(th3)*sin(eps3) - (ca*cg+sa*sb*sg)*r3*cos(th3)*cos(eps3) - (- ca*sg+sa*sb*cg)*r3*sin(th3) - B3y + L3*cos(phi3)*sin(del3); F6 = (cb*cg)*LB + LA + (-sb)*M3x + (cb*sg)*M3y + (-sb)*r3*cos(th3)*sin(eps3) - (cb*sg)*r3*cos(th3)*cos(eps3) - (cb*cg)*r3*sin(th3) - L3*sin(phi3); 140

F = [F1;F2;F3;F4;F5;F6]; % ------Jacobian Matrix ------% ------Column 1 ------J11 = (sa*sg+ca*sb*cg)*LB + (ca*cb)*M1x +(-sa*cg+ca*sb*sg)*M1y +(ca*cb)*r1*cos(th1)*sin(eps1) +(sa*cg-ca*sb*sg)*r1*cos(th1)*cos(eps1) - (sa*sg+ca*sb*cg)*r1*sin(th1); J21 = 0;

J31 = (sa*sg+ca*sb*cg)*LB + (ca*cb)*M2x +(-sa*cg+ca*sb*sg)*M2y +(ca*cb)*r2*cos(th2)*sin(eps2) +(sa*cg-ca*sb*sg)*r2*cos(th2)*cos(eps2) - (sa*sg+ca*sb*cg)*r2*sin(th2); J41 = 0;

J51 = (sa*sg+ca*sb*cg)*LB + (ca*cb)*M3x +(-sa*cg+ca*sb*sg)*M3y +(ca*cb)*r3*cos(th3)*sin(eps3) +(sa*cg-ca*sb*sg)*r3*cos(th3)*cos(eps3) - (sa*sg+ca*sb*cg)*r3*sin(th3); J61 = 0; % ------Column 2 ------J12 = (sa*cb*cg)*LB + (-sa*sb)*M1x + (sa*cb*sg)*M1y + (- sa*sb)*r1*cos(th1)*sin(eps1) - (sa*cb*sg)*r1*cos(th1)*cos(eps1) - (sa*cb*cg)*r1*sin(th1); J22 = (-sb*cg)*LB + LA +(-cb)*M1x + (-sb*sg)*M1y + (-cb)*r1*cos(th1)*sin(eps1) - (- sb*sg)*r1*cos(th1)*cos(eps1) - (-sb*cg)*r1*sin(th1);

J32 = (sa*cb*cg)*LB + (-sa*sb)*M2x + (sa*cb*sg)*M2y + (- sa*sb)*r2*cos(th2)*sin(eps2) - (sa*cb*sg)*r2*cos(th2)*cos(eps2) - (sa*cb*cg)*r2*sin(th2); J42 = (-sb*cg)*LB + LA +(-cb)*M2x + (-sb*sg)*M2y + (-cb)*r2*cos(th2)*sin(eps2) - (- sb*sg)*r2*cos(th2)*cos(eps2) - (-sb*cg)*r2*sin(th2);

J52 = (sa*cb*cg)*LB + (-sa*sb)*M3x + (sa*cb*sg)*M3y + (- sa*sb)*r3*cos(th3)*sin(eps3) - (sa*cb*sg)*r3*cos(th3)*cos(eps3) - (sa*cb*cg)*r3*sin(th3); J62 = (-sb*cg)*LB + LA +(-cb)*M3x + (-sb*sg)*M3y + (-cb)*r3*cos(th3)*sin(eps3) - (- sb*sg)*r3*cos(th3)*cos(eps3) - (-sb*cg)*r3*sin(th3);

% ------Column 3 ------

J13 = (-ca*cg-sa*sb*sg)*LB - (ca*sg-sa*sb*cg)*M1y + (ca*sg- sa*sb*cg)*r1*cos(th1)*cos(eps1) + (ca*cg+sa*sb*sg)*r1*sin(th1); J23 = (-cb*sg)*LB + (cb*cg)*M1y - (cb*cg)*r1*cos(th1)*cos(eps1) + (cb*sg)*r1*sin(th1);

141

J33 = (-ca*cg-sa*sb*sg)*LB - (ca*sg-sa*sb*cg)*M2y + (ca*sg- sa*sb*cg)*r2*cos(th2)*cos(eps2) + (ca*cg+sa*sb*sg)*r2*sin(th2); J43 = (-cb*sg)*LB + (cb*cg)*M2y - (cb*cg)*r2*cos(th2)*cos(eps2) + (cb*sg)*r2*sin(th2);

J53 = (-ca*cg-sa*sb*sg)*LB - (ca*sg-sa*sb*cg)*M3y + (ca*sg- sa*sb*cg)*r3*cos(th3)*cos(eps3) + (ca*cg+sa*sb*sg)*r3*sin(th3); J63 = (-cb*sg)*LB + (cb*cg)*M3y - (cb*cg)*r3*cos(th3)*cos(eps3) + (cb*sg)*r3*sin(th3);

% ------Column 4 ------

J14 = -L1*sin(phi1)*sin(del1); J24 = -L1*cos(phi1); J34 = 0; J44 = 0; J54 = 0; J64 = 0;

% ------Column 5 ------

J15 = 0; J25 = 0; J35 = -L2*sin(phi2)*sin(del2); J45 = -L2*cos(phi2); J55 = 0; J65 = 0;

% ------Column 6 ------

J16 = 0; J26 = 0; J36 = 0; J46 = 0; J56 = -L3*sin(phi3)*sin(del3); J66 = -L3*cos(phi3);

% ------Calculate Jacobian Matrix ------

J_NR = [J11,J12,J13,J14,J15,J16; J21,J22,J23,J24,J25,J26; J31,J32,J33,J34,J35,J36; J41,J42,J43,J44,J45,J46; J51,J52,J53,J54,J55,J56; J61,J62,J63,J64,J65,J66]; % ------% ------del_X = -inv(J_NR)*F; a = a + del_X(1); b = b + del_X(2); g = g + del_X(3); phi1 = phi1 + del_X(4); 142 phi2 = phi2 + del_X(5); phi3 = phi3 + del_X(6); counts = counts + 1; end counts a*RD b*RD g*RD phi1*RD phi2*RD phi3*RD ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! !

Thesis and Dissertation Services ! !