UFMF5P-20-3: R OBOTS M ECHANICS

Coursework 2: Parallel Manipulator Kinematics

Queron Williams - 10031755

December 12, 2013

1. I NTRODUCTION
This document outlines the methods used to derive Inverse Kinematics (IK) for a 3 Degrees Of Freedom planar parallel robot. This robot is used to guide a surgical instrument that allows the surgeon to precisely insert a needle during surgery. Using the derived IK we will create a Matlab model of the parallel robot. The model will allow us to plot and analyse the workspace.

2. I NVERSE K INEMATICS (IK)

2.1. K INEMATIC S TRUCTURE
The planar parallel robot has 3 Degrees Of Freedom (X, Y and ). As shown in gure 2.1 the robot consists of a xed base plate, a moving platform, six passive revolute joints (Mi and PPi where i=1:3) and three active revolute joints (PBi where i=1:3). The ith strut of the robot consists therefore of three joints that are subdivided in the upper section L1 and the lower section L2. The planar parallel robot that I will be using has a base radius of 290mm and a platform radius of 130mm. This means that each P B i will be 290mm from the center of the base (CB) and each P P i will be 130mm from the center of the platform (CP). The length of L1 will be 170mm and L2 will be 130mm. It is important to note that for our example L2 < L1 as this effects the shape of the workspace. The input parameter X describes the translation of CP relative to CB along the X axis. Likewise Y describes the translation of CP relative to CB along the Y axis. describes the rotation of the platform around the Z axis about the point CP.

10031755

Queron Williams

Active Revolute Joint

Base

Passive Revolute Joint Platform

CP CB

PPi PBi L1 Mi
Figure 2.1: Joint and Link structure of the parallel robot ( at 30 degrees)

L2

Solution 1

i
Solution 2

2.2. S OLVING I NVERSE K INEMATICS

In order to solve the IK we will start by nding the Cartesian position of P B i and P P i for each leg (where i=1:3). These positions will then be used to nd the distance between P B i and P P i , then solve i before naly solving the i for each leg. 2.2.1. F INDING P OSITION OF P B i Finding the Cartesian position of P B i is simply found using sin and cos as shown in gure 2.2 . If i=1 then =0, if i=2 then =120 and if i=2 then =240 (each set of links is 120 degrees apart to form the base triangle). To nd the position of P B i we then use P B i X =baseRadius*cos( ) and P B i Y =baseRadius*sin( )

10031755

Queron Williams

y Radius * cos

Radius * sin

ad

iu

Figure 2.2: X,Y position of a point around a circle 2.2.2. F INDING P OSITION OF P P i Finding the Cartesian position of P P i is similar to nding the position of P B i however the position is offset by the X and Y input parameters. Also each of the values must have added to them to account for the rotation of the platform. To nd the position of P P i we use P B i X =X+platformRadius*cos( +) and P B i Y =Y+platformRadius*sin( + ) 2.2.3. S OLVING REMAINING ANGLES

Y ti (PPi) l2 Mi MZ PBi l1 MX tX X

tZ

Figure 2.3: Diagram of structure of each set of links

10031755

Queron Williams

From this point we must solve each set of links individually. For ease of notation we will let t i = P P i -P B i , making t i the position of P P i relative toP B i . 2.2.4. F INDING To nd the t x value we need only sum the lengths of the adjacent sides of the two right-angled triangles formed by l 1 & and l 2 &. For t y it is the same but using the opposite sides.

t x = l 2 c + l 1 c t y = l 2 s + l 1 s

(2.1) (2.2)

Where c = cos , s = si n , c = cos , s = si n , c = cos ( + ), s = si n ( + ). The soloution to can be computed from summation of squaring both equations 2.1 and 2.2

2 2 2 2 2 tx = l2 c + l 1 c + 2l 2 l 1 c c 2 2 2 2 2 ty = l2 s + l 1 s + 2l 2 l 1 s s 2 2 2 2 2 2 2 2 + ty = l2 tx (c + s ) + l 1 (c + s ) + 2l 2 l 1 (c c + s s )

