Manipulator Kinematics

UFMF5P-20-3: ROBOTS MECHANICS

Coursework 1: Manipulator Kinematics

Queron Williams - 10031755

November 14, 2013

1. INTRODUCTION

This document outlines the methods used to derive both the forward kinematics and inverse kinematics for a 5Degrees Of Freedom Lynxmotion robot arm. From this we will then create a Matlab model of the arm. The modelwill allow us to perform tasks such as plotting the workspace and planning movement sequences. Finally resultsfrom the model will be tested on the robot.

2. FORWARD KINEMATICS

Forward kinematics enable us to work out the final position of the end effector relative to the base link of the robot.To do this we must first know the relation between each link of the robot arm.

2.1. KINEMATIC STRUCTURE OF LYNXMOTION 5DOF ARM

The Lynxmotion arm has 5 Degrees Of Freedom. The configuration of the joints in the arm are shown below.The robot consists of 6 links labelled G0 −G6. These Links are connected through joints j1 − j5. For each pair ofsuccessive joints there is a common perpendicular distance l1 − l5.

Figure 2.1: Joint and Link structure of Lynxmotion robot arm

10031755 Queron Williams 1

Each of these joint then has axes added. The z-axis is chosen to lie along the joint axis of each joint. The x-axis isperpendicular to the z-axis of the current joint and the z-axis of the next joint. The y-axis is chosen to completethe right hand coordinate frame for each joint.

Figure 2.2: Axis structure of Lynxmotion robot arm

Some of the links (l5) in the diagram actually have a link length of zero on the real robot however they have beenshow for clarity.

2.2. DENAVIT-HARTENBERG PARAMETERS

Denavit-Hartenberg (DH) parameters are used to describe the translation between each set of axis in the diagram.there are two types of DH notation, these are called Proximal and Distal. For this example we will be using theProximal method. The derived DH parameters for the Lynxmotion arm are as follows:

Link ai−1 αi−1 θi di

1 0 0 θ1 l1

2 0 90 θ2 03 l2 0 θ3 04 l3 0 θ4 05 0 -90 θ5 l4

E 0 0 0 l5

2.3. TRANSFORMATION MATRICES

Transformation matrices are used to describe the transformation between 2 coordinate frames in a robot. Theyare made up of both a rotation and translation matrix.

T =

R T

0 0 0 1

(2.1)

There is a transformation matrix that describes any possible transformation when using the proximal method.

i−1Ti =

cosθi −si nθi 0 ai−1

si nθi cosαi−1 cosθi cosαi−1 −si nαi−1 −si nαi−1di

si nθi si nαi−1 cosθi si nαi−1 cosαi−1 cosαi−1di

0 0 0 1

(2.2)


This can be used to create a transformation matrix for each joint or the robot arm. This is accomplished by sub-stituting values from each line of the DH table into the above matrix. The transformation matrix for each separatejoint is shown below.

0T1 =

cosθ1 −si nθ1 0 0si nθ1 cosθ1 0 0

0 0 1 l1

0 0 0 1

(2.3)

1T2 =

cosθ2 −si nθ2 0 0

0 0 −1 0si nθ2 cosθ2 0 0

0 0 0 1

(2.4)

2T3 =

cosθ3 −si nθ3 0 l2

si nθ3 cosθ3 0 00 0 1 00 0 0 1

(2.5)

3T4 =

cosθ4 −si nθ4 0 l3

si nθ4 cosθ4 0 00 0 1 00 0 0 1

(2.6)

4T5 =

cosθ5 −si nθ5 0 0

0 0 1 l4

−si nθ5 −cosθ5 0 00 0 0 1

(2.7)

5TE =

1 0 0 00 1 0 00 0 1 l5

0 0 0 1

(2.8)

Once we have found each links transformation matrix we can multiply them together to find a complete transfor-mation matrix from the base to the end effector.

0TE =0 T 11 T 2

2 T 33 T 4

4 T 55 TE (2.9)

0TE =

c234 ∗ c1 ∗ c5 − s1 ∗ s5 −c5 ∗ s1 − c234 ∗ c1 ∗ s5 −s234 ∗ c1 c1 ∗ (L3∗ c23 +L2∗ c2 −L4∗ s234 −L5∗ s234)c1 ∗ s5 + c234 ∗ c5 ∗ s1 c1 ∗ c5 − c234 ∗ s1 ∗ s5 −s234 ∗ s1 s1 ∗ (L3∗ c23 +L2∗ c2 −L4∗ s234 −L5∗ s234)

s234 ∗ c5 −s234 ∗ s5 c234 L1+L3∗ s23 +L2∗ s2 +L4∗ c234 +L5∗ c234

0 0 0 1

(2.10)

Where:ci = cosθi

si = si nθi

c23 = cos(θ2 +θ3)s23 = si n(θ2 +θ3)c234 = cos(θ2 +θ3 +θ4)s234 = si n(θ2 +θ3 +θ4)


2.4. MODELLING IN MATLAB

To model the robot arm in Matlab we are using the previously calculated transformation matrix to find the positionof each joint then drawing a lime between each joint. For all simulations I will be using link lengths measured fromthe actual robot. These link lengths are: L1 = 6.9cm, L2 = 9.4cm, L3 = 10.8cm, L4 = 8.5cm, L5 = 0cm.

Figure 2.3: MATLAB model of robot arm with all joints set to 0 degrees

2.5. TESTING FORWARD KINEMATICS

To test that this model works for many different sets of given theta values I have repeated this with the robot in 3different Cartesian positions as shown below.

Figure 2.4: MATLAB model angles: θ1 = 0, θ2 = 70, θ3 =−20, θ4 =−90, θ5 = 0,


Figure 2.5: MATLAB model angles: θ1 =−45, θ2 = 50, θ3 =−40, θ4 =−110, θ5 = 0,

Figure 2.6: MATLAB model with angles: θ1 =−90, θ2 = 30, θ3 =−60, θ4 =−130, θ5 = 0,

3. WORKSPACE ANALYSIS

A robots workspace describes all possible positions that the robot can reach with its end effector. In order toaccurately plot the workspace the range of possible angles for each joint were measured from the robot. Thesewere as follows : θ1 = -90 to 90 , θ2 = 0 to 135 , θ3 = 0 to -135 , θ4 = 0 to -180. θ5 has not been changed as its angledoes not affect the final position of the end effector.

3.1. PLOTTING THE WORKSPACE

The Forward kinematic model we have created for the Lynxmotion robot arm can be used to easily plot its workspace.This is done by iterating each joint through its range of motion. If we do this for all joints we will get every pos-sible configuration the arm can be in. For each set of joint angles we use the forward kinematics to calculate the


Cartesian position of the end effector. We can then graph the output of the Cartesian points to show which areascan be reached by the robot.

3.2. RESULTS

Whilst plotting the workspace I have varied the colours of the points based on the angle of each of the joints θ2−θ4.The red channel represents the value of θ2 , green shows θ3 and blue shows θ4. This helps give some indication ofthe angles requited to achieve any given target position.

Figure 3.1: Cross-section of workspace across the XZ plane

The red towards the bottom of the workspace shows that θ2 (shoulder) must be further forward for the arm toreach below Zero targets. The green shows that θ3 (elbow) must be outstretched to reach the outer edges of theworkspace and the area above/behind. The blue shows that θ4 (wrist) must be bent downwards in order to reachareas closer to the center of the workspace.In reality the robots workspace would be slightly more restricted than the plot shows due to physical constraintsof the robot. The Lynxmotion arm would not be able to reach below Z=0 as it is mounted to a flat surface.


Figure 3.2: Side view of the workspace Figure 3.3: Top view of the workspace

Figure 3.4: Back view of the workspace Figure 3.5: Isometric view of the workspace

4. INVERSE KINEMATICS

Often in robotics tasks it is useful to be able to give a robot arm a Cartesian target position and calculate the θvalues required to reach the target. In order to achieve this we must be able to solve the Inverse Kinematics for therobot.

4.1. DERIVING THE INVERSE KINEMATICS

The easiest way to solve inverse kinematics is using the geometric method. This involves breaking the prob-lem down into multiple 2D problems and solving each separately. For the Lynxmotion arm we will seperate thesoloution into two seperate problems. The first problem will be to solve θ1 as viewed from above, only consideringthe XY plane. The second problem is to solve θ2, θ3 and θ4. This is done by considering the arm as a 2D problemin a new XZ plane, where X is the hypotenuse formed by the target position in the previous XY plane. To solve theIK we will be given a target x, y, z, pitch and roll for the end effector.


4.1.1. FINDING θ1

X

Y

1/2

3

4

5

5X

5Y

4X

4Y

θ1

Figure 4.1: Robot arm as viewed in XY plane

θ1 can be easily solved using the trigonometric function atan 2 as shown below.

θ1 = at an2(5y ,5x ) (4.1)

4.1.2. FINDING θ3

X

Z

2

3

45

4Z

θ2

4X3X

3Z

θ3

θ2

Figure 4.2: Robot arm as viewed in XZ plane

To find θ2 and θ3 we need to first know the position of joint 4 on the XZ plane. This means that we need to calculate


joint 4’s position (X4 and Z4). We can do this as we know the target position, link 4’s length and the target pitch (we will call ϕ)

4x = 5x − l4cϕ (4.2)

4z = 5z − l4sϕ (4.3)

To find the X value we need only sum the lengths of the adjacent sides of the two right-angled triangles formed byl2&θ2 and l3&θ3. For Z it is the same but using the opposite sides.

4x = l3c23 + l2c2 (4.4)

4z = l3s23 + l2s2 (4.5)

Where ci = cosθi , si = si nθi , c23 = cos(θ2 + θ3), s23 = si n(θ2 + θ3). The soloution to θ3 can be computed fromsummation of squaring both equations 4.4 and 4.5

42x = l 2

3 c223 + l 2

2 c22 +2l3l2c23c2 (4.6)

42z = l 2

3 s223 + l 2

2 s22 +2l3l2s23s2 (4.7)

42x +42

z = l 23 (c2

23 + s223)+ l 2

2 (c22 + s2

2)+2l3l2(c23c2 + s23s2) (4.8)

Because c2θi + s2θi = 1, equation 4.8 can be rearanged and simplified as follows:

42x +42

z = l 23 + l 2

2 +2l3l2(c23c2 + s23s2) (4.9)

42x +42

z = l 23 + l 2

2 +2l3l2(c(θ2 +θ3)c2 + s(θ2 +θ3)s2) (4.10)

42x +42

z = l 23 + l 2

2 +2l3l2(1/2(c(2θ2 +θ3)+ c3)+1/2(c3 − c(2θ2 +θ3))) (4.11)

42x +42

z = l 23 + l 2

2 +2l3l2c3 (4.12)

Rearranged for c(θ3):

cθ3 =−l 2

2 − l 23 +42

x +42z

2l2l3(4.13)

The acos() function can cause inaccuracies at small values of θ, to get around this problem we will use atan2() thisrequires sθ3 and cθ3 but sine we know c2θi + s2θi = 1 we can simply rearrange to get sθ3.

sθ3 =±√√√√1−

(−l 22 − l 2

3 +42x +42

z

2l2l3

)2

(4.14)

The final atan2() solution can now be created, note that the ± in the formula will allow us to get both possiblesolutions for this joint.

θ3 = At an2

±√√√√1−

(−l 22 − l 2

3 +42x +42

z

2l2l3

)2

,−l 2

2 − l 23 +42

x +42z

2l2l3

(4.15)


4.1.3. FINDING θ2

Now that we have θ3, we can use equation 4.4 and 4.5 to calculate θ2. To do this we multiply equation 4.4 by cθ2

and the equation 4.5 by sθ2

c24x = l3c(θ2 +θ3)c2 + l2c22 (4.16)

s24z = l3s(θ2 +θ3)s2 + l2s22 (4.17)

c24x + s24z = l3(c(θ2 +θ3)c2 + s(θ2 +θ3)s2)+ l2(c22 + s2

2) (4.18)

c24x + s24z = l3(1/2(c(2θ2 +θ3)+ c3)+1/2(c3 − c(2θ2 +θ3)))+ l2(c22 + s2

2) (4.19)

c24x + s24z = l3c3 + l2(c22 + s2

2) (4.20)

c24x + s24z = l3c3 + l2 (4.21)

We repeat this process but this time multiply equation 4.4 by −sθ2 and the equation 4.5 by cθ2

−s24x =−l3c(θ2 +θ3)s2 − l2c2s2 (4.22)

c24z = l3s(θ2 +θ3)c2 + l2s2c2 (4.23)

−s24x + c24z = l3s(θ2 +θ3)c2 − l3c(θ2 +θ3)s2 (4.24)

−s24x + c24z = l3s3 (4.25)

Now we multiply both sides of equation 4.21 by −s(θ2), and both sides of equation 4.25 by c(θ2): before addingthe resultant equations:

c242x + s24z 4x = 4x (l3c3 + l2) (4.26)

−s24x 4z + c242z = l3s34z (4.27)

c2(42x +42

z ) = 4x (l3c3 + l2)+ l3s34z (4.28)

This is then rearranged for c(θ2):

cθ2 = 4x (l3c3 + l2)+ l3s34z

42x +42

z(4.29)

We know c2θi + s2θi = 1 so we can rearrange to get s(θ2):

sθ2 =±√

1−(

4x (l3c3 + l2)+ l3s34z

42x +42

z

)2

(4.30)

This can be used to give us the final atan2 solution for θ2, as before the ± in the formula will allow us to get bothpossible solutions for this joint.

θ2 = At an2

(±

√1−

(4x (l3c3 + l2)+ l3s34z

42x +42

z

)2

,4x (l3c3 + l2)+ l3s34z

42x +42

z

)(4.31)


4.1.4. FINDING θ3

Now that we have θ2 and θ3 we can easily calculate θ4 using ϕ:

θ4 =ϕ− (θ2 +θ3)−90 (4.32)

This will have to be repeated using the alternate θ2 and θ3 values to get both solutions for θ4.

4.2. TESTING THE INVERSE KINEMATICS

To test my inverse kinematics I have used the end effector target positions I acquired during my forward kinematictests. I have then solved the inverse kinematics for these targets but specifying a pitch of 45 degrees with respectto ground. θ5 has been ignored as it does not affect the position of the end effector, only orientation.For the first set of theta angles (θ1 = 0, θ2 = 70, θ3 =−20, θ4 =−90) Matlab produces this target:

End efector position: x= 15.62 y= 0.00 z= 30.52

And with a target pitch of 45 degrees it gives these solutions:

Solution 1: 1= 0.00 2= 54.12 3= 13.56 4= -112.68

Solution 2: 1= 0.00 2= 68.63 3= -13.56 4= -100.06

>>

I then plotted these 2 solutions to check them against the original angles and make sure the target position andpitch were right.

Figure 4.3: Test 1 input Figure 4.4: Test 1 solution 1 Figure 4.5: Test 1 solution 2

This process was repeated for a second set of theta angles and a pitch of 45 degrees. Matlab produced:

input angles: 1= -22.50 2= 60.00 3= -30.00 4= -100.00

End efector position: x= 20.36 y= -8.43 z= 23.35

Solution 1: 1= -22.50 2= 12.93 3= 37.58 4= -95.51

Solution 2: 1= -22.50 2= 53.21 3= -37.58 4= -60.63

This was then plotted against the original.



The process was repeated once more for a final set of theta angles and a pitch of 0 degrees. Matlab produced:

input angles: 1= -45.00 2= 50.00 3= -40.00 4= -110.00

End efector position: x= 17.71 y= -17.71 z= 14.50

Solution 1: 1= -45.00 2= 2.95 3= 51.41 4= -144.37

Solution 2: 1= -45.00 2= 52.29 3= -51.41 4= -90.87

This was then plotted against the original.


5. REAL WORLD APPLICATION

In order to test the model I will use it to perform a task with the Lynxmotion robot arm. The Lynxmotion robotarm uses a serial servo controller. I have written the following functions to make real time control of the arm easy:

• updateJoint(); allows me to open the serial port and sends values directly to a servo. This function also dealswith mapping required joint angles to the correct pulse width for each joint. this allows for the robot to beupdated in real time as the program runs.

• solveIK(); this function is provided with the target x/y/z/pitch/roll and peforms all of the IK calculations,returning the required θ vales for each joint to reach the target.

• UpdateArm(); this function gives a set of target positions to solveIK() then passes each of the returned θ

values out to the correct joint using updateJoint(). It then waits for the movement to be completed beforereturning and allowing the next command to run.

The use of these functions allows for easy task planning with very little code. To create a task you need to spec-ify the starting variables for x/y/z/pitch. Each move only requires one variable to be updated, followed by theUpdateArm() command. The example below shows the code to draw a 10cm*10cm square in the XZ plane:

x=10;

y=0;

z=10;

pitch=0;

gripper=0; %gripper is open

time=2; %time for each move to take (2 seconds)

UpdateArm(x,y,z,pitch,gripper,time); %move to start point

x=20;

UpdateArm(x,y,z,pitch,gripper,time); %move along edge 1

z=20;


x=10;



z=10;


Whilst performing a task matlab produces a log file showing the target position and joint angles at each time thearm is updated. This can be used for debugging or manually checking output from the IK.

x= 10.00 y= 0.00 z= 10.00 pitch= 0.00 1=0.00 2=168.87 3=-162.03 4=-96.84

x= 20.00 y= 0.00 z= 10.00 pitch= 0.00 1=0.00 2=74.61 3=-108.12 4=-56.49

x= 20.00 y= 0.00 z= 20.00 pitch= 0.00 1=0.00 2=81.48 3=-60.86 4=-110.62

x= 10.00 y= 0.00 z= 20.00 pitch= 0.00 1=0.00 2=137.50 3=-98.82 4=-128.68

x= 10.00 y= 0.00 z= 10.00 pitch= 0.00 1=0.00 2=168.87 3=-162.03 4=-96.84

By putting the angles from each line of the log file into the forward kinematics it can be seen in figure 5.1-5.4 thatthe arm accurately draws the square.

Figure 5.1: Matlab, Frame1 of sequence Figure 5.2: Matlab, Frame2 of sequence

Figure 5.3: Matlab, Frame3 of sequencee Figure 5.4: Matlab, Frame4 of sequence

This is also matched by the physical arm as shown in figure 5.5-5.8A more complex pick and place task was also performed. This involved picking up a pen and drawing a circle. alog file for this task is included in the appendix, a video can be seen at http://youtu.be/rd4mS6ZGti4 . This taskwas performed well however it did not produce a perfect circle. I found that this was due to a hardware limitationof the robot, there is too much friction in the base rotation servo so it cannot keep θ1 accurate.


Figure 5.5: Video, Frame1 of sequence Figure 5.6: Video, Frame2 of sequence

Figure 5.7: Video, Frame3 of sequencee Figure 5.8: Video, Frame4 of sequence

6. CONCLUSION

In conclusion I have shown how to derive the forward kinematics for the Lynxmotion robot arm using the Prox-imal method. If i had more time i would like to look into using the Distal method as it seams that this is a morestandardised method. I have also produced a model of the robot arm and used this to test and verify that theforward kinematics are correct.The forward kinematics have then been used to accurately plot the robots workspace, showing what areas arewithin reach and also indicating required joint angels to reach the target.I have also shown how to derive the inverse kinematics using the geometric method. The geometric method issimpler for smaller robot arms but can become difficult when working with larger robot arms. If I was to repeatthis task on a robot arm with more degrees of freedom I would try and use the algebraic method. The Inversekinematics were then modelled and tested in a variety of positions using the Forwards kinematics to check eachset of results, including multiple solutions.Finally i have used this model to plan a task. The output of each stage of the task have been checked using theforward kinematics. The task has then been executed on the physical robot and its output is shown to match withwhat was expected from the forward kinematics. The most difficult problem whilst planning the task was dealingwith the hardware design of the robot arm. Often things would work in the model but not on the physical arm.this was because the robot arm often did not have the range of motion needed or would collide with its self beforereaching its target.Overall I feel that I have produced a stable and reliable model of the robot arm. Whilst completing this task i havegained a good understanding of both forward / inverse kinematics and why these tools can be useful for designingrobotic systems.


A.MATLAB CODE - MAIN.M

1 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%2 %%%%%%%%%%% Queron Williams %%%%%%%%%%%3 %%%%%%%%%% Robotic Mechanics %%%%%%%%%%4 %%%%%%%%%%%% October 2013 %%%%%%%%%%%%%5 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%6 %% f i r s t c lear everything7 cl c8 clear a l l9 close a l l

10

11 %% s t a t i c varables and globals12 global rad2deg deg2rad L1 L2 L3 L4 L513 global IKtheta1 IKtheta2a IKtheta3a IKtheta4a IKtheta2b IKtheta3b IKtheta4b14 deg2rad = pi /180 ; % multiplying by t h i s w i l l convert degrees to rads15 rad2deg = 180/ pi ; % multiplying by t h i s w i l l convert degrees to rads16

17 %% Program options18 PlotAxis = 0 ;19 PlotLinks = 1 ;20 PlotWorkspace = 0 ;21 InverseKinematics = 0 ;22 executePath = 1 ;23

24 %% variables such as l i n k length and theta ’ s25 %l i n k lengths26 L1 = 6 . 9 ;27 L2 = 9 . 4 ;28 L3 = 1 0 . 8 ;29 L4 = 8 . 5 ;30 L5 = 0 ;31 % thetas32 theta1 = 00 * deg2rad ; %−90 to 9033 theta2 = 137.50 * deg2rad ; % around −10 to 9034 theta3 = −98.82 * deg2rad ;35 theta4 = −128.68 * deg2rad ;36 theta5 = 0 * deg2rad ;37

38 %% DH table39 %l i n k ai−1 alphai−1 d theta40 DH1 = [ 0 , 0 , L1 , theta1 ] ;41 DH2 = [ 0 , ( pi /2) , 0 , theta2 ] ;42 DH3 = [ L2 , 0 , 0 , theta3 ] ;43 DH4 = [ L3 , 0 , 0 , theta4 ] ;44 DH5 = [ 0 , −(pi /2) , L4 , theta5 ] ;45 DHE = [ 0 , 0 , L5 , 0 ] ;46

47 %% transformation matrix ’ s48 % calculate the transformation matrix for each l i n k49 BL = get_dh_matrix ( [ 0 , 0 , 0 , 0 ] ) ; %Base Link


50 T01 = get_dh_matrix (DH1) ;51 T12 = get_dh_matrix (DH2) ;52 T23 = get_dh_matrix (DH3) ;53 T34 = get_dh_matrix (DH4) ;54 T45 = get_dh_matrix (DH5) ;55 T5E = get_dh_matrix (DHE) ;56

57 % calculate the t o t a l transformation matrix58 T02 = T01 * T12 ;59 T03 = T02 * T23 ;60 T04 = T03 * T34 ;61 T05 = T04 * T45 ;62 T0E = T05 * T5E ;63

64 %%65 %%66 %%67 hold on68 grid on ;69 ylabel ( ’Y(cm) ’ ) , x label ( ’X(cm) ’ ) , z label ( ’Z(cm) ’ )70 daspect ( [ 1 1 1 ] ) %keep aspect r a t i o the same71 %view ([−1 ,−1 ,1])%isometric72 view ([0 , −1 ,0])%side73 %view ([ −1 ,0 ,0])%back74 %axis auto75 axis ([−10 25 −0 25 −5 25]) ; %manualy set axis76 %axis ([−30 30 −30 30 −15 40]) ; %manualy set axis77

78 i f PlotLinks >= 179 OOx = [ 0 ; T01 ( 1 , 4 ) ; T02 ( 1 , 4 ) ; T03 ( 1 , 4 ) ; T04 ( 1 , 4 ) ; T05 ( 1 , 4 ) ; T0E( 1 , 4 ) ] ;80 OOy = [ 0 ; T01 ( 2 , 4 ) ; T02 ( 2 , 4 ) ; T03 ( 2 , 4 ) ; T04 ( 2 , 4 ) ; T05 ( 2 , 4 ) ; T0E( 2 , 4 ) ] ;81 OOz = [ 0 ; T01 ( 3 , 4 ) ; T02 ( 3 , 4 ) ; T03 ( 3 , 4 ) ; T04 ( 3 , 4 ) ; T05 ( 3 , 4 ) ; T0E( 3 , 4 ) ] ;82 plot3 (OOx,OOy,OOz, ’b ’ , ’ LineWidth ’ , PlotLinks ) % t h i s plots the p vector83 t e x t (BL( 1 , 4 ) ,BL( 2 , 4 ) ,BL( 3 , 4 ) , ’ L0 ’ ) ;84 t e x t ( T01 ( 1 , 4 ) , T01 ( 2 , 4 ) , T01 ( 3 , 4 ) , ’ L1 ’ ) ;85 t e x t ( T02 ( 1 , 4 ) , T02 ( 2 , 4 ) , T02 ( 3 , 4 ) , ’ L2 ’ ) ;86 t e x t ( T03 ( 1 , 4 ) , T03 ( 2 , 4 ) , T03 ( 3 , 4 ) , ’ L3 ’ ) ;87 t e x t ( T04 ( 1 , 4 ) , T04 ( 2 , 4 ) , T04 ( 3 , 4 ) , ’ L4 ’ ) ;88 t e x t ( T05 ( 1 , 4 ) , T05 ( 2 , 4 ) , T05 ( 3 , 4 ) , ’ L5 ’ ) ;89 t e x t (T0E( 1 , 4 ) ,T0E( 2 , 4 ) ,T0E ( 3 , 4 ) , ’LE ’ )90 end91

92 i f PlotAxis == 193 t r p l o t (BL , ’ frame ’ , ’ 0 ’ , ’ color ’ , [ 1 , 0 , 0 ] ) ;94 t r p l o t ( T01 , ’ frame ’ , ’ 1 ’ , ’ color ’ , [ 0 . 6 , 0 , 0 . 3 ] ) ;95 t r p l o t ( T02 , ’ frame ’ , ’ 2 ’ , ’ color ’ , [ 0 . 3 , 0 , 0 . 6 ] ) ;96 t r p l o t ( T03 , ’ frame ’ , ’ 3 ’ , ’ color ’ , [ 0 , 0 , 1 ] ) ;97 t r p l o t ( T04 , ’ frame ’ , ’ 4 ’ , ’ color ’ , [ 0 , 0 . 3 , 0 . 6 ] ) ;98 t r p l o t ( T05 , ’ frame ’ , ’ 5 ’ , ’ color ’ , [ 0 , 0 . 6 , 0 . 3 ] ) ;99 t r p l o t (T0E , ’ frame ’ , ’ e ’ , ’ color ’ , [ 0 , 1 , 0 ] ) ;

100 end101


102 i f PlotWorkspace == 1103 X = [ ] ;104 Y = [ ] ;105 Z = [ ] ;106 C = [ ] , [ ] ;107 for i =−90:2:90 ,108 f p r i n t f ( ’ at i t e r a t i o n : %d \n ’ , i ) %progress report 4 workspace plot109 for j =0:10:135 ,110 for k=−135:10:0 ,111 for l =−180:5:0 ,112 theta1= i * deg2rad ;113 theta2= j * deg2rad ;114 theta3=k* deg2rad ;115 theta4= l * deg2rad ;116

117 DH1 = [ 0 , 0 , L1 , theta1 ] ;118 DH2 = [ 0 , ( pi /2) , 0 , theta2 ] ;119 DH3 = [ L2 , 0 , 0 , theta3 ] ;120 DH4 = [ L3 , 0 , 0 , theta4 ] ;121 DH5 = [ 0 , −(pi /2) , L4 , theta5 ] ;122 DHE = [ 0 , 0 , L5 , 0 ] ;123

124 T01 = get_dh_matrix (DH1) ;125 T02 = T01 * get_dh_matrix (DH2) ;126 T03 = T02 * get_dh_matrix (DH3) ;127 T04 = T03 * get_dh_matrix (DH4) ;128 T05 = T04 * get_dh_matrix (DH5) ;129 T0E = T05 * get_dh_matrix (DHE) ;130

131 C(end+1 ,1) = 1−( j /135) ; %set red value from j o i n t 2132 C(end , 2 ) = ( ( k+135) /135) ;%set green value from j o i n t 3133 C(end , 3 ) = ( l /−180) ; %set blue value from j o i n t 4134 X(end+1) = T0E( 1 , 4 ) ;135 Y(end+1) = T0E( 2 , 4 ) ;136 Z(end+1) = T0E( 3 , 4 ) ;137

138 end139 end140 end141 end142 s ca t te r 3 (X , Y , Z, 2 0 ,C, ’ f i l l e d ’ ) ;143 hold on144

145 end146

147 %hold o f f148

149 i f InverseKinematics == 1150 %phi = 91+theta2+theta3+theta4 ; %comment in for same phi as input151 phi = 45; %comment in to manualy set wrwtg angles152


153 [ IKtheta1 , IKtheta2a , IKtheta3a , IKtheta4a , IKtheta2b , IKtheta3b , IKtheta4b ] = solveIK (T0E( 1 , 4 ) ,T0E( 2 , 4 ) ,T0E( 3 , 4 ) , phi ) ;

154 f p r i n t f ( ’ input angles : \ t 1= %.2 f \ t 2= %.2 f \ t 3= %.2 f \ t 4= %.2 f \n ’ , theta1 *rad2deg , theta2 * rad2deg , theta3 * rad2deg , theta4 * rad2deg )

155 f p r i n t f ( ’End efector position : \ t x= %.2 f \ t y= %.2 f \ t z= %.2 f \n ’ , T0E( 1 , 4 ) , T0E( 2 , 4 ) , T0E( 3 , 4 ) )

156 f p r i n t f ( ’ Solution 1 : \ t 1= %.2 f \ t 2= %.2 f \ t 3= %.2 f \ t 4= %.2 f \n ’ , IKtheta1 ,IKtheta2a , IKtheta3a , IKtheta4a )

157 f p r i n t f ( ’ Solution 2 : \ t 1= %.2 f \ t 2= %.2 f \ t 3= %.2 f \ t 4= %.2 f \n ’ , IKtheta1 ,IKtheta2b , IKtheta3b , IKtheta4b )

158

159 end160

161 i f executePath == 2 %simple square path162 x =10;163 y =0;164 z =10;165 pitch =0;166 gripper =0; %gripper i s open167 time =2; %time for each move to take (2 seconds )168 UpdateArm( x , y , z , pitch , gripper , time ) ; %move to s t a r t point169

170 x =20;171 UpdateArm( x , y , z , pitch , gripper , time ) ; %move along edge 1172

173 z =20;174 UpdateArm( x , y , z , pitch , gripper , time ) ; %move along edge 2175

176 x =10;177 UpdateArm( x , y , z , pitch , gripper , time ) ; %move along edge 3178

179 z =10;180 UpdateArm( x , y , z , pitch , gripper , time ) ; %move along edge 4181

182 end183

184 i f executePath == 1 %pick and place task path185 pathX = 11;186 pathY = 0 ;187 pathZ = 5 ;188 pathWRWTG = −90;189 pathGripper = 0 ;190 UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 2 ) ;191

192 pathZ = 2 ;193 UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 2 ) ;194

195 pathGripper = 45;196 UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 2 ) ;197

198 pathZ = 8 ;199 UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 2 ) ;


200

201 pathX = 15;202 UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 2 ) ;203

204 for i =−90:10:0 ,205 pathWRWTG = i ;206 UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 0 . 4 ) ;207 end208


212 pathY = 5 ;213 UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 2 ) ;214


218

219 for i =0:10:360 ,220 pathX = 21 + (5* sin ( i * deg2rad ) ) ;221 pathY = (5* cos ( i * deg2rad ) ) ;222 UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 0 . 4 ) ;223 end224 pathY = 0 ;225 pathZ = 8 ;226 UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 2 ) ;227


231 for i =0:−10:−90 ,232 pathWRWTG = i ;233 UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 0 . 4 ) ;234 end235



242 pathGripper = 0 ;243 UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 2 ) ;244


248 end


B.MATLAB CODE - SOLVEIK.M

1 function [ IKtheta1 , IKtheta2a , IKtheta3a , IKtheta4a , IKtheta2b , IKtheta3b , IKtheta4b ] =solveIK ( xTarget , yTarget , zTarget , wrwtgTarget )

2 global rad2deg deg2rad L1 L2 L3 L4 IKtheta1 IKtheta2a IKtheta3a IKtheta4a IKtheta2bIKtheta3b IKtheta4b

3

4 IKtheta1 = atan2 ( yTarget , xTarget ) * rad2deg ;5 %wrwtg = 90+theta2+theta3+theta4 ;6 wrwtg = wrwtgTarget ;7 z = zTarget−(L4* sin ( wrwtg* deg2rad ) )−L1 ;8 XYhyp = sqrt ( ( xTarget ^2) +( yTarget ^2) )−(L4* cos ( wrwtg* deg2rad ) ) ;9

10 IKcostheta3 = ( ( XYhyp^2) +( z ^2)−(L2^2)−(L3^2) ) /(2* L2*L3 ) ;11

12 IKtheta3a = atan2 (+ sqrt (1−( IKcostheta3 ^2) ) , IKcostheta3 ) * rad2deg ;13 IKtheta3b = atan2(−sqrt (1−( IKcostheta3 ^2) ) , IKcostheta3 ) * rad2deg ;14

15 IKcostheta2a = ( ( XYhyp * ( L2+(L3* cos ( IKtheta3a * deg2rad ) ) ) ) +( z *L3* sin ( IKtheta3a * deg2rad) ) ) / ( ( XYhyp^2) +( z ^2) ) ;

16 IKcostheta2b = ( ( XYhyp * ( L2+(L3* cos ( IKtheta3b * deg2rad ) ) ) ) +( z *L3* sin ( IKtheta3b * deg2rad) ) ) / ( ( XYhyp^2) +( z ^2) ) ;

17

18 IKtheta2a = atan2 (+ sqrt (1−( IKcostheta2a ^2) ) , IKcostheta2a ) * rad2deg ;19 IKtheta2b = atan2 (+ sqrt (1−( IKcostheta2b ^2) ) , IKcostheta2b ) * rad2deg ;20

21 IKtheta4a = wrwtg−(IKtheta2a+IKtheta3a ) −90;22 IKtheta4b = wrwtg−(IKtheta2b+IKtheta3b ) −90;23 end


C.MATLAB CODE - UPDATEJOINT.M

1 function updateJoint ( joint , target , SpeedOrTime , SpeedOrTimeAmount )2 p e r s i s t e n t f i r s t ; %check i f t h i s i s the f i r s t use3 p e r s i s t e n t arm ;4 i f isempty ( f i r s t )5 delete ( i n s t r f i n d a l l ) ; %i f f i r s t use clear any previous s e r i a l objects6 arm = s e r i a l ( ’COM13’ , ’ BaudRate ’ , 115200 , ’ Terminator ’ , ’CR/LF ’ ) ;7 fopen (arm) ; %open new s e r i a l port8 f i r s t = 1 ;9 end

10

11 switch j o i n t ; %depending on joint , convert degrees to pulse width12 case 013 t a r g e t = round (map( target ,−90 ,90 ,500 ,2500) ) ; %checked14 case 115 t a r g e t = round (map( target ,0 ,180 ,750 ,2250) ) ; %checked16 case 217 t a r g e t = round (map( target ,−180 ,0 ,2500 ,500) ) ; %checked18 case 319 t a r g e t = round (map( target ,−180 ,0 ,500 ,2500) ) ; %checked20 case 421 t a r g e t = round (map( target ,−90 ,90 ,500 ,2500) ) ; %checked22 case 523 t a r g e t = round (map( target ,−90 ,90 ,500 ,2500) ) ; %checked24 end25 %send pulse width to servo26 f p r i n t f (arm, s p r i n t f ( ’#%d P %d %c %d ’ , joint , target , SpeedOrTime , SpeedOrTimeAmount) )27

28 end

D.MATLAB CODE - UPDATEARM.M

1 function UpdateArm( x , y , z , wrwtg , gripper , time )2 global IKtheta1 IKtheta2a IKtheta3a IKtheta4a IKtheta2b IKtheta3b IKtheta4b ;3 [ IKtheta1 , IKtheta2a , IKtheta3a , IKtheta4a , IKtheta2b , IKtheta3b , IKtheta4b ] = solveIK ( x

, y , z , wrwtg ) ;4 UpdateJoint ( 0 , IKtheta1 , ’ t ’ , ( time *1000) ) ;5 UpdateJoint ( 1 , IKtheta2b , ’ t ’ , ( time *1000) ) ;6 UpdateJoint ( 2 , IKtheta3b , ’ t ’ , ( time *1000) ) ;7 UpdateJoint ( 3 , IKtheta4b , ’ t ’ , ( time *1000) ) ;8 UpdateJoint ( 4 , gripper , ’ t ’ ,500 ) ;9 UpdateJoint ( 5 ,0 , ’ t ’ ,500 ) ;

10 f p r i n t f ( ’ x= %.2 f \ t y= %.2 f \ t z= %.2 f \ t pitch= %.2 f \ t ’ , x , y , z , wrwtg ) ;11 f p r i n t f ( ’ 1= %.2 f \ t 2= %.2 f \ t 3= %.2 f \ t 4= %.2 f \n ’ , IKtheta1 , IKtheta2b , IKtheta3b ,

IKtheta4b ) ;12

13 delay ( time ) ;14 end


E.MATLAB CODE - GET_DH_MATRIX.M

1 function T = get_dh_matrix ( parameters )2 T = [ ( cos ( parameters ( 1 , 4 ) ) ) ,−( sin ( parameters ( 1 , 4 ) ) ) , 0 , ( parameters ( 1 , 1 ) ) ;3 ( sin ( parameters ( 1 , 4 ) ) ) * ( cos ( parameters ( 1 , 2 ) ) ) , ( cos ( parameters ( 1 , 4 ) ) ) * ( cos (

parameters ( 1 , 2 ) ) ) ,−( sin ( parameters ( 1 , 2 ) ) ) ,−( sin ( parameters ( 1 , 2 ) ) ) * (parameters ( 1 , 3 ) ) ;

4 ( sin ( parameters ( 1 , 4 ) ) ) * ( sin ( parameters ( 1 , 2 ) ) ) , ( cos ( parameters ( 1 , 4 ) ) ) * ( sin (parameters ( 1 , 2 ) ) ) , ( cos ( parameters ( 1 , 2 ) ) ) , ( cos ( parameters ( 1 , 2 ) ) ) * ( parameters( 1 , 3 ) ) ;

5 0 , 0 , 0 , 1 ] ;6 end

F.MATLAB CODE - MAP.M

1 function output = map( x , in_min , in_max , out_min , out_max )2 output = ( x − in_min ) * ( out_max − out_min ) / ( in_max − in_min ) + out_min ;3 end

G.MATLAB CODE - DELAY.M

1 function delay ( seconds )2 % function pause the program3 % seconds = delay time in seconds4 t i c ;5 while toc < seconds6 end7 end


H.LOG FILE FOR COMPLEX TASK

x= 11.00 y= 0.00 z= 5.00 pitch= -90.00 1= 0.00 2= 86.56 3= -101.49 4= -165.07

x= 11.00 y= 0.00 z= 2.00 pitch= -90.00 1= 0.00 2= 79.07 3= -110.48 4= -148.58

x= 11.00 y= 0.00 z= 2.00 pitch= -90.00 1= 0.00 2= 79.07 3= -110.48 4= -148.58

x= 11.00 y= 0.00 z= 8.00 pitch= -90.00 1= 0.00 2= 88.77 3= -87.70 4= -181.07

x= 15.00 y= 0.00 z= 8.00 pitch= -90.00 1= 0.00 2= 62.98 3= -56.47 4= -186.52

x= 15.00 y= 0.00 z= 8.00 pitch= -90.00 1= 0.00 2= 62.98 3= -56.47 4= -186.52

x= 15.00 y= 0.00 z= 8.00 pitch= -80.00 1= 0.00 2= 73.09 3= -70.55 4= -172.53

x= 15.00 y= 0.00 z= 8.00 pitch= -70.00 1= 0.00 2= 82.08 3= -83.26 4= -158.82

x= 15.00 y= 0.00 z= 8.00 pitch= -60.00 1= 0.00 2= 90.06 3= -95.04 4= -145.01

x= 15.00 y= 0.00 z= 8.00 pitch= -50.00 1= 0.00 2= 96.88 3= -106.05 4= -130.83

x= 15.00 y= 0.00 z= 8.00 pitch= -40.00 1= 0.00 2= 102.21 3= -116.27 4= -115.94

x= 15.00 y= 0.00 z= 8.00 pitch= -30.00 1= 0.00 2= 105.45 3= -125.54 4= -99.91

x= 15.00 y= 0.00 z= 8.00 pitch= -20.00 1= 0.00 2= 105.66 3= -133.50 4= -82.16

x= 15.00 y= 0.00 z= 8.00 pitch= -10.00 1= 0.00 2= 101.63 3= -139.51 4= -62.12

x= 15.00 y= 0.00 z= 8.00 pitch= 0.00 1= 0.00 2= 92.57 3= -142.71 4= -39.86

x= 21.00 y= 0.00 z= 8.00 pitch= 0.00 1= 0.00 2= 61.83 3= -103.54 4= -48.29

x= 21.00 y= 5.00 z= 8.00 pitch= 0.00 1= 13.39 2= 59.07 3= -99.22 4= -49.85

x= 21.00 y= 5.00 z= 4.00 pitch= 0.00 1= 13.39 2= 40.58 3= -97.16 4= -33.42

x= 21.00 y= 5.00 z= 4.00 pitch= 0.00 1= 13.39 2= 40.58 3= -97.16 4= -33.42

x= 21.87 y= 4.92 z= 4.00 pitch= 0.00 1= 12.69 2= 37.67 3= -90.83 4= -36.84

x= 22.71 y= 4.70 z= 4.00 pitch= 0.00 1= 11.69 2= 34.72 3= -84.56 4= -40.16

x= 23.50 y= 4.33 z= 4.00 pitch= 0.00 1= 10.44 2= 31.83 3= -78.51 4= -43.32

x= 24.21 y= 3.83 z= 4.00 pitch= 0.00 1= 8.99 2= 29.09 3= -72.84 4= -46.24

x= 24.83 y= 3.21 z= 4.00 pitch= 0.00 1= 7.38 2= 26.59 3= -67.75 4= -48.84

x= 25.33 y= 2.50 z= 4.00 pitch= 0.00 1= 5.64 2= 24.46 3= -63.43 4= -51.03

x= 25.70 y= 1.71 z= 4.00 pitch= 0.00 1= 3.81 2= 22.82 3= -60.12 4= -52.70

x= 25.92 y= 0.87 z= 4.00 pitch= 0.00 1= 1.92 2= 21.77 3= -58.03 4= -53.75

x= 26.00 y= 0.00 z= 4.00 pitch= 0.00 1= 0.00 2= 21.41 3= -57.31 4= -54.10

x= 25.92 y= -0.87 z= 4.00 pitch= 0.00 1= -1.92 2= 21.77 3= -58.03 4= -53.75

x= 25.70 y= -1.71 z= 4.00 pitch= 0.00 1= -3.81 2= 22.82 3= -60.12 4= -52.70

x= 25.33 y= -2.50 z= 4.00 pitch= 0.00 1= -5.64 2= 24.46 3= -63.43 4= -51.03

x= 24.83 y= -3.21 z= 4.00 pitch= 0.00 1= -7.38 2= 26.59 3= -67.75 4= -48.84

x= 24.21 y= -3.83 z= 4.00 pitch= 0.00 1= -8.99 2= 29.09 3= -72.84 4= -46.24

x= 23.50 y= -4.33 z= 4.00 pitch= 0.00 1= -10.44 2= 31.83 3= -78.51 4= -43.32

x= 22.71 y= -4.70 z= 4.00 pitch= 0.00 1= -11.69 2= 34.72 3= -84.56 4= -40.16

x= 21.87 y= -4.92 z= 4.00 pitch= 0.00 1= -12.69 2= 37.67 3= -90.83 4= -36.84

x= 21.00 y= -5.00 z= 4.00 pitch= 0.00 1= -13.39 2= 40.58 3= -97.16 4= -33.42

x= 20.13 y= -4.92 z= 4.00 pitch= 0.00 1= -13.74 2= 43.38 3= -103.42 4= -29.96

x= 19.29 y= -4.70 z= 4.00 pitch= 0.00 1= -13.69 2= 46.01 3= -109.47 4= -26.53

x= 18.50 y= -4.33 z= 4.00 pitch= 0.00 1= -13.17 2= 48.37 3= -115.17 4= -23.21

x= 17.79 y= -3.83 z= 4.00 pitch= 0.00 1= -12.15 2= 50.42 3= -120.36 4= -20.06

x= 17.17 y= -3.21 z= 4.00 pitch= 0.00 1= -10.60 2= 52.10 3= -124.90 4= -17.20

x= 16.67 y= -2.50 z= 4.00 pitch= 0.00 1= -8.53 2= 53.38 3= -128.63 4= -14.75

x= 16.30 y= -1.71 z= 4.00 pitch= 0.00 1= -5.99 2= 54.26 3= -131.42 4= -12.84

x= 16.08 y= -0.87 z= 4.00 pitch= 0.00 1= -3.09 2= 54.77 3= -133.14 4= -11.63

x= 16.00 y= -0.00 z= 4.00 pitch= 0.00 1= -0.00 2= 54.94 3= -133.72 4= -11.21

x= 16.08 y= 0.87 z= 4.00 pitch= 0.00 1= 3.09 2= 54.77 3= -133.14 4= -11.63

x= 16.30 y= 1.71 z= 4.00 pitch= 0.00 1= 5.99 2= 54.26 3= -131.42 4= -12.84

x= 16.67 y= 2.50 z= 4.00 pitch= 0.00 1= 8.53 2= 53.38 3= -128.63 4= -14.75


x= 17.17 y= 3.21 z= 4.00 pitch= 0.00 1= 10.60 2= 52.10 3= -124.90 4= -17.20

x= 17.79 y= 3.83 z= 4.00 pitch= 0.00 1= 12.15 2= 50.42 3= -120.36 4= -20.06

x= 18.50 y= 4.33 z= 4.00 pitch= 0.00 1= 13.17 2= 48.37 3= -115.17 4= -23.21

x= 19.29 y= 4.70 z= 4.00 pitch= 0.00 1= 13.69 2= 46.01 3= -109.47 4= -26.53

x= 20.13 y= 4.92 z= 4.00 pitch= 0.00 1= 13.74 2= 43.38 3= -103.42 4= -29.96

x= 21.00 y= 5.00 z= 4.00 pitch= 0.00 1= 13.39 2= 40.58 3= -97.16 4= -33.42

x= 21.00 y= 0.00 z= 8.00 pitch= 0.00 1= 0.00 2= 61.83 3= -103.54 4= -48.29

x= 15.00 y= 0.00 z= 8.00 pitch= 0.00 1= 0.00 2= 92.57 3= -142.71 4= -39.86

x= 15.00 y= 0.00 z= 8.00 pitch= 0.00 1= 0.00 2= 92.57 3= -142.71 4= -39.86

x= 15.00 y= 0.00 z= 8.00 pitch= -10.00 1= 0.00 2= 101.63 3= -139.51 4= -62.12

x= 15.00 y= 0.00 z= 8.00 pitch= -20.00 1= 0.00 2= 105.66 3= -133.50 4= -82.16

x= 15.00 y= 0.00 z= 8.00 pitch= -30.00 1= 0.00 2= 105.45 3= -125.54 4= -99.91

x= 15.00 y= 0.00 z= 8.00 pitch= -40.00 1= 0.00 2= 102.21 3= -116.27 4= -115.94

x= 15.00 y= 0.00 z= 8.00 pitch= -50.00 1= 0.00 2= 96.88 3= -106.05 4= -130.83

x= 15.00 y= 0.00 z= 8.00 pitch= -60.00 1= 0.00 2= 90.06 3= -95.04 4= -145.01

x= 15.00 y= 0.00 z= 8.00 pitch= -70.00 1= 0.00 2= 82.08 3= -83.26 4= -158.82

x= 15.00 y= 0.00 z= 8.00 pitch= -80.00 1= 0.00 2= 73.09 3= -70.55 4= -172.53

x= 15.00 y= 0.00 z= 8.00 pitch= -90.00 1= 0.00 2= 62.98 3= -56.47 4= -186.52

x= 11.00 y= 0.00 z= 8.00 pitch= -90.00 1= 0.00 2= 88.77 3= -87.70 4= -181.07

x= 11.00 y= 0.00 z= 2.00 pitch= -90.00 1= 0.00 2= 79.07 3= -110.48 4= -148.58

x= 11.00 y= 0.00 z= 2.00 pitch= -90.00 1= 0.00 2= 79.07 3= -110.48 4= -148.58

x= 11.00 y= 0.00 z= 8.00 pitch= -90.00 1= 0.00 2= 88.77 3= -87.70 4= -181.07

>>

(a video can be seen at http://youtu.be/rd4mS6ZGti4)


Manipulator Kinematics

Documents

Transcript of Manipulator Kinematics