UFMF5P-20-3: R OBOTS M ECHANICS

Coursework 1: Manipulator Kinematics

Queron Williams - 10031755

November 14, 2013

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

2. F ORWARD K INEMATICS
Forward kinematics enable us to work out the nal position of the end effector relative to the base link of the robot. To do this we must rst know the relation between each link of the robot arm.

2.1. KINEMATIC STRUCTURE OF LYNXMOTION 5DOF A RM

The Lynxmotion arm has 5 Degrees Of Freedom. The conguration of the joints in the arm are shown below. The robot consists of 6 links labelled G 0 G 6 . These Links are connected through joints j 1 j 5 . For each pair of successive joints there is a common perpendicular distance l 1 l 5 .

Figure 2.1: Joint and Link structure of Lynxmotion robot arm

10031755

Queron Williams

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 is perpendicular to the z-axis of the current joint and the z-axis of the next joint. The y-axis is chosen to complete the right hand coordinate frame for each joint.

Figure 2.2: Axis structure of Lynxmotion robot arm Some of the links (l 5 ) in the diagram actually have a link length of zero on the real robot however they have been show for clarity.

2.2. D ENAVIT-H ARTENBERG 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 the Proximal method. The derived DH parameters for the Lynxmotion arm are as follows: Link 1 2 3 4 5 E a i 1 0 0 l2 l3 0 0 i 1 0 90 0 0 -90 0 i 1 2 3 4 5 0 di l1 0 0 0 l4 l5

2.3. T RANSFORMATION M ATRICES

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

R 0 0

T 0 1

(2.1)

There is a transformation matrix that describes any possible transformation when using the proximal method. cos i si n i 0 a i 1 si n cos i i 1 cos i cos i 1 si n i 1 si n i 1 d i i 1 Ti = (2.2) cos i 1 d i si n i si n i 1 cos i si n i 1 cos i 1 0 0 0 1

10031755

Queron Williams

This can be used to create a transformation matrix for each joint or the robot arm. This is accomplished by substituting values from each line of the DH table into the above matrix. The transformation matrix for each separate joint is shown below. cos 1 si n 1 0 0 si n cos 1 0 0 1 0 (2.3) T1 = 0 0 1 l1 0 0 0 1

T2 =

cos 2 0 si n 2 0 cos 3 si n 3 0 0 cos 4 si n 4 0 0 cos 5 0 si n 5 0

si n 2 0 cos 2 0 si n 3 cos 3 0 0 si n 4 cos 4 0 0 si n 5 0 cos 5 0 0 1 0 0

0 1 0 0

0 0 0 1

(2.4)

T3 =

0 l2 0 0 1 0 0 1 0 l3 0 0 1 0 0 1 0 0 1 l4 0 0 0 1

(2.5)

T4 =

(2.6)

T5 =

(2.7)

TE =

1 0 0 0

0 0 0 0 1 l5 0 1

(2.8)

Once we have found each links transformation matrix we can multiply them together to nd a complete transformation matrix from the base to the end effector.
0 1 2 3 4 5 TE =0 T1 T2 T3 T4 T5 TE

(2.9)

TE =

c 234 c 1 c 5 s 1 s 5 c 1 s 5 + c 234 c 5 s 1 s 234 c 5 0

c 5 s 1 c 234 c 1 s 5 c 1 c 5 c 234 s 1 s 5 s 234 s 5 0

s 234 c 1 s 234 s 1 c 234 0

c 1 (L 3 c 23 + L 2 c 2 L 4 s 234 L 5 s 234 ) s 1 (L 3 c 23 + L 2 c 2 L 4 s 234 L 5 s 234 ) L 1 + L 3 s 23 + L 2 s 2 + L 4 c 234 + L 5 c 234 1 (2.10)

Where: c i = cos i s i = si n i c 23 = cos (2 + 3 ) s 23 = si n (2 + 3 ) c 234 = cos (2 + 3 + 4 ) s 234 = si n (2 + 3 + 4 )

10031755

Queron Williams

2.4. M ODELLING IN MATLAB

To model the robot arm in Matlab we are using the previously calculated transformation matrix to nd the position of each joint then drawing a lime between each joint. For all simulations I will be using link lengths measured from the 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. T ESTING F ORWARD K INEMATICS

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

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

10031755

Queron Williams

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. W ORKSPACE A NALYSIS
A robots workspace describes all possible positions that the robot can reach with its end effector. In order to accurately plot the workspace the range of possible angles for each joint were measured from the robot. These were 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 angle does not affect the nal position of the end effector.

3.1. P LOTTING THE W ORKSPACE

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 possible conguration the arm can be in. For each set of joint angles we use the forward kinematics to calculate the

10031755

Queron Williams

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