(2.3) (2.4) (2.5)

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

2 2 2 2 tx + ty = l2 + l1 + 2 l 2 l 1 (c c + s s ) 2 2 2 2 tx + ty = l2 + l1 + 2l 2 l 1 (c ( + )c + s ( + )s ) 2 2 2 2 + ty = l2 tx + l1 + 2l 2 l 1 (1/2(c (2 + ) + c ) + 1/2(c c (2 + ))) 2 2 2 2 + ty = l2 + l1 + 2l 2 l 1 c tx

(2.6) (2.7) (2.8) (2.9)

Rearranged for c :

c =

2 2 2 2 l 1 l2 + tx + ty

2l 1 l 2

(2.10)

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

s =

2 2 2 2 l 1 l2 + tx + ty

2l 1 l 2

(2.11)

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.

= At an 2

2 2 2 2 2 l 1 l2 + tx + ty

2l 1 l 2

2 2 2 2 l 1 l2 + tx + ty

2l 1 l 2

(2.12)

10031755

Queron Williams

2.2.5. F INDING Now that we have , we can use equation 2.1 and 2.2 to calculate . To do this we multiply equation 2.1 by c and the equation 2.2 by s

2 c t x = l 2 c ( + )c + l 1 c 2 s t y = l 2 s ( + )s + l 1 s 2 2 c t x + s t y = l 2 (c ( + )c + s ( + )s ) + l 1 (c + s ) 2 2 c t x + s t y = l 2 (1/2(c (2 + ) + c ) + 1/2(c c (2 + ))) + l 1 (c + s ) 2 2 c t x + s t y = l 2 c + l 1 (c + s )

(2.13) (2.14) (2.15) (2.16) (2.17) (2.18)

c t x + s t y = l 2 c + l 1 We repeat this process but this time multiply equation 2.1 by s and the equation 2.2 by c

s t x = l 2 c ( + )s l 1 c s c t y = l 2 s ( + )c + l 1 s c s t x + c t y = l 2 s ( + )c l 2 c ( + )s s t x + c t y = l 2 s

(2.19) (2.20) (2.21) (2.22)

Now we multiply both sides of equation 2.18 by s , and both sides of equation 2.22 by c : before adding the resultant equations:

2 c t x + s t y t x = t x (l 2 c + l 1 ) 2 s t x t y + c t y = l 2 s t y 2 2 c (t x + ty ) = t x (l 2 c + l 1 ) + l 2 s t y

(2.23) (2.24) (2.25)

This is then rearranged for c :

c =

t x (l 2 c + l 1 ) + l 2 s t y
2 2 tx + ty

(2.26)

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

s =

t x (l 2 c + l 1 ) + l 2 s t y
2 2 tx + ty

(2.27)

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

= At an 2

t x (l 2 c + l 1 ) + l 2 s t y
2 2 tx + ty

t x (l 2 c + l 1 ) + l 2 s t y 2 2 tx + ty

(2.28)

10031755

Queron Williams

2.2.6. F INDING This Process must be repeated for each set of links using the respective P B i and P P i values to get each set of i and i values.

2.3. I MPLEMENTING IN M ATLAB

In order to implement this within Matlab I rst use the desired X, Y and values to generate the X,Y positions of P B i and P P i for each set of links. I then wrote a Function that is able to review each set of links. The function assesses whether it is possible for this link to be solved or not. If it is possible then it returns both sets of possible solutions, if not then it returns an error to the main program. If all 3 sets of links return valid solutions without errors then the desired target position is achievable. These solutions are then rendered. All of the possible solutions are also printed in the terminal. With X, Y and values set to zero Figure 2.3 is produced.

Figure 2.4: IK soloution at X=0 Y=0 =0 This target position gave me the following solution in the terminal

