This document discusses solving the inverse kinematics problem for a 6 degree of freedom robot using numerical methods and algorithms. It begins by introducing the robot and describing the inverse kinematics problem. It then outlines the process used to determine the lengths of each link by analyzing the robot configuration. Equations are developed relating the joint angles to cylindrical coordinates. Initial conditions are established indicating how the links contribute to the radial and length components at different joint angles.

1. Robot Analysis
Obtaining InverseKinematic Solutions with
Numerical Methods and Algorithms
Thisdocumentcontainsinformationregardingthe processes
involvedinsolvingthe inverse kinematicsproblemforrobots.
Chad Ryan Weiss
4/25/2016

2. 1
Abstract
Before robots,humanswere limitedinfunctionalitydue toonlyhavingtwohands,eightfingersandtwo
opposable thumbs. Withthis,surgeonshadtorelyon theirnervestoperformthe mostdifficult
proceduresknowntoman. One twitchand that couldspell life ordeathforthe patient. Thisisjustone
example whererobotshave hadanunbelievableimpactonsociety. Theyare usedtosave livesaswell
as to build,create anddestroy. Inan attemptto recreate a tool that isas dexterousasthe humanhand,
that isas smart as the human brainand as strongas the hardesttitaniumalloy,scientists,
mathematicians,engineersandtechnologistsfromall overthe worldhave dedicatedtheirlivestothe
subjectthatis roboticsandto its problems. The major problemthatthispiece of workseekstoaddress
isthe inverse kinematicsproblem.

3. 2
Table of Contents
Abstract 1
Contents 2
Introduction 3
Theory 3
Implementation 4
Results 18
Conclusion 18
List of Figures
Figure 1: 6 DOF Robot 3
Figure 2: Determiningthe Lengthof Link0 4
Figure 3: Determiningthe Lengthof the SecondLink(Link1) 5
Figure 4: Determiningthe Lengthof Link2 6
Figure 5: Base Revolute JointinAction 7
Figure 6: Initial Settingof Robot 7
Figure 7: LinkAnalysisof Robot 8
Figure 8: JointAnalysisof Robot 9
Figure 9: Jointand LinkAnalysisof Robot 10
Figure 10: OffsetAnalysisof Robot 11
Figure 11: RobotLengthLowerBound 14
Figure 12: RobotLengthUpper Bound 15
Figure 13: RobotRadiusUpperBound 15

4. 3
Introduction
The objective of this documentistoprovide insightof the processesrequiredto developanalgorithm
that will calculate the inverse kinematicssolution of a6 degree of freedom(DOF)robot. Ourproblem
will onlyconsiderthe firstthree jointsandlinksof the robotwhen obtainingthe inversekinematics
solution. Figure 1shows the robotthat we will be workingwith:
Theory
Like the hammeror the axe,the robot isa tool designedbyman(engineers) toserve a specificpurpose.
In orderto designamultipurpose robotlikethe one showninFigure 1,one must considerthe kinematic
engineeringrequirements of the robot. Inhighschool physicswe learnedaboutthe basickinematic
equations, indynamicswe learnedhowtoanalyze the motionof objectsinaninterconnectedsystem,in
roboticswe put thisknowledgetoworkusingforwardandinverse kinematicstoanalyze anddesign
robotsso that theycan fulfill theirpurpose. The conceptof forwardversusinverse kinematicsis very
importantandcannot be overlookedwhenanalyzingordesigningrobots.
Forwardkinematicsissimilartowhatwe learnedindynamics. If we rotate the base X degrees,then
rotate the secondjointYdegreesandthe thirdjointZ degrees,wherewillthe robot tipbe located? Itis
the studyof howobjectsmove togetherinaninterconnectedsystem. Inverse kinematicsonthe other
hand,is the same thingbutwitha differentapproach. The scenariobeing,if Iwantthe robottip to be
locatedat the coordinate (A,B,C) withrespecttothe universal (fixed)axis,whatjointrotations (X,Y,Z)
wouldbe requiredtogetto thispointinspace?
Figure 1: 6 DOF Robot