3.2. R ESULTS
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 of the 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 to reach below Zero targets. The green shows that 3 (elbow) must be outstretched to reach the outer edges of the workspace and the area above/behind. The blue shows that 4 (wrist) must be bent downwards in order to reach areas closer to the center of the workspace. In reality the robots workspace would be slightly more restricted than the plot shows due to physical constraints of the robot. The Lynxmotion arm would not be able to reach below Z=0 as it is mounted to a at surface.

10031755

Queron Williams

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. I NVERSE K INEMATICS
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 the robot.

4.1. D ERIVING THE I NVERSE K INEMATICS

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

10031755

Queron Williams

4.1.1. F INDING 1

Y 5Y 5 4Y 3 1 1/2 4X 5X X 4

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 an 2(5 y , 5x ) 4.1.2. F INDING 3

(4.1)

Z 4 3 3 3Z 2 2 3X 4X X 2 5

4Z

Figure 4.2: Robot arm as viewed in XZ plane To nd 2 and 3 we need to rst know the position of joint 4 on the XZ plane. This means that we need to calculate

10031755

Queron Williams

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

4x = 5x l 4 c 4z = 5z l 4 s

(4.2) (4.3)

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

4x = l 3 c 23 + l 2 c 2 4z = l 3 s 23 + l 2 s 2

(4.4) (4.5)

Where c i = cos i , s i = si n i , c 23 = cos (2 + 3 ), s 23 = si n (2 + 3 ). The soloution to 3 can be computed from summation of squaring both equations 4.4 and 4.5

2 2 2 2 42 x = l 3 c 23 + l 2 c 2 + 2l 3 l 2 c 23 c 2 2 2 2 2 42 z = l 3 s 23 + l 2 s 2 + 2l 3 l 2 s 23 s 2

(4.6) (4.7) (4.8)

42 x

+ 42 z

2 2 2 2 2 2 = l3 (c 23 + s 23 ) + l2 (c 2 + s 2 ) + 2l 3 l 2 (c 23 c 2 + s 23 s 2 )

Because c 2 i + s 2 i = 1, equation 4.8 can be rearanged and simplied as follows:

2 2 2 42 x + 4z = l 3 + l 2 + 2l 3 l 2 (c 23 c 2 + s 23 s 2 ) 2 2 2 42 x + 4 z = l 3 + l 2 + 2 l 3 l 2 (c ( 2 + 3 )c 2 + s ( 2 + 3 ) s 2 )

(4.9) (4.10) (4.11) (4.12)

42 x

+ 42 z

2 2 = l3 + l2 + 2l 3 l 2 (1/2(c (22 + 3 ) + c 3 ) + 1/2(c 3 c (22 + 3 ))) 2 2 2 42 x + 4 z = l 3 + l 2 + 2l 3 l 2 c 3

Rearranged for c (3 ):

c 3 =

2 2 2 l 2 l3 + 42 x + 4z

2l 2 l 3

(4.13)

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

s 3 =

2 2 2 l 2 l3 + 42 x + 4z

2l 2 l 3

(4.14)

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

3 = At an 2

2 2 l 2 l3 + 42 x

2 + 42 z

2l 2 l 3

2 2 l 2 l3 + 42 x

2l 2 l 3

+ 42 z

(4.15)

10031755

Queron Williams

4.1.3. F INDING 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

2 c 2 4x = l 3 c (2 + 3 )c 2 + l 2 c 2 2 s 2 4 z = l 3 s ( 2 + 3 ) s 2 + l 2 s 2 2 2 c 2 4x + s 2 4z = l 3 (c (2 + 3 )c 2 + s (2 + 3 )s 2 ) + l 2 (c 2 + s2 ) 2 2 c 2 4x + s 2 4z = l 3 (1/2(c (22 + 3 ) + c 3 ) + 1/2(c 3 c (22 + 3 ))) + l 2 (c 2 + s2 ) 2 2 c 2 4 x + s 2 4 z = l 3 c 3 + l 2 (c 2 + s2 )

(4.16) (4.17) (4.18) (4.19) (4.20) (4.21)

c 2 4x + s 2 4z = l 3 c 3 + l 2 We repeat this process but this time multiply equation 4.4 by s 2 and the equation 4.5 by c 2

s 2 4x = l 3 c (2 + 3 )s 2 l 2 c 2 s 2 c 2 4 z = l 3 s ( 2 + 3 )c 2 + l 2 s 2 c 2 s 2 4 x + c 2 4 z = l 3 s ( 2 + 3 )c 2 l 3 c ( 2 + 3 ) s 2 s 2 4x + c 2 4z = l 3 s 3