If the platform is at X=0 and Y=0 with a rotation angle of 0 degrees : Leg 1, (bottom left) : Solution 1 (red) : Theta=103.72 Psi=117.19 Solution 2 (pink): Theta=-163.72 Psi=-117.19 Leg 2, (bottom right) : Solution 1 (red) : Theta=-16.28 Psi=117.19 Solution 2 (pink): Theta=76.28 Psi=-117.19 Leg 3, (top center) : Solution 1 (red) : Theta=-136.28 Psi=117.19 Solution 2 (pink): Theta=-43.72 Psi=-117.19
Each of the angles provided in the solution can be checked against the produced drawing of the robot to verify that it is correct.

10031755

Queron Williams

In order to verify that the model remains accurate with different inputs I shall run this same test again, each time changing the input variables. rstley i will set X=60.

Figure 2.5: IK soloution at X=60 Y=0 =0 The target position shown in Figure 2.3 gave me the following solution in the terminal.

If the platform is at X=60 and Y=0 with a rotation angle of 0 degrees : Leg 1, (bottom left) : Solution 1 (red) : Theta=84.61 Psi=138.74 Solution 2 (pink): Theta=-175.65 Psi=-138.74 Leg 2, (bottom right) : Solution 1 (red) : Theta=-15.45 Psi=89.96 Solution 2 (pink): Theta=59.34 Psi=-89.96 Leg 3, (top center) : Solution 1 (red) : Theta=-114.28 Psi=112.06 Solution 2 (pink): Theta=-24.61 Psi=-112.06

10031755

Queron Williams

Next I will set Y=60.

Figure 2.6: IK soloution at X=0 Y=60 =0 The target position shown gave me the following solution in the terminal.

If the platform is at X=0 and Y=60 with a rotation angle of 0 degrees : Leg 1, (bottom left) : Solution 1 (red) : Theta=94.04 Psi=99.11 Solution 2 (pink): Theta=175.37 Psi=-99.11 Leg 2, (bottom right) : Solution 1 (red) : Theta=4.63 Psi=99.11 Solution 2 (pink): Theta=85.96 Psi=-99.11 Leg 3, (top center) : Solution 1 (red) : Theta=-139.68 Psi=144.09 Solution 2 (pink): Theta=-40.32 Psi=-144.09

10031755

Queron Williams

Next I will set =30.

Figure 2.7: IK soloution at X=0 Y=0 =30 The target position shown gave me the following solution in the terminal.

If the platform is at X=0 and Y=0 with a rotation angle of 30 degrees : Leg 1, (bottom left) : Solution 1 (red) : Theta=87.83 Psi=103.21 Solution 2 (pink): Theta=171.93 Psi=-103.21 Leg 2, (bottom right) : Solution 1 (red) : Theta=-32.17 Psi=103.21 Solution 2 (pink): Theta=51.93 Psi=-103.21 Leg 3, (top center) : Solution 1 (red) : Theta=-152.17 Psi=103.21 Solution 2 (pink): Theta=-68.07 Psi=-103.21

10031755

Queron Williams

Finaly i will set all input varables at once. X=60 Y=60 =30.

Figure 2.8: IK soloution at X=60 Y=60 =30 The target position shown gave me the following solution in the terminal.

If the platform is at X=60 and Y=60 with a rotation angle of 30 degrees : Leg 1, (bottom left) : Solution 1 (red) : Theta=69.19 Psi=90.05 Solution 2 (pink): Theta=144.03 Psi=-90.05 Leg 2, (bottom right) : Solution 1 (red) : Theta=-4.25 Psi=58.14 Solution 2 (pink): Theta=45.41 Psi=-58.14 Leg 3, (top center) : Solution 1 (red) : Theta=-142.20 Psi=136.36 Solution 2 (pink): Theta=-42.68 Psi=-136.36
It can be seen that through all of these tests the solutions remain accurate.