5. 4
In theory,itispossible tohave nosolution,one solution,more thanone solutionorinfinitelymany
solutions tothistype of problem. Inpractice however, itisnotpossible tohave infinitelymanysolutions
due to the constraintsof the robotsuch as the resolutionof the steppermotor,actuatorordriveras
well asthe lengthsof the links,etc. Thisprocessof discoveringthe necessaryjointmovementsinorder
to reach a specificenddestinationisthe inverse kinematicmethodforsolvingrobotequationsandwe
will use thistheoryin thisreport.
Implementation
Basically,we will be assemblingaMatLab computerprogramthat will tryto get the jointanglesof the
robot to converge (the robotend-effector)withthe cylindrical coordinatesof the Cartesianinputs
providedbythe user.
First,itis importanttoknowthe dimensionsof the robot. Hence,we will beginby calculatingthe
lengthsof the links. Refertothe figure below tosee how we determine the lengthof the firstlink.
If you lookat Figure 2, youwill see thatthe lengthof the robot base can be determinedbyliningupthe
end-effectorwiththe jointaxisof rotation. Since the software wasdesignedtotell usthe coordinatesof
the end-effector,liningthe end-effectorupwiththe endpointof link0(the base) will give usthe length
of link0. We can determinethe lengthbyobservingthe heightatwhichthe end-effectorhasbeen
placed. Hence we can confidentlysaythatthe lengthof link0 is approximately285.277 mm.
% Define Robot Variables
L0 = 285; % Base length of robot in mm
Figure 2: Determining the Length of Link 0

6. 5
Nowwe will move ontolink1. If you refertothe figure below youwill see how we determinedthe
lengthof the linkdirectlyconnectedtothe base link(link1).
Figure 3: Determining the Length of the Second Link (Link 1)
If you lookcloselyatFigure 3, youwill see thatwe usedthe same methodindeterminingthe lengthof
the firstlink(i.e.the base) todetermine the secondlink. We linedup the end-effectorwiththe axisof
rotationof the thirdjoint,whichconstitutesthe endof the secondlinkandbeginningof the thirdlink,
and we lookedatthe total heightof the robotend-effector. Thistime the Zcoordinate reads545.175
mm. Thislengthisthe lengthof the firstand secondlinkcombined. Tofindthe lengthof the second
linkwe simplysubtractthe lengthof the firstlink,whichwe foundpreviously. Sothe lengthof the
secondlinkcomesoutto be:
𝐿𝑙𝑖𝑛𝑘1 = 𝐻𝑡𝑜𝑡𝑎𝑙 − 𝐿𝑙𝑖𝑛𝑘0
Equation1 givesusa link1 lengthof 545.175 mm minus285.277 mm or approximately,259.898 mm.
We make sure to define thislinkinMatLabwiththe followingcode:
% Define Robot Variables
L1 = 260; % Link 1 length in mm
Finally,todetermine the lastlink,take alookatthe figure below.
(1)

7. 6
Lookingat Figure 4, we see thatthe total heighthasnow reached1035.785 mm at full extension. Using
the same methodsas before we candetermine the lengthof the lastlink(link2) byusingthe following
equation:
𝐿𝑙𝑖𝑛𝑘2 = 𝐻𝑡𝑜𝑡𝑎𝑙 − 𝐿𝑙𝑖𝑛𝑘0 − 𝐿𝑙𝑖𝑛𝑘1
UsingEquation2, we determinethe lengthof link2to be 1035.785 mm minus545.175 mm or 490.61
mm. We thenrecord thisinformationandstore itinthe MatLab variable C2.
% Define Robot Variables
L2 = 490; % Link 2 length in mm
Nowthat we have determinedthe lengthsof the linksof ourrobotwe can begintoanalyze the robotto
come up witha seriesof equationsthatwe canuse to relate the jointanglestothe coordinate systemof
choice.
To choose the bestcoordinate system,letusconsiderwhattype of robotwe are dealingwith. There are
three joints,the base,the secondlinkandthe third. The base jointrotatesaboutthe z-axisata sweep
of 165 to -165 degrees.RefertoFigure 5below tosee the base revolute jointinaction.
Figure 4: Determining the Length of Link 2
(2)