(4.22) (4.23) (4.24) (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 adding the resultant equations:

c 2 42 x + s 2 4 z 4 x = 4 x (l 3 c 3 + l 2 ) s 2 4 x 4 z + c 2 42 z = l 3 s 3 4z
2 c 2 (42 x + 4 z ) = 4 x (l 3 c 3 + l 2 ) + l 3 s 3 4 z

(4.26) (4.27) (4.28)

This is then rearranged for c (2 ):

c 2 =

4x (l 3 c 3 + l 2 ) + l 3 s 3 4z
2 42 x + 4z

(4.29)

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

s 2 = 1

4 x (l 3 c 3 + l 2 ) + l 3 s 3 4 z
2 42 x + 4z

(4.30)

This can be used to give us the nal atan2 solution for 2 , as before the in the formula will allow us to get both possible solutions for this joint.

2 = At an 2 1

4 x (l 3 c 3 + l 2 ) + l 3 s 3 4 z 42 x + 42 z

4x (l 3 c 3 + l 2 ) + l 3 s 3 4z
2 42 x + 4z

(4.31)

10031755

Queron Williams

10

4.1.4. F INDING 3 Now that we have 2 and 3 we can easily calculate 4 using :

4 = (2 + 3 ) 90 This will have to be repeated using the alternate 2 and 3 values to get both solutions for 4 .

(4.32)

4.2. T ESTING THE I NVERSE K INEMATICS

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

End efector position: Solution 1: Solution 2: >> 1= 0.00 1= 0.00

x= 15.62 2= 54.12 2= 68.63

y= 0.00

z= 30.52

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

3= 13.56 4= -112.68 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 and pitch 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.

Figure 4.6: Test 2 input

Figure 4.7: Test 2 solution 1

Figure 4.8: Test 2 solution 2

10031755

Queron Williams

11

The process was repeated once more for a nal 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.

Figure 4.9: Test 3 input

Figure 4.10: Test 3 solution 1

Figure 4.11: Test 3 solution 2

5. R EAL W ORLD A PPLICATION

In order to test the model I will use it to perform a task with the Lynxmotion robot arm. The Lynxmotion robot arm 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 deals with mapping required joint angles to the correct pulse width for each joint. this allows for the robot to be updated 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 before returning 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 specify the starting variables for x/y/z/pitch. Each move only requires one variable to be updated, followed by the UpdateArm() 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; UpdateArm(x,y,z,pitch,gripper,time); %move along edge 2 x=10;

10031755

Queron Williams

12

UpdateArm(x,y,z,pitch,gripper,time); %move along edge 3 z=10; UpdateArm(x,y,z,pitch,gripper,time); %move along edge 4

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

x= x= x= x= x=

10.00 20.00 20.00 10.00 10.00

y= y= y= y= y=

0.00 0.00 0.00 0.00 0.00

z= z= z= z= z=

10.00 10.00 20.00 20.00 10.00

pitch= pitch= pitch= pitch= pitch=

0.00 0.00 0.00 0.00 0.00

1=0.00 1=0.00 1=0.00 1=0.00 1=0.00

2=168.87 3=-162.03 4=-96.84 2=74.61 3=-108.12 4=-56.49 2=81.48 3=-60.86 4=-110.62 2=137.50 3=-98.82 4=-128.68 2=168.87 3=-162.03 4=-96.84

By putting the angles from each line of the log le into the forward kinematics it can be seen in gure 5.1-5.4 that the 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 gure 5.5-5.8 A more complex pick and place task was also performed. This involved picking up a pen and drawing a circle. a log le for this task is included in the appendix, a video can be seen at http://youtu.be/rd4mS6ZGti4 . This task was performed well however it did not produce a perfect circle. I found that this was due to a hardware limitation of the robot, there is too much friction in the base rotation servo so it cannot keep 1 accurate.

10031755

Queron Williams

13

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. C ONCLUSION
In conclusion I have shown how to derive the forward kinematics for the Lynxmotion robot arm using the Proximal method. If i had more time i would like to look into using the Distal method as it seams that this is a more standardised method. I have also produced a model of the robot arm and used this to test and verify that the forward kinematics are correct. The forward kinematics have then been used to accurately plot the robots workspace, showing what areas are within 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 is simpler for smaller robot arms but can become difcult when working with larger robot arms. If I was to repeat this task on a robot arm with more degrees of freedom I would try and use the algebraic method. The Inverse kinematics were then modelled and tested in a variety of positions using the Forwards kinematics to check each set 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 the forward kinematics. The task has then been executed on the physical robot and its output is shown to match with what was expected from the forward kinematics. The most difcult problem whilst planning the task was dealing with 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 before reaching its target. Overall I feel that I have produced a stable and reliable model of the robot arm. Whilst completing this task i have gained a good understanding of both forward / inverse kinematics and why these tools can be useful for designing robotic systems.

10031755

Queron Williams

14

A. MATLAB C ODE - MAIN . M

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49

% % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % Queron Williams % % % % % % % % % % % % % % % % % % % % % Robotic Mechanics % % % % % % % % % % % % % % % % % % % % % % October 2013 % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % f i r s t c l e a r everything clc clear a l l close a l l % % s t a t i c varables and gl o ba l s global rad2deg deg2rad L1 L2 L3 L4 L5 global IKtheta1 IKtheta2a IKtheta3a IKtheta4a IKtheta2b IKtheta3b IKtheta4b deg2rad = pi /180 ; % multiplying by t h i s w i l l convert degrees to rads rad2deg = 180/ pi ; % multiplying by t h i s w i l l convert degrees to rads % % Program options PlotAxis = 0; PlotLinks = 1 ; PlotWorkspace = 0 ; InverseKinematics = 0 ; executePath = 1 ; % % v a r i a b l e s such as l i n k length and theta s %l i n k lengths L1 = 6 . 9 ; L2 = 9 . 4 ; L3 = 1 0 . 8 ; L4 = 8 . 5 ; L5 = 0 ; % thetas theta1 = 00 * deg2rad ; %90 to 90 theta2 = 137.50 * deg2rad ; % around 10 to 90 theta3 = 98.82 * deg2rad ; theta4 = 128.68 * deg2rad ; theta5 = 0 * deg2rad ; % % DH %l i n k DH1 = DH2 = DH3 = DH4 = DH5 = DHE = table ai 1 [ 0, [ 0, [ L2 , [ L3 , [ 0, [ 0,

alphai 1 0, ( pi /2) , 0, 0, ( pi /2) , 0,

d L1 , 0, 0, 0, L4 , L5 ,

theta theta1 ] ; theta2 ] ; theta3 ] ; theta4 ] ; theta5 ] ; 0];

% % transformation matrix s % c a l c u l a t e the transformation matrix f o r each l i n k BL = get_dh_matrix ( [ 0 , 0 , 0 , 0 ] ) ; %Base Link

10031755

Queron Williams

15

50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101

T01 T12 T23 T34 T45 T5E

= = = = = =

get_dh_matrix (DH1) ; get_dh_matrix (DH2) ; get_dh_matrix (DH3) ; get_dh_matrix (DH4) ; get_dh_matrix (DH5) ; get_dh_matrix (DHE) ; the t o t a l transformation matrix T12 ; T23 ; T34 ; T45 ; T5E ;

% calculate T02 = T01 * T03 = T02 * T04 = T03 * T05 = T04 * T0E = T05 *

% % % % % % hold on grid on ; y l a b e l ( Y (cm) ) , x l a b e l ( X(cm) ) , z l a b e l ( Z(cm) ) daspect ( [ 1 1 1 ] ) %keep aspect r a t i o the same %view ([ 1 , 1 ,1])%isometric view ( [ 0 , 1 , 0 ] )%side %view ( [ 1 , 0 , 0 ] )%back %a x i s auto a x i s ([ 10 25 0 25 5 2 5 ] ) ; %manualy s e t a x i s %a x i s ([ 30 30 30 30 15 4 0 ] ) ; %manualy s e t a x i s i f PlotLinks >= 1 OOx = [ 0 ; T01 ( 1 , 4 ) ; T02 ( 1 , 4 ) ; T03 ( 1 , 4 ) ; OOy = [ 0 ; T01 ( 2 , 4 ) ; T02 ( 2 , 4 ) ; T03 ( 2 , 4 ) ; OOz = [ 0 ; T01 ( 3 , 4 ) ; T02 ( 3 , 4 ) ; T03 ( 3 , 4 ) ; plot3 (OOx,OOy,OOz, b , LineWidth , PlotLinks ) t e x t (BL( 1 , 4 ) ,BL( 2 , 4 ) ,BL( 3 , 4 ) , L0 ) ; t e x t ( T01 ( 1 , 4 ) , T01 ( 2 , 4 ) , T01 ( 3 , 4 ) , L1 ) ; t e x t ( T02 ( 1 , 4 ) , T02 ( 2 , 4 ) , T02 ( 3 , 4 ) , L2 ) ; t e x t ( T03 ( 1 , 4 ) , T03 ( 2 , 4 ) , T03 ( 3 , 4 ) , L3 ) ; t e x t ( T04 ( 1 , 4 ) , T04 ( 2 , 4 ) , T04 ( 3 , 4 ) , L4 ) ; t e x t ( T05 ( 1 , 4 ) , T05 ( 2 , 4 ) , T05 ( 3 , 4 ) , L5 ) ; t e x t ( T0E ( 1 , 4 ) ,T0E ( 2 , 4 ) ,T0E ( 3 , 4 ) , LE ) end i f P l o t A x i s == 1 t r p l o t (BL , frame , t r p l o t ( T01 , frame t r p l o t ( T02 , frame t r p l o t ( T03 , frame t r p l o t ( T04 , frame t r p l o t ( T05 , frame t r p l o t ( T0E , frame end

T04 ( 1 , 4 ) ; T05 ( 1 , 4 ) ; T0E ( 1 , 4 ) ] ; T04 ( 2 , 4 ) ; T05 ( 2 , 4 ) ; T0E ( 2 , 4 ) ] ; T04 ( 3 , 4 ) ; T05 ( 3 , 4 ) ; T0E ( 3 , 4 ) ] ; % t h i s p l o t s the p vector

, , , , , ,

0 , 1 2 3 4 5 e

, , , , , ,

color , color color color color color color

, , , , , ,

[1 ,0 ,0]) ; [0.6 ,0 ,0.3]) ; [0.3 ,0 ,0.6]) ; [0 ,0 ,1]) ; [0 ,0.3 ,0.6]) ; [0 ,0.6 ,0.3]) ; [0 ,1 ,0]) ;

10031755

Queron Williams

16

102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152

i f PlotWorkspace == 1 X = []; Y = []; Z = []; C = [] ,[]; f o r i = 90:2:90 , f p r i n t f ( at i t e r a t i o n : %d \n , i ) %progress report 4 workspace plot f o r j =0:10:135 , f o r k= 135:10:0 , f o r l = 180:5:0 , theta1= i * deg2rad ; theta2= j * deg2rad ; theta3=k * deg2rad ; theta4= l * deg2rad ; DH1 DH2 DH3 DH4 DH5 DHE T01 T02 T03 T04 T05 T0E = = = = = = = = = = = = [ [ [ [ [ [ 0, 0, L2 , L3 , 0, 0, 0, ( pi /2) , 0, 0, ( pi /2) , 0, L1 , 0, 0, 0, L4 , L5 , theta1 ] ; theta2 ] ; theta3 ] ; theta4 ] ; theta5 ] ; 0];

get_dh_matrix (DH1) ; T01 * get_dh_matrix (DH2) ; T02 * get_dh_matrix (DH3) ; T03 * get_dh_matrix (DH4) ; T04 * get_dh_matrix (DH5) ; T05 * get_dh_matrix (DHE) ;

C( end+1 ,1) = 1 ( j /135) ; %s e t red value from j o i n t 2 C( end , 2 ) = ( ( k+135) /135) ;%s e t green value from j o i n t 3 C( end , 3 ) = ( l / 180) ; %s e t blue value from j o i n t 4 X( end+1) = T0E ( 1 , 4 ) ; Y ( end+1) = T0E ( 2 , 4 ) ; Z( end+1) = T0E ( 3 , 4 ) ; end end end end s c a t t e r 3 (X , Y , Z, 2 0 ,C, f i l l e d ) ; hold on end %hold o f f i f InverseKinematics == 1 %phi = 91+ theta2+theta3+theta4 ; %comment in f o r same phi as input phi = 4 5; %comment in to manualy s e t wrwtg angles

10031755

Queron Williams

17

153

154

155

156

157

[ IKtheta1 , IKtheta2a , IKtheta3a , IKtheta4a , IKtheta2b , IKtheta3b , IKtheta4b ] = solveIK ( T0E ( 1 , 4 ) ,T0E ( 2 , 4 ) ,T0E ( 3 , 4 ) , phi ) ; 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 ) f p r i n t f ( End e f e c t o r position : \ t x= %.2 f \ t y= %.2 f \ t z= %.2 f \n , T0E ( 1 , 4 ) , T0E ( 2 , 4 ) , T0E ( 3 , 4 ) ) 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 ) 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 ) end i f executePath == 2 %simple square path x =10; y =0; z =10; pitch =0; gripper =0; %gripper i s open time =2; %time f o r each move to take (2 seconds ) UpdateArm( x , y , z , pitch , gripper , time ) ; %move to s t a r t point x =20; UpdateArm( x , y , z , pitch , gripper , time ) ; %move along edge 1 z =20; UpdateArm( x , y , z , pitch , gripper , time ) ; %move along edge 2 x =10; UpdateArm( x , y , z , pitch , gripper , time ) ; %move along edge 3 z =10; UpdateArm( x , y , z , pitch , gripper , time ) ; %move along edge 4 end i f executePath == 1 %pick and place task path pathX = 11; pathY = 0 ; pathZ = 5 ; pathWRWTG = 90; pathGripper = 0 ; UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 2 ) ; pathZ = 2 ; UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 2 ) ; pathGripper = 45; UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 2 ) ; pathZ = 8 ; UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 2 ) ;