3. PARALLEL R OBOT W ORKSPACE

3.1. P LOTTING THE W ORKSPACE
There are 2 main ways of plotting a robot workspace. The rst involves using the Forward Kinematics of a robot to plot the points that are reachable for every possible set of input angles. The second uses Inverse Kinematics to determine if there is a possible solution for the robot arm at every possible target position around the robot. With My Matlab model of the Parallel Robot I only have Inverse Kinematics so i have used the second Method for plotting the workspace. This was implemented by creating a grid of target positions around the end effector of the parallel robot. I then change my input variables to match each given target position and check if this position has a valid IK solution for

10031755

Queron Williams

10

all 3 Links. If there is a valid solution then this grid cell is marked. This process is repeated for all the cells in the grid. The nal product of this process is shown in Figure 3.2 where the rotation of the platform has been locked.

3.2. W ORKSAPCE A NALYSIS

For Figure 3.1 of the workspace the rotation of the platform has been locked at =0.

Figure 3.1: Workspace with =0 The long convex edges of the workspace are caused by the limited reach of the platform away from each corner. The smaller concave curves on the corners of the workspace however are caused by the fact that L1 > L2 on this particular robot. Because L1 is longer than L2, P P i can never reach back to P B i even if i is 180 degrees, causing a small area where the platform cannot reach near each P B i .

10031755

Queron Williams

11

Figure 3.2: Workspace with =0 Figure 3.2 shows the workspace for =0 and =10 plotted on the same axis. It can be seen that some of the areas outside of the =0 workspace are now accessible within the =10 workspace. This means that some solutions within the complete operational workspace of the robot require a specic angle in order to be reached by the platform.

Figure 3.3: Workspace with =10

Figure 3.4: Workspace with =20

10031755

Queron Williams

12

Figure 3.5: Workspace with =30

Figure 3.6: Workspace with =40

Figure 3.7: Workspace with =50

Figure 3.8: Workspace with =60

Figure 3.9: Workspace with =70

Figure 3.10: Workspace with =80

When we begin to vary the value for the platform we can see that the workspace begins to shrink. This is because the distance between P B i and P P i is increased when at the same target position, meaning it can travel less distance before it reaches its limits.

10031755

Queron Williams

13

Figure 3.11: Workspace with at 5 degree intervals Figure 3.11 shows the total workspace over a range of values (5 degree increments). The colour of shows the largest possible value that can provide valid solutions at any given target position. This graph also starts to give an idea of the overall total workspace of the parallel robot if rotation of the platform is not critical.

4. R ESEARCH E SSAY
4.1. S AFE W ORKING IN R OBOTIC E NVIROMENTS
Working and interacting with robots can be very dangerous in certain situations. Robotic systems are often heavy and powerful, they can do large amounts of damage to users and equipment if something goes wrong. Within industry, robotic and automated systems are usually kept safe by keeping them separate from human workers. This means that any faults that occur cannot hurt other workers. Whilst effective, This system is also very limiting. It means that these robots can never operate whilst in close proximity to humans and cannot do co-operative tasks with humans. As robots are becoming more common in places other than factorys it has become apparent that robots require the ability for safe physical Human Robot Interaction (HRI). Currently there are 2 main areas of research on HRI systems [De Santis(2008)].

4.2. C HRI
This describes Human Robot Interaction on a cognitive level. This is the principle that the robot will contain accurate mental models of itself, its surroundings and objects that it may be interacting with, allowing for fast and dynamic reactive planing [Jung et al.(2007)Jung, Choi, Park, Shin, and Myaeng]. This type of system would mean that a robot would be able to predict collisions with humans or equipment before they happen and take preventative measures to avoid accidents happening. However the problem with this system is that if the robot has not noticed a threat it may still perform a dangerous action, which could result in damage or injury. These types of systems can also require large amounts of processing power.

10031755

Queron Williams

14