8. 7
Basedon thisone joint,itisincrediblysimple tomake thisproblemacylindrical one because the base
jointcan act as a fixedangle inourequations. Thisallowsustoskipthe stepof writingan equation for
the firstjoint,because inacylindrical coordinate system theta_0equalsalphaor(𝜃0 = 𝛼). Despite this
fact, we still have todevelopthe equationsforoursecondandthirdjoints. Todo this,we will analyze
the robot some more.
Figure 6 shows the initial settingof the robot. The firstthree jointanglesare settozerodegreesandthe
Cartesiancoordinatesof the end-effectorare (475.785mm, -82.198mm, 628.840mm). See the figure
belowforconfirmationof thisfact.
Figure 5: Base Revolute Joint in Action
Figure 6: Initial Setting of Robot

9. 8
Figure 7: Link Analysis of Robot
!!! ImportantNote:(RefertoFigure 7)
Althoughthe end-effectorcoordinatesshow (475.785, -82.198, 628.840) we can assume thatthe end-
effectorcoordinatesof ourrobotreside atthe point(475.785, 0, 628.840). Thisassumptionismade
because of our hypothetical situation,the robotendlink(i.e.link2or L_2) can be representedasa long
straightarm to the endpoint. In all practicality,thisrobothasan end-effectortool thathasa tip that
jutsout intothe y-plane -82.198 mm. We can disregardthisfactfor the purpose of thisanalysis.
End Note !!!
Nowthat we have takena lookat the robot,we will convertthe Cartesiancoordinatesof the end-
effectortoa setof Cylindrical coordinates. Todo this,we mustapplythe followingequations:
𝑟 = √𝑥2 + 𝑦2
𝑙 = 𝑧
The variable r representsthe radiusof the cylinderwhile the variablel representsthe heightof the
cylinder. Ina Cartesiancoordinate system,the coordinatesare representedbythe variablesx,yandz
whichresemble the length,widthand heightordepthof the three-dimensional object. The cylindrical
(3)
(4)

10. 9
coordinate systemisathree dimensional coordinatesystemaswellbutitiswrittenina slightlydifferent
format. Cylindrical coordinatesare writteninthe form(radius, alpha, length). Alphaisthe base angle of
the cylinder(See Figure 5) andthe l and r variableswere alreadydiscussed.
Lookingback at Figure 7 and includingourfirstassumption,the end-effectorCartesiancoordinatesare
(475.785, 0, 628.840). In Cylindrical,thissetof pointsbecomes(475.785, 0, 628.840). Eventhoughthey
may lookthe same,theyare not! The middle coordinate hasswitchedfromameasure of distance toa
measure of degrees.
Withthisknowledge,we cannowsetsome initial conditionsthatwill helpuswrite the equationsthat
relate the jointanglesof ourrobot to the cylindrical coordinatevariables. Inorderto determine the
initial conditions,we have toconducta jointanalysisof the robot. See the figure belowforthisstep.
Figure 8: Joint Analysis of Robot
Upon lookingatFigure 8, itis immediatelyrecognizedthatTheta_0is Alphaor(𝜃0 = 𝛼). Theta_1 and
Theta_2 however,are notso easy. The length and radiuscoordinatesof theend-effectoraredependent
on Theta_1 and Theta_2. See the nextfigure fora betterdepictionof thisrelationship.