158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199

10031755

Queron Williams

18

200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248

pathX = 15; UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 2 ) ; f o r i = 90:10:0 , pathWRWTG = i ; UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 0 . 4 ) ; end pathX = 21; UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 2 ) ; pathY = 5 ; UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 2 ) ; pathZ = 4 ; UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 2 ) ;

f o r i =0 : 10 :360 , pathX = 21 + ( 5 * sin ( i * deg2rad ) ) ; pathY = ( 5 * cos ( i * deg2rad ) ) ; UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 0 . 4 ) ; end pathY = 0 ; pathZ = 8 ; UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 2 ) ; pathX = 15; UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 2 ) ; f o r i =0: 10: 90 , pathWRWTG = i ; UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 0 . 4 ) ; end pathX = 11; UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 2 ) ; pathZ = 2 ; UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 2 ) ; pathGripper = 0 ; UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 2 ) ; pathZ = 8 ; UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 2 ) ; end

10031755

Queron Williams

19

B. MATLAB C ODE - SOLVE IK. M

1

function [ IKtheta1 , IKtheta2a , IKtheta3a , IKtheta4a , IKtheta2b , IKtheta3b , IKtheta4b ] = solveIK ( xTarget , yTarget , zTarget , wrwtgTarget ) global rad2deg deg2rad L1 L2 L3 L4 IKtheta1 IKtheta2a IKtheta3a IKtheta4a IKtheta2b IKtheta3b IKtheta4b IKtheta1 = atan2 ( yTarget , xTarget ) * rad2deg ; %wrwtg = 90+ theta2+theta3+theta4 ; wrwtg = wrwtgTarget ; z = zTarget (L4 * sin ( wrwtg * deg2rad ) )L1 ; XYhyp = s q r t ( ( xTarget ^2) +( yTarget ^2) ) (L4 * cos ( wrwtg * deg2rad ) ) ; IKcostheta3 = ( ( XYhyp^2) +( z ^2) (L2^2) (L3^2) ) / ( 2 * L2 * L3 ) ; IKtheta3a = atan2 (+ s q r t (1 ( IKcostheta3 ^2) ) , IKcostheta3 ) * rad2deg ; IKtheta3b = atan2( s q r t (1 ( IKcostheta3 ^2) ) , IKcostheta3 ) * rad2deg ; IKcostheta2a = ( ( XYhyp * ( L2+(L3 * cos ( IKtheta3a * deg2rad ) ) ) ) +( z * L3 * sin ( IKtheta3a * deg2rad ) ) ) / ( ( XYhyp^2) +( z ^2) ) ; IKcostheta2b = ( ( XYhyp * ( L2+(L3 * cos ( IKtheta3b * deg2rad ) ) ) ) +( z * L3 * sin ( IKtheta3b * deg2rad ) ) ) / ( ( XYhyp^2) +( z ^2) ) ; IKtheta2a = atan2 (+ s q r t (1 ( IKcostheta2a ^2) ) , IKcostheta2a ) * rad2deg ; IKtheta2b = atan2 (+ s q r t (1 ( IKcostheta2b ^2) ) , IKcostheta2b ) * rad2deg ; IKtheta4a = wrwtg ( IKtheta2a+IKtheta3a ) 90; IKtheta4b = wrwtg (IKtheta2b+IKtheta3b ) 90; end