4.3. P HRI
This describes Human Robot Interaction on a physical level. pHRI systems focus on making robots safer in the event that a collision does occur. This is often achieved by using lightweight materials and making joints more compliant [De Luca and Flacco(2012)]. Some systems also measure force or torque in the joints of the robot to detect collisions and react [Doisy(2012)]. This has become far more common in service robots as it guarantees safe operation and interaction. Passive compliance within the joins of the robot also has the added benet that they will continue to be safe, even in events where the robot becomes un-responsive or has a technical fault.

4.4. HRI IN SURGICAL ROBOTICS

Within surgical robotics the need for safe, reliable, compliant robotics is even more important. Robots are known to enhance surgery by improving precision, repeatability, stability, and dexterity however they can often be in delicate environments where a small scrape or puncture could cause major complications for a patient. Currently problems with compliance of surgical robots is due to difcultys associated with getting accurate sensory feedback at the scale required by these types of robot.

5. C ONCLUSION
5.1. I NVERSE K INEMATICS
I think that my chosen method for solving the IK was probably not the simplest method, however it was chosen because of past experience with the method. I feel that this increased understanding has left me with a simpler nal solution which was easily implemented in code (even if it took longer to derive). Due to the fault tolerance of my IK function, my nal IK model provided accurate and reliable solutions.

5.2. W ORKSPACE
My main program was very simple which helped allot whilst implementing features for plotting the workspace. I was able to produce high quality plots of xed rotation angles as well as plots representing multiple angles. Key features of the workspace were clearly visible in the plots and I learnt allot about how the parallel robot moves from changing variables and re-rendering the workspace. In past assignments I have used Forward Kinematics to plot the workspace however the parallel robot required that the workspace was calculated using the Inverse Kinematics. After completing these workspaces I favour the method using IK. Once I had developed functions for checking solutions this method was much faster and I prefer the plots it produced, they are much clearer and more detailed.

5.3. R ESEARCH E SSAY

Whilst researching for this essay I read papers in an area that I am not familiar with and have not done much past research in. I found most of the papers were very interesting and i feel that i learnt design concepts that I will be able to apply to my own work in the future. After reading about different HRI ideas and concepts, I think that robots of the future will need to have both cognitive understanding of situations and there environment as well as the many physical systems that designers are including within hardware. In order to be procient in completing co-operative tasks robots need to have the cognitive understanding however this alone is not enough to protect users in all situations.

5.4. O VERALL
I feel that this report demonstrates a good understanding of the theory used in solving the IK for the planar parallel robot. I also think that it shows a well rounded knowledge about robot workspaces and what information they can provide.

10031755

Queron Williams

15

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 format SHORT % %% % % % % % % % % % % Variables % % % % % % % % % % % % % % % % % % % % % % % % % % % % % Globals % % % % % % % % % % % % % % % global l 1 l 2 global s o l u t i o ns

% % % % % % % % % % % Program Options % % % % % % % % % % % doIK = 0 ; plotWorkspace = 1 ; % % % % % % % % % Ajustable Variables % % % % % % % % % l 1 = 170 ; %l i n k 1 length ( base to f i r s t j o i n t ) l 2 = 130 ; %l i n k 2 length ( f i r s t j o i n t to platform ) baseRadius = 290; %radius of base platformRadius = 130; %radius of platform platformX = 60 ; %x o f f s e t of end e f e c t o r 30 platformY = 6 0; %y o f f s e t of end e f e c t o r 2 platformR = 30 ; %r o t a t i o n of end e f e c t o r 30 % % % % % % % % % I n t e r n a l Variables % % % % % % % % % % legX = zeros ( 1 , 3 ) ; legY = zeros ( 1 , 3 ) ; legLinkX1 = zeros ( 1 , 3 ) ; legLinkY1 = zeros ( 1 , 3 ) ; legLinkX2 = zeros ( 1 , 3 ) ; legLinkY2 = zeros ( 1 , 3 ) ; legTargetX = zeros ( 1 , 3 ) ; legTargetY = zeros ( 1 , 3 ) ; % %% % % % % % % % % % % % % Main % % % % % % % % % % % % % % % % % hold on daspect ( [ 1 1 1 ] ) % a l l a x i s scaled the same x l a b e l ( X (m m) , FontSize , 1 6 ) y l a b e l ( Y (m m) , FontSize , 1 6 ) f o r i = 1 : 1 : 3 , %f o r each corner of base t r i a n g l e , c a l c u l a t e position . angle = ( ( i * 120) ) ; % 120 * between corners of a t r i a n g l e legX ( i ) = baseRadius * sind ( angle ) ;