11. 10
Figure 9: Joint and Link Analysis of Robot
Rememberingthatthisinitial setupof the robotiswhenall of our jointanglesare equal tozero. For
that reason itis clearto note that the secondlink(link1) contributestothe lengththe mostwhenits
correspondingjoint(i.e.jointangle theta_1) isatzerodegrees. The base linkwill alwayshave afixed
lengthinthe L directionof the cylindrical coordinate systembutthe secondlinkwill contribute mostto
thisdirectionwhenithasa jointangle theta_1equal to zero. From thiswe can make the following
initial conditionstatementsaboutlink1.
Whentheta_1 equalszero,the lengthof the robotlink (link 1) isat itsmaximumvalue (inthe L
direction). Onthe otherhand,whentheta_1equals90 degrees,the lengthof the robotlink(link1- in
the L direction) iszero. Thissoundslike the cosinefunction.
It issafe to say that 𝐿1_𝑚𝑎𝑥 = 𝐿1 ∗ cos(𝜃1 ) whentheta_1 isequal tozero.
Furthermore,we cansay thatthe lengthof the thirdlink(link2) inthe L directionisat a minimumwhen
theta_2 isequal to zero. If youreferto Figure 9,you can clearlysee thatthe lengthof the thirdlinkis
greatestinthe R directionwhentheta_2equalszero. Itturns outthat thislinkresemblesthe sine

12. 11
functionandit issafe to say that 𝐿2_𝑚𝑎𝑥 = 𝐿2 ∗ sin(𝜃2) whentheta_2equals90 degrees. Inall
actuality,the lengthof the thirdlink(link2) isgreatestinthe L directionwhen theta_2is-90 degrees,
(accordingto the robot andsoftware) hence thislinkisbestrepresentedbyaninvertedsinefunction.
Here are the initial conditionequations:
For the radial componentsof link1
𝐿1_(𝑟−𝑐𝑜𝑚𝑝𝑜𝑛𝑒𝑛𝑡) = 𝐿1_(𝑟−𝑐𝑜𝑚𝑝𝑜𝑛𝑒𝑛𝑡_𝑚𝑖𝑛) 𝑤ℎ𝑒𝑛 𝜃1 = 0, sin(0°) = 0
𝐿1_(𝑟−𝑐𝑜𝑚𝑝𝑜𝑛𝑒𝑛𝑡) = 𝐿1_(𝑟−𝑐𝑜𝑚𝑝𝑜𝑛𝑒𝑛𝑡_𝑚𝑎𝑥) 𝑤ℎ𝑒𝑛 𝜃1 = ±90, abs[sin(±90°)] = 1
For the height(orlength) componentsof link1
𝐿1_(𝑙−𝑐𝑜𝑚𝑝𝑜𝑛𝑒𝑛𝑡) = 𝐿1_(𝑙−𝑐𝑜𝑚𝑝𝑜𝑛𝑒𝑛𝑡_𝑚𝑖𝑛) 𝑤ℎ𝑒𝑛 𝜃1 = ±90, cos(±90°) = 0
𝐿1_(𝑙−𝑐𝑜𝑚𝑝𝑜𝑛𝑒𝑛𝑡) = 𝐿1_(𝑙−𝑐𝑜𝑚𝑝𝑜𝑛𝑒𝑛𝑡_𝑚𝑎𝑥) 𝑤ℎ𝑒𝑛 𝜃1 = 0, cos(0°) = 1
* It isactuallyquite possible tohave alengththatcontributestolessthanzerounitsin the L direction
because the robotcan reach downwards,hence the minimumRandL componentsaren’texactlymet
whentheta_1hits 0 and 90 degreesrespectively. Justthinkhow the absolute minimumof the sine and
cosine functionsare actually -1and notzero andpicture each of the linksbendingbelow itsaxisof
rotation. There can be a negative lengthasthe robotreachesbelow the base jointaxisof rotation,but
there cannotbe a negative radiusof the cylinder. Thatiswhy we put the absolute value function
aroundthe sine functionforthe radial componentof link1. Withthese thoughtsinmindthese
equationsstill provideaveryaccurate if not perfectmodel of the robotitself.
For the radial componentsof link2
𝐿2_(𝑟−𝑐𝑜𝑚𝑝𝑜𝑛𝑒𝑛𝑡) = 𝐿2_(𝑟−𝑐𝑜𝑚𝑝𝑜𝑛𝑒𝑛𝑡_𝑚𝑖𝑛) 𝑤ℎ𝑒𝑛 𝜃2 = ±90, cos(±90°) = 0
𝐿2_(𝑟−𝑐𝑜𝑚𝑝𝑜𝑛𝑒𝑛𝑡) = 𝐿2_(𝑟−𝑐𝑜𝑚𝑝𝑜𝑛𝑒𝑛𝑡_𝑚𝑎𝑥) 𝑤ℎ𝑒𝑛 𝜃2 = 0, cos(0°) = 1
For the height(orlength) componentsof link 2
𝐿2_(𝑙−𝑐𝑜𝑚𝑝𝑜𝑛𝑒𝑛𝑡) = 𝐿2_(𝑙−𝑐𝑜𝑚𝑝𝑜𝑛𝑒𝑛𝑡_𝑚𝑖𝑛) 𝑤ℎ𝑒𝑛 𝜃2 = 0, sin(0°) = 0
𝐿2_(𝑙−𝑐𝑜𝑚𝑝𝑜𝑛𝑒𝑛𝑡) = 𝐿2_(𝑙−𝑐𝑜𝑚𝑝𝑜𝑛𝑒𝑛𝑡_𝑚𝑎𝑥) 𝑤ℎ𝑒𝑛 𝜃2 = −90, −sin(−90°) = 1
* Please note thatthe L directioncomponentof the thirdlink(link2) isnotactuallyat a minimumat
theta_2 equalszero. Like we saidearlier,the linkscancontribute tothe L directionina negative manner
and therefore,the sinefunctionisstill anaccurate wayto resemble thisparticularlink. The radial
componentof the thirdlink(link2) isgreatestwhentheta_2equalszeroandfor that reason,the cosine
functionis usedtoresemble thislink inthe Rdirection.
From these initialconditions, we come upwiththe followingequations: (NextPage)