3 4 5 6 7 8 9 10 11 12 13 14 15

16

17 18 19 20 21 22 23

10031755

Queron Williams

20

C. MATLAB C ODE - U PDATE J OINT. M

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28

function updateJoint ( j o i n t , t a r g e t , SpeedOrTime , SpeedOrTimeAmount ) 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 use p e r s i s t e n t arm ; i f isempty ( f i r s t ) delete ( i n s t r f i n d a l l ) ; %i f f i r s t use c l e a r any previous s e r i a l objects arm = s e r i a l ( COM13 , BaudRate , 115200 , Terminator , CR/LF ) ; fopen (arm) ; %open new s e r i a l port f i r s t = 1; end switch j o i n t ; %depending on j o i n t , convert degrees to pulse width case 0 t a r g e t = round (map( t a r g e t , 90 ,90 ,500 ,2500) ) ; %checked case 1 t a r g e t = round (map( t a r g e t ,0 ,180 ,750 ,2250) ) ; %checked case 2 t a r g e t = round (map( t a r g e t , 180 ,0 ,2500 ,500) ) ; %checked case 3 t a r g e t = round (map( t a r g e t , 180 ,0 ,500 ,2500) ) ; %checked case 4 t a r g e t = round (map( t a r g e t , 90 ,90 ,500 ,2500) ) ; %checked case 5 t a r g e t = round (map( t a r g e t , 90 ,90 ,500 ,2500) ) ; %checked end %send pulse width to servo f p r i n t f (arm , s p r i n t f ( #%d P %d %c %d , j o i n t , t a r g e t , SpeedOrTime , SpeedOrTimeAmount) ) end

D. MATLAB C ODE - U PDATE A RM . M

1 2 3

4 5 6 7 8 9 10 11

function UpdateArm( x , y , z , wrwtg , gripper , time ) global IKtheta1 IKtheta2a IKtheta3a IKtheta4a IKtheta2b IKtheta3b IKtheta4b ; [ IKtheta1 , IKtheta2a , IKtheta3a , IKtheta4a , IKtheta2b , IKtheta3b , IKtheta4b ] = solveIK ( x , y , z , wrwtg ) ; UpdateJoint ( 0 , IKtheta1 , t , ( time * 1000) ) ; UpdateJoint ( 1 , IKtheta2b , t , ( time * 1000) ) ; UpdateJoint ( 2 , IKtheta3b , t , ( time * 1000) ) ; UpdateJoint ( 3 , IKtheta4b , t , ( time * 1000) ) ; UpdateJoint ( 4 , gripper , t ,500 ) ; UpdateJoint ( 5 , 0 , t ,500 ) ; 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 ) ; 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 ) ; delay ( time ) ; end