10031755

Queron Williams

16

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

legY ( i ) = baseRadius * cosd ( angle ) ; plot ( legX ( i ) , legY ( i ) , r+ ) ; %plot each point end l i n e ( [ legX ( 1 ) ; legX ( 2 ) ] , [ legY ( 1 ) ; legY ( 2 ) ] ) ; %j o i n base points l i n e ( [ legX ( 2 ) ; legX ( 3 ) ] , [ legY ( 2 ) ; legY ( 3 ) ] ) ; l i n e ( [ legX ( 3 ) ; legX ( 1 ) ] , [ legY ( 3 ) ; legY ( 1 ) ] ) ; % % % % % % % % % Inverse Kinematics % % % % % % % % % % i f ( doIK == 1) f o r i = 1 : 1 : 3 , %f o r each corner of platform , c a l c u l a t e position angle = ( ( i * 120) platformR ) ; %acount f o r angular r o t a t i o n legTargetX ( i ) = ( platformRadius * sind ( angle ) ) + platformX ; legTargetY ( i ) = ( platformRadius * cosd ( angle ) ) + platformY ; plot ( legTargetX ( i ) , legTargetY ( i ) , rx ) ; %plot each point end l i n e ( [ legTargetX ( 1 ) ; legTargetX ( 2 ) ] , [ legTargetY ( 1 ) ; legTargetY ( 2 ) ] ) ; l i n e ( [ legTargetX ( 2 ) ; legTargetX ( 3 ) ] , [ legTargetY ( 2 ) ; legTargetY ( 3 ) ] ) ; l i n e ( [ legTargetX ( 3 ) ; legTargetX ( 1 ) ] , [ legTargetY ( 3 ) ; legTargetY ( 1 ) ] ) ; e r r o r = 0 ; %make sure e r r o r f l a g i s r e s e t %p r i n t debug info f p r i n t f ( I f the platform i s at X=%d and Y=%d , platformX , platformY ) f p r i n t f ( with a r o t a t i o n angle of %d degrees : \ n , platformR ) f o r i = 1 : 1 : 3 , %f o r each l e g t a r g e t = [ legX ( i ) ; %from base x position legY ( i ) ; %from base Y position legTargetX ( i ) ; %to platfrom x position legTargetY ( i ) ] ; %to platfrom y position i f ( IK ( t a r g e t ) == 1) % i f soloution f o r l e g i s posible Theta1a Theta2a Theta1b Theta2b = = = = solu tio ns ( 1 , 1 ) ; solu tio ns ( 2 , 1 ) ; solu ti ons ( 1 , 2 ) ; solu ti ons ( 2 , 2 ) ; %s e t %s e t %s e t %s e t theta theta theta theta 1 2 1 2 for for for for soloution soloution soloution soloution 1 1 2 2

%c a l c u l a t e x , y position f o r j o i n t in soloution 1 angle = Theta1a ; legLinkX1 ( i ) = ( l 1 * cosd ( angle ) ) + legX ( i ) ; legLinkY1 ( i ) = ( l 1 * sind ( angle ) ) + legY ( i ) ; %c a l c u l a t e x , y position f o r j o i n t in soloution 2 angle = Theta1b ; legLinkX2 ( i ) = ( l 1 * cosd ( angle ) ) + legX ( i ) ; legLinkY2 ( i ) = ( l 1 * sind ( angle ) ) + legY ( i ) ; %p r i n t debug info i f ( i == 1)