13. 12
𝒍 = 𝑳 𝟎 + 𝑳 𝟏 ∗ 𝐜𝐨𝐬( 𝜽 𝟏)− 𝑳 𝟐 ∗ 𝐬𝐢𝐧(𝜽 𝟐 + 𝜽 𝟏)
𝒓 = 𝑳 𝟏 ∗ 𝐚𝐛𝐬[𝐬𝐢𝐧( 𝜽 𝟏)] + 𝑳 𝟐 ∗ 𝐜𝐨 𝐬( 𝜽 𝟐 + 𝜽 𝟏)
𝜶 = 𝜽 𝟎
It isalso importanttonote that the robot has jointangle movement restrictions.
−165° ≤ 𝜃0 ≤ 165°
−110° ≤ 𝜃1 ≤ 110°
−90° ≤ 𝜃2 ≤ 70°
Takingintoaccount all of thisinformation,we will use the knowledgeacquiredfromthisanalysisto
create an algorithmthatutilizesthe Gauss-Seidel methodforsolvingsimultaneouslinear equationsto
come up withan inverse kinematicsolutionforthisparticularrobot. Toaccomplishthis,we mustfirst
rewrite the equationsintermsof theta_1andtheta_2. We can simplydefinetheta_0asalpha inour
MatLab computerprogram. See the code below.
% Convert to Robot
theta0 = cylindrical_alpha;
theta1 = real(acos((cylindrical_l+L2*sin(theta2)-L0)/L1));
theta2 = real(acos((cylindrical_r - L1*sin(theta1))/L2));
Problem! There isanoffsetthatneedstobe takenintoaccount. See the figure below.
Figure 10: Offset Analysis of Robot
Figure 10 shows that when
theta_2 equalszero,the offset of
about 68.840 mmextendsonly in
the L direction. Since the offsetis
strictly dependent on theta_2,
you will see a modified equation
for L including another cosine of
theta_2 with some coefficient in
front. Furthermore, when
theta_2 equals -90 degrees, the
offset is applied strictly in the R
direction. Therefore, the R
equationwill have an added sine
function for theta_2 with a
coefficientequal in magnitude to
the total offset displacement.
(5)
(6)
(7)