12 13 14

10031755

Queron Williams

21

E. MATLAB C ODE - GET _ DH _ MATRIX . M

1 2 3

5 6

function T = get_dh_matrix ( parameters ) T = [ ( cos ( parameters ( 1 , 4 ) ) ) , ( sin ( parameters ( 1 , 4 ) ) ) , 0 , ( parameters ( 1 , 1 ) ) ; ( 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 ) ) ; ( 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) ) ; 0 ,0 ,0 ,1]; end

F. MATLAB C ODE - MAP. M

1 2 3

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

G. MATLAB C ODE - DELAY. M

1 2 3 4 5 6 7

function delay ( seconds ) % function pause the program % seconds = delay time in seconds tic ; while toc < seconds end end

10031755

Queron Williams

22

H. L OG FILE FOR COMPLEX TASK

x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= 11.00 11.00 11.00 11.00 15.00 15.00 15.00 15.00 15.00 15.00 15.00 15.00 15.00 15.00 15.00 21.00 21.00 21.00 21.00 21.87 22.71 23.50 24.21 24.83 25.33 25.70 25.92 26.00 25.92 25.70 25.33 24.83 24.21 23.50 22.71 21.87 21.00 20.13 19.29 18.50 17.79 17.17 16.67 16.30 16.08 16.00 16.08 16.30 16.67 y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 5.00 5.00 5.00 4.92 4.70 4.33 3.83 3.21 2.50 1.71 0.87 0.00 -0.87 -1.71 -2.50 -3.21 -3.83 -4.33 -4.70 -4.92 -5.00 -4.92 -4.70 -4.33 -3.83 -3.21 -2.50 -1.71 -0.87 -0.00 0.87 1.71 2.50 z= 5.00 z= 2.00 z= 2.00 z= 8.00 z= 8.00 z= 8.00 z= 8.00 z= 8.00 z= 8.00 z= 8.00 z= 8.00 z= 8.00 z= 8.00 z= 8.00 z= 8.00 z= 8.00 z= 8.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 pitch= -90.00 1= 0.00 2= 86.56 3= -101.49 4= -165.07 pitch= -90.00 1= 0.00 2= 79.07 3= -110.48 4= -148.58 pitch= -90.00 1= 0.00 2= 79.07 3= -110.48 4= -148.58 pitch= -90.00 1= 0.00 2= 88.77 3= -87.70 4= -181.07 pitch= -90.00 1= 0.00 2= 62.98 3= -56.47 4= -186.52 pitch= -90.00 1= 0.00 2= 62.98 3= -56.47 4= -186.52 pitch= -80.00 1= 0.00 2= 73.09 3= -70.55 4= -172.53 pitch= -70.00 1= 0.00 2= 82.08 3= -83.26 4= -158.82 pitch= -60.00 1= 0.00 2= 90.06 3= -95.04 4= -145.01 pitch= -50.00 1= 0.00 2= 96.88 3= -106.05 4= -130.83 pitch= -40.00 1= 0.00 2= 102.21 3= -116.27 4= -115.94 pitch= -30.00 1= 0.00 2= 105.45 3= -125.54 4= -99.91 pitch= -20.00 1= 0.00 2= 105.66 3= -133.50 4= -82.16 pitch= -10.00 1= 0.00 2= 101.63 3= -139.51 4= -62.12 pitch= 0.00 1= 0.00 2= 92.57 3= -142.71 4= -39.86 pitch= 0.00 1= 0.00 2= 61.83 3= -103.54 4= -48.29 pitch= 0.00 1= 13.39 2= 59.07 3= -99.22 4= -49.85 pitch= 0.00 1= 13.39 2= 40.58 3= -97.16 4= -33.42 pitch= 0.00 1= 13.39 2= 40.58 3= -97.16 4= -33.42 pitch= 0.00 1= 12.69 2= 37.67 3= -90.83 4= -36.84 pitch= 0.00 1= 11.69 2= 34.72 3= -84.56 4= -40.16 pitch= 0.00 1= 10.44 2= 31.83 3= -78.51 4= -43.32 pitch= 0.00 1= 8.99 2= 29.09 3= -72.84 4= -46.24 pitch= 0.00 1= 7.38 2= 26.59 3= -67.75 4= -48.84 pitch= 0.00 1= 5.64 2= 24.46 3= -63.43 4= -51.03 pitch= 0.00 1= 3.81 2= 22.82 3= -60.12 4= -52.70 pitch= 0.00 1= 1.92 2= 21.77 3= -58.03 4= -53.75 pitch= 0.00 1= 0.00 2= 21.41 3= -57.31 4= -54.10 pitch= 0.00 1= -1.92 2= 21.77 3= -58.03 4= -53.75 pitch= 0.00 1= -3.81 2= 22.82 3= -60.12 4= -52.70 pitch= 0.00 1= -5.64 2= 24.46 3= -63.43 4= -51.03 pitch= 0.00 1= -7.38 2= 26.59 3= -67.75 4= -48.84 pitch= 0.00 1= -8.99 2= 29.09 3= -72.84 4= -46.24 pitch= 0.00 1= -10.44 2= 31.83 3= -78.51 4= -43.32 pitch= 0.00 1= -11.69 2= 34.72 3= -84.56 4= -40.16 pitch= 0.00 1= -12.69 2= 37.67 3= -90.83 4= -36.84 pitch= 0.00 1= -13.39 2= 40.58 3= -97.16 4= -33.42 pitch= 0.00 1= -13.74 2= 43.38 3= -103.42 4= -29.96 pitch= 0.00 1= -13.69 2= 46.01 3= -109.47 4= -26.53 pitch= 0.00 1= -13.17 2= 48.37 3= -115.17 4= -23.21 pitch= 0.00 1= -12.15 2= 50.42 3= -120.36 4= -20.06 pitch= 0.00 1= -10.60 2= 52.10 3= -124.90 4= -17.20 pitch= 0.00 1= -8.53 2= 53.38 3= -128.63 4= -14.75 pitch= 0.00 1= -5.99 2= 54.26 3= -131.42 4= -12.84 pitch= 0.00 1= -3.09 2= 54.77 3= -133.14 4= -11.63 pitch= 0.00 1= -0.00 2= 54.94 3= -133.72 4= -11.21 pitch= 0.00 1= 3.09 2= 54.77 3= -133.14 4= -11.63 pitch= 0.00 1= 5.99 2= 54.26 3= -131.42 4= -12.84 pitch= 0.00 1= 8.53 2= 53.38 3= -128.63 4= -14.75