10031755

Queron Williams

17

102

f p r i n t f ( \ tLeg %d , ( bottom l e f t ) : \ n , i ) % i f soloution f o r l e g i s posible end i f ( i == 2) f p r i n t f ( \ tLeg %d , ( bottom r i g h t ) : \ n , i ) % i f soloution f o r l e g i s posible end i f ( i == 3) f p r i n t f ( \ tLeg %d , ( top center ) : \ n , i ) % i f soloution f o r l e g i s posible end f p r i n t f ( \ t \ tSolution 1 ( red ) : \ tTheta=%.2 f \ t P s i =%.2 f \n , Theta1a , Theta2a ) f p r i n t f ( \ t \ tSolution 2 ( pink ) : \ tTheta=%.2 f \ t P s i =%.2 f \n , Theta1b , Theta2b ) e l s e % i f soloution f o r l e g i s NOT posible e r r o r = 1 ; %s e t e r r o r f l a g end end i f ( e r r o r == 0) % i f e r r o r f l a g i s c l e a r for i =1:1:3 , %draw both soloutions fo each l e g OOx = [ legX ( i ) ; legLinkX1 ( i ) ; legTargetX ( i ) ] ; OOy = [ legY ( i ) ; legLinkY1 ( i ) ; legTargetY ( i ) ] ; hold on plot (OOx,OOy, r , LineWidth , 2 , Marker , o ) ; OOx = [ legX ( i ) ; legLinkX2 ( i ) ; legTargetX ( i ) ] ; OOy = [ legY ( i ) ; legLinkY2 ( i ) ; legTargetY ( i ) ] ; hold on plot (OOx,OOy, m , LineWidth , 2 , Marker , o ) ; end else % i f error flag i s set print error f p r i n t f ( \n ! ERROR: IK t a r g e t out of bounds ! \ n ) end end % % % % % % % % % % % % % % Workspace % % % % % % % % % % % % % % i f ( plotWorkspace == 1) X = [ ] ; %declare arrays f o r s c a t t e r plot Y = []; Z = []; C = []; h = waitbar ( 0 , Please wait . . . ) ;%i n i t i a l i s e waitbar f o r x = 180:1:180 , %f o r each posible x position f o r y = 180:1:180 , %f o r each posible y position platformX = x ; platformY = y ; zTemp=0; f o r z =00:10:10 , %f o r each posible r o t a t i o n f o r i = 1 : 1 : 3 , %c a l c u l a t e platform edge points angle = ( ( i * 120)+ z ) ;

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

10031755

Queron Williams

18

151 152 153 154 155 156 157 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

legTargetX ( i ) = ( platformRadius * sind ( angle ) ) + platformX ; legTargetY ( i ) = ( platformRadius * cosd ( angle ) ) + platformY ; end e r r o r = 0 ; %r e s e t e r r o r f l a g f o r i = 1 : 1 : 3 ,%check i f each l e g provides v a l i d soloution t a r g e t = [ legX ( i ) ; legY ( i ) ; legTargetX ( i ) ; legTargetY ( i ) ] ; i f ( IK ( t a r g e t ) == 0) %i f no v a l i d soloution e r r o r = 1 ; %s e t e r r o r f l a g end end i f ( e r r o r == 0) zTemp = zTemp+1; %colour based on posible soloutions %zTemp = z +90; %colour based on r o t a t i o n end end i f zTemp >=1 X( end+1) Y ( end+1) Z( end+1) C( end+1) end end waitbar ( ( x +181) /361 ,h) ; %update the waitbar end f i g u r e ( 1 ) ; %make sure your rendering on f i g u r e 1 s c a t t e r 3 (X , Y , Z , 3 ,C, f i l l e d , s ) ;% f u l l colour s c a t t e r plot %s c a t t e r 3 (X , Y , Z , 3 , b , f i l l e d , s ) ; colormap ( j e t ) ; %make c o l o u r f u l l hold on end % %% % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % %i f there were v a l i d soloutions , create data point = platformX ; = platformY ; = zTemp ; %s e t zheight based on no of soloutions = zTemp ; %s e t colour based on no of soloutions

10031755

Queron Williams

19

B. MATLAB C ODE - IK. 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

function [ s t a t e ] = IK ( t a r g e t ) % perform IK c a l c u l a t i o n s global l 1 l 2 global s o l u ti ons target ; x = t a r g e t ( 3 , 1 ) t a r g e t ( 1 , 1 ) ;%s e t t a r g e t s y = t a r g e t ( 4 , 1 ) t a r g e t ( 2 , 1 ) ;%s e t t a r g e t s cosT2 = ( ( x ^2) +( y ^2) ( l 1 ^2) ( l 2 ^2) ) / ( 2 * l 1 * l 2 ) ; i f ( x^2+y^2>=( l 1 + l 2 ) ^2) %check i f v a l i d soloution e x i s i t s s o l u ti o ns = [ 0 , 0 ; %i f no soloution return zeros 0 ,0]; % f p r i n t f ( \ n ! ERROR: IK t a r g e t out of bounds ! \ n ) s t a t e = 0 ; %c l e a r s u c e s s f u l f l a g return e l s e i f ( cosT2< 1) %check i f out of bounds s o l u ti o ns = [ 0 , 0 ;%i f no soloution return zeros 0 ,0]; s t a t e = 0 ;%c l e a r s u c e s s f u l f l a g return end s2a=+ s q r t (1 (cosT2 ^2) ) ; s2b= s q r t (1 (cosT2 ^2) ) ; t2a = atan2d ( s2a , cosT2 ) ; %theta 2 t2b = atan2d ( s2b , cosT2 ) ; t1a = atan2d( l 2 * s2a * x + ( l 1 + l 2 * cosT2 ) * y , ( l 1 + l 2 * cosT2 ) * x + l 2 * s2a * y ) ; t1b = atan2d( l 2 * s2b * x + ( l 1 + l 2 * cosT2 ) * y , ( l 1 + l 2 * cosT2 ) * x + l 2 * s2b * y ) ; s o l u ti o ns = [ t1a , t1b ; %i f s u c e s s f u l return soloutions t2a , t2b ] ; state = 1; %s e t f l a g f o r s u c e s s f u l l soloutions end

10031755

Queron Williams

20

Bibliography

R EFERENCES
[De Santis(2008)] Agostino De Santis. Modelling and control for humanrobot interaction. PhD thesis, Universit degli Studi di Napoli Federico II, 2008. [Jung et al.(2007)Jung, Choi, Park, Shin, and Myaeng] Yuchul Jung, Yoonjung Choi, Hugun Park, Wookhyun Shin, and Sung-Hyon Myaeng. Integrating robot task scripts with a cognitive architecture for cognitive humanrobot interactions. In Information Reuse and Integration, 2007. IRI 2007. IEEE International Conference on, pages 152157, 2007. doi: 10.1109/IRI.2007.4296613. [De Luca and Flacco(2012)] A. De Luca and F. Flacco. Integrated control for phri: Collision avoidance, detection, reaction and collaboration. In Biomedical Robotics and Biomechatronics (BioRob), 2012 4th IEEE RAS EMBS International Conference on, pages 288295, 2012. doi: 10.1109/BioRob.2012.6290917. [Doisy(2012)] G. Doisy. Sensorless collision detection and control by physical interaction for wheeled mobile robots. In Human-Robot Interaction (HRI), 2012 7th ACM/IEEE International Conference on, pages 121122, 2012.

10031755

Queron Williams

21