14. 13
The modifiedequationsare:
𝒍 = 𝑳 𝟎 + 𝑳 𝟏 ∗ 𝐜𝐨𝐬( 𝜽 𝟏) − 𝑳 𝟐 ∗ 𝐬𝐢 𝐧( 𝜽 𝟐 + 𝜽 𝟏)+ 𝑪 𝟎 ∗ 𝒄𝒐𝒔(𝜽 𝟐 + 𝜽 𝟏)
𝒓 = 𝑳 𝟏 ∗ 𝐚𝐛𝐬[𝐬𝐢𝐧( 𝜽 𝟏)] + 𝑳 𝟐 ∗ 𝐜𝐨 𝐬( 𝜽 𝟐 + 𝜽 𝟏)+ 𝑪 𝟎 ∗ 𝒂𝒃𝒔[𝒔𝒊𝒏( 𝜽 𝟐 + 𝜽 𝟏)]
𝜶 = 𝜽 𝟎
C_0 isthe offsetconstantandisequal to68.840 mm. We will definethisinourMatLab computer
program as well asthe three equationsrewrittenintermsof theta_0,theta_1and theta_2 as follows.
% Define Robot Variables
C0 = 69; % Offset in units mm
% Convert to Robot
theta0 = cylindrical_alpha;
theta1 = real(acos((cylindrical_l+L2*sin(theta2+theta1)-L0-
C0*cos(theta2))/L1));
theta2 = real(acos((cylindrical_r-L1*abs(sin(theta1))-
C0*abs(sin(theta2)))/L2));
Withthese equations,we have finishedanalyzingourrobot. Withthisknowledge we will proceedto
buildthe algorithmthatwill findthe inverse kinematicsolutions(jointangles)fora givenCartesian
coordinate input. Refertothe code below fora holisticoverviewof the algorithm.
1) First,we ask the userto enterthe desiredcoordinates
% Acquire Cartesian Coordinates
x = input('Enter x-coordinate: ');
y = input('Enter y-coordinate: ');
z = input('Enter z-coordinate: ');
2) Then,we convertto cylindrical usingtheseequations
𝛼 = 𝑡𝑎𝑛−1(
𝑦
𝑥
)
𝑟 = √𝑥2 + 𝑦2
𝑙 = 𝑧
% Convert to Cylindrical
cylindrical_r = sqrt(x^2+y^2);
cylindrical_l = z;
* Whenconvertingthe Cartesiancoordinatestothe cylindrical alpha,itisimportanttomake sure alpha
isin the rightquadrant,otherwise the angle will be off. We dothisbe settingsome conditional if
statements.
% Making Sure Alpha is in the Correct Quadrant
if (x > 0 && y > 0)
cylindrical_alpha = atan(y/x)*(180/pi);
(8)
(9)
(10)
(11)
(12)
(13)

15. 14
end
if (x > 0 && y < 0)
cylindrical_alpha = (atan(y/x))*(180/pi)+360;
end
if (x < 0 && y > 0)
cylindrical_alpha = (atan(y/x))*(180/pi)+180;
end
if (x < 0 && y < 0)
cylindrical_alpha = (atan(y/x)+pi)*(180/pi);
end
3) Now,we establishourrobot’s jointangle movementrestrictionsusingthe following constraints:
−165° ≤ 𝜃0 ≤ 165°
−110° ≤ 𝜃1 ≤ 110°
−90° ≤ 𝜃2 ≤ 70°
% Base Joint Limitations
if (cylindrical_alpha > 165 && cylindrical_alpha < 195)
display('Alpha Out of Range'); break; % Return to top
end
% Robot Length Limitations
if (cylindrical_l < -279 || cylindrical_l > 1035)
display('Length Out of Range'); break;
end
% Robot Radius Limitations
if (cylindrical_r > 745)
display('Radius Out of Range'); break;
end
Figure 11: Robot Length Lower Bound

16. 15
Figure 12: Robot Length Upper Bound
Figure 13: Robot Radius Upper Bound
You can see fromFigures11 – 13 that the robothas height(length) andradial limitations. Thatiswhy
we made sure to define these regionsasinaccessibleinourMatLab computerprogram. In reality, the
robotlimitationsshould be examined moreclosely and defined moreconservatively.
4) Define ourconstants(i.e.the individual lengthsof the linksof ourrobotas well asanyoffsets)
% Define Robot Variables
C0 = 69; % Offset in units mm
L0 = 228; % Base length of robot in mm
L1 = 326; % Link 1 length in mm
L2 = 490; % Link 2 length in mm
theta0 = 0;
theta1 = 0;
theta2 = 0;

17. 16
5) Define ourrobot’sjointanglesintermsof the cylindrical coordinate systemwe chose
% Convert to Robot
theta0 = cylindrical_alpha;
theta1 = real(acos((cylindrical_l+L2*sin(theta2)-L0-C0*cos(theta2))/L1));
theta2 = real(acos((cylindrical_r-L1*abs(sin(theta1))-
C0*abs(sin(theta2)))/L2));
We use the real functionsothat the thetavaluesare strictlyreal and notimaginaryvalues.
6) We double checktoensure the coordinatesare withinthe jointangle limitationranges
% Base Joint Limitations
if (theta0 > 165 && theta0 < 195)
display('Theta0 Out of Range'); break; % Return to top
end
% Second Joint Limitations
if (theta1 > 110 && cylindrical_alpha < 250)
display('Theta1 Out of Range'); break; % Return to top
end
% Base Joint Limitations
if (theta2 > 70 && theta2 < 270)
display('Theta2 Out of Range'); break; % Return to top
end
* If the code passesthispoint,thatmeansthe inputisvalidandwithinrange for the robot.
7) We nowapplythe iterative numerical methodforsolvingsimultaneouslinearequations
Thispart of the code will make aninitial assumptionabouttheta_1then:
 Calculate theta_2
 Calculate the cylindrical coordinatesassociatedwiththe assumedtheta_1andnew theta_2
 Compare the calculatedcylindrical coordinatestothe inputprovidedbythe user
If the calculatedcylindrical coordinatesmeetthe maximumerrorcriteria(alsodefinedinthispartof the
code) the program will stopandrecordthe jointangles. If it doesnotmeetthe maximumerrorcriteria
it will:
 Calculate theta_1basedon the new theta_2
 Calculate the cylindrical coordinatesassociatedwith the new theta_1andoldtheta_2
 Compare the calculatedcylindrical coordinatestothe inputprovidedbythe user
 Repeat
Each time the code repeats,itwill use the newlycalculatedthetavalue tocalculate anothernew theta
value forthe opposite joint. Thismethod of solvinglinearequationsisalsoknownasthe Gauss-Seidel
method. See the code below.

18. 17
i = 1; % Counter
theta1 = 90; % Initial Assumption
error_L = 1;
error_R = 1;
while (error_L || error_R > .05)
if (mod(i,2)==0)
% Calculate the other Joint Angle
theta1 = real(acos((cylindrical_l+L2*sin(theta2)-L0-C0*cos(theta2))/L1));
theta1 = theta1*(180/pi);
% Calculate the New Cylindrical Coordinates
new_l = L0+L1*cos(theta1)-L2*sin(theta2)+C0*cos(theta2);
new_r = L1*abs(sin(theta1))+L2*cos(theta2)+C0*abs(sin(theta2));
% Calculate the Difference
error_L = abs(cylindrical_l - new_l);
error_R = abs(cylindrical_r - new_r);
% Increment Count
i = i+1;
else
% Calculate the Joint Angles
theta2 = real(acos((cylindrical_r-L1*abs(sin(theta1))-
C0*abs(sin(theta2)))/L2));
theta2 = theta2*(180/pi);
% Calculate the New Cylindrical Coordinates
new_l = L0+L1*cos(theta1)-L2*sin(theta2)+C0*cos(theta2);
new_r = L1*abs(sin(theta1))+L2*cos(theta2)+C0*abs(sin(theta2));
% Calculate the Difference
error_L = abs(cylindrical_l - new_l);
error_R = abs(cylindrical_r - new_r);
% Increment Count
i = i+1;
end
% Terminate Loop After 500000 Counts
if i > 500000
break;
end
end
The way thisloopiteratesbetweencalculatingtheta_1andtheta_2 isby usingthe modulusoperator. If
the countervariable i isdivisible bytwo,thenitwill calculate theta_1andproceed,otherwise itwill
calculate theta_2and proceed. Eitherway,the code was setup to iterate throughbothequationsuntil
the maximumnumberof iterationshasbeenreachedorthe maximumerrorcriteriahasbeenmet.

19. 18
Results
UsingEquations8 – 10 (see below),we were able todevelopalinearsystemof equationsrelatingthe
cylindrical coordinatestothe robotjointangle movements.
𝒍 = 𝑳 𝟎 + 𝑳 𝟏 ∗ 𝐜𝐨𝐬( 𝜽 𝟏)− 𝑳 𝟐 ∗ 𝐬𝐢 𝐧( 𝜽 𝟐)+ 𝑪 𝟎 ∗ 𝒄𝒐𝒔(𝜽 𝟐)
𝒓 = 𝑳 𝟏 ∗ 𝐚𝐛𝐬[𝐬𝐢𝐧( 𝜽 𝟏)] + 𝑳 𝟐 ∗ 𝐜𝐨 𝐬( 𝜽 𝟐) + 𝑪 𝟎 ∗ 𝒂𝒃𝒔[𝒔𝒊𝒏( 𝜽 𝟐)]
𝜶 = 𝜽 𝟎
Giventhe Cartesiancoordinatesasaninput,we convert themto cylindrical formatusingthe Cartesian
to Cylindrical conversionformulasorequations11 – 13 (see below).
𝛼 = 𝑡𝑎𝑛−1(
𝑦
𝑥
)
𝑟 = √ 𝑥2 + 𝑦2
𝑙 = 𝑧
Once we obtainthe cylindrical coordinatesof the Cartesianinput,we canapplythese (cylindrical)
coordinatestoour three equations(equations8– 10). Usingthe substitutionmethodwiththese three
equationsallowsustosolve forthe three unknowns.
𝜃1 = 𝑐𝑜𝑠−1[
𝑙 − 𝐿0 + 𝐿2 ∗ cos( 𝜃2) − 𝐶0 ∗ cos( 𝜃2)
𝐿1
]
Substitutingequation14intoequation 9 givesusequation 15:
𝑟 = 𝐿1 ∗ abs[sin (𝑐𝑜𝑠−1[
𝑙−𝐿0+𝐿2∗cos( 𝜃2)−𝐶0∗cos( 𝜃2)
𝐿1
])] + 𝐿2 ∗ cos( 𝜃2) + 𝐶0 ∗ 𝑎𝑏𝑠[𝑠𝑖𝑛( 𝜃2)]
From equation15 we can solve fortheta_2 thensubstitute theta_2intoequation14whichwill give us
our theta_1 value. Theta_0can thensimplybe settothe cylindrical coordinate alphaandthatwill give
us all three of our unknown robotjointangle values(theta_1,theta_2and theta_0).
Conclusion
We setout to uncoverthe inverse kinematicsolutionof a6 DOF robotfor the first3 linksandjoints. By
choosinga cylindrical coordinatesystem,we foundthatitwasmuch easiertoanalyze the robotwith
respectto thiscoordinate system. Afteranalyzingthe robot,itsphysical dimensions,limitationsandthe
relationshipsbetweenthe jointangle movementsandthe final positionof the end-effector,we were
able to come up withan algorithm. Switchingtoa cylindrical coordinatesystemallowedustoreduce
the numberof unknownstotwo forthe particularproblem. Relatingthe jointanglestothe final
positionof the end-effectorthenallowedustocome upwithtwo equationsthatcouldbe usedtogether
to solve forthe tworemainingunknownjointangles(theta_1andtheta_2).
(14)
(15)