10031755

Queron Williams

23

x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= >>

17.17 17.79 18.50 19.29 20.13 21.00 21.00 15.00 15.00 15.00 15.00 15.00 15.00 15.00 15.00 15.00 15.00 15.00 11.00 11.00 11.00 11.00

y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y=

3.21 3.83 4.33 4.70 4.92 5.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00

z= z= z= z= z= z= z= z= z= z= z= z= z= z= z= z= z= z= z= z= z= z=

4.00 4.00 4.00 4.00 4.00 4.00 8.00 8.00 8.00 8.00 8.00 8.00 8.00 8.00 8.00 8.00 8.00 8.00 8.00 2.00 2.00 8.00

pitch= pitch= pitch= pitch= pitch= pitch= pitch= pitch= pitch= pitch= pitch= pitch= pitch= pitch= pitch= pitch= pitch= pitch= pitch= pitch= pitch= pitch=

0.00 1= 10.60 0.00 1= 12.15 0.00 1= 13.17 0.00 1= 13.69 0.00 1= 13.74 0.00 1= 13.39 0.00 1= 0.00 0.00 1= 0.00 0.00 1= 0.00 -10.00 1= 0.00 -20.00 1= 0.00 -30.00 1= 0.00 -40.00 1= 0.00 -50.00 1= 0.00 -60.00 1= 0.00 -70.00 1= 0.00 -80.00 1= 0.00 -90.00 1= 0.00 -90.00 1= 0.00 -90.00 1= 0.00 -90.00 1= 0.00 -90.00 1= 0.00

2= 52.10 3= -124.90 4= -17.20 2= 50.42 3= -120.36 4= -20.06 2= 48.37 3= -115.17 4= -23.21 2= 46.01 3= -109.47 4= -26.53 2= 43.38 3= -103.42 4= -29.96 2= 40.58 3= -97.16 4= -33.42 2= 61.83 3= -103.54 4= -48.29 2= 92.57 3= -142.71 4= -39.86 2= 92.57 3= -142.71 4= -39.86 2= 101.63 3= -139.51 4= -62.12 2= 105.66 3= -133.50 4= -82.16 2= 105.45 3= -125.54 4= -99.91 2= 102.21 3= -116.27 4= -115.94 2= 96.88 3= -106.05 4= -130.83 2= 90.06 3= -95.04 4= -145.01 2= 82.08 3= -83.26 4= -158.82 2= 73.09 3= -70.55 4= -172.53 2= 62.98 3= -56.47 4= -186.52 2= 88.77 3= -87.70 4= -181.07 2= 79.07 3= -110.48 4= -148.58 2= 79.07 3= -110.48 4= -148.58 2= 88.77 3= -87.70 4= -181.07

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

10031755

Queron Williams

24