ROBOTS MECHANICS Coursework2: Parallel Manipulator Kinematics

8/13/2019 ROBOTS MECHANICS Coursework2: Parallel Manipulator Kinematics

1/21

UFMF5P-20-3: ROBOTSM ECHANICS

Coursework 2: Parallel Manipulator Kinematics

Queron Williams - 10031755

December 12, 2013

1. INTRODUCTION

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. INVERSEKINEMATICS (IK)

2.1. KINEMATIC

STRUCTURE

The planar parallel robot has 3 Degrees Of Freedom (X, Y and ). As shown in figure 2.1 the robot consists of a

fixed 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 Bi will be 290mm from the center of the base (CB) and each P Piwill 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 thetranslation 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 1


2/21

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

2.2. SOLVINGI NVERSE KINEMATICS

In order to solve the IK we will start by finding the Cartesian position ofP Bi andP Pi for each leg (where i=1:3).

These positions will then be used to find the distance between P Bi andP Pi, then solve ibefore finaly solving

theifor each leg.

2.2.1. FINDINGP OSITION OFP Bi

Finding the Cartesian position ofP Bi is simply found using sin and cos as shown in figure 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 find the

position ofP Biwe then useP BiX=baseRadius*cos() andP BiY=baseRadius*sin()



3/21


4/21

From this point we must solve each set of links individually. For ease of notation we will letti=P Pi-P Bi, making

tithe position ofP Pirelative toP Bi.

2.2.4. FINDING

To find thetxvalue we need only sum the lengths of the adjacent sides of the two right-angled triangles formed by

l1&andl2&. Fortyit is the same but using the opposite sides.

tx= l2c+ l1c (2.1)

ty= l2s+ l1s (2.2)

Wherec = cos,s = si n,c = cos,s = si n,c = co s(+),s = si n(+). The soloution to can be

computed from summation of squaring both equations 2.1 and 2.2

t2

x= l2

2c2

+ l2

1c2

+2l2

l1

cc (2.3)

t2y= l22 s

2

+ l21 s2+2l2l1ss (2.4)

t2x+ t2

y= l22 (c

2

+ s2

)+ l21 (c2+ s2

)+2l2l1(cc+ ss) (2.5)

Becausec2i+ s2i= 1, equation 2.5 can be rearanged and simplified as follows:

t2x+ t2

y= l22 + l

21 +2l2l1(cc+ ss) (2.6)

t2x+ t2

y= l22 + l

21 +2l2l1(c(+)c+ s(+)s) (2.7)

t2x+ t2y= l22 + l21 +2l2l1(1/2(c(2+)+c)+1/2(cc(2+))) (2.8)

t2x+ t2

y= l22 + l

21 +2l2l1c (2.9)

Rearranged forc:

c =l21 l

22 + t

2x+ t

2y

2l1l2(2.10)

The acos() function can cause inaccuracies at small values of, to get around this problem we will use atan2() this

requires sandcbut sine we knowc2i+ s

2i= 1 we can simply rearrange to gets.

s =

1

l21 l

22 + t

2x+ t

2y

2l1l2

2(2.11)

The final atan2() solution can now be created, note that the in the formula will allow us to get both possible

solutions for this joint.

= At an21l

2

1

l2

2

+ t2x+ t2

y

2l1l2

2,

l21 l22 + t

2x+ t

2y

2l1l2

(2.12)



5/21

2.2.5. FINDING

Now that we have, we can use equation 2.1 and 2.2 to calculate. To do this we multiply equation 2.1 bycand the equation 2.2 bys

ctx= l

2c(+)c+ l

1c2

(2.13)

sty= l2s(+)s+ l1s2

(2.14)

ctx+ sty= l2(c(+)c+ s(+)s)+ l1(c2+ s2

) (2.15)

ctx+ sty= l2(1/2(c(2+)+c)+1/2(cc(2+)))+ l1(c2+ s2

) (2.16)

ctx+ sty= l2c+ l1(c2+ s2

) (2.17)

ctx+ sty= l2c+ l1 (2.18)

We repeat this process but this time multiply equation 2.1 bysand the equation 2.2 byc

stx= l2c(+)s l1cs (2.19)

cty= l2s(+)c+ l1sc (2.20)

stx+cty= l2s(+)c l2c(+)s (2.21)

stx+cty= l2s (2.22)

Now we multiply both sides of equation 2.18 bys, and both sides of equation 2.22 byc: before adding the

resultant equations:

ct2x+ stytx= tx(l2c+ l1) (2.23)

stxty+ct2

y= l2sty (2.24)

c(t2x+ t

2y)= tx(l2c+ l1)+ l2sty (2.25)

This is then rearranged forc:

c =tx(l2c+ l1)+ l2sty

t2x+ t2

y

(2.26)

We knowc2i+ s2

i= 1 so we can rearrange to gets:

s =

1

tx(l2c+ l1)+ l2sty

t2x+ t2

y

2(2.27)

This can be used to give us the final atan2 solution for , as before the in the formula will allow us to get both

possible solutions for this joint.

=At an21 tx(l2c+ l1)+ l2styt2x+ t2y

2

,tx(l2c+ l1)+ l2sty

t2x+ t2

y

(2.28)



6/21

2.2.6. FINDING

This Process must be repeated for each set of links using the respectiveP Bi andP Pivalues to get each set ofiandivalues.

2.3. IMPLEMENTING INMATL AB

In order to implement this within Matlab I first use the desired X, Y and values to generate the X,Y positions ofP Bi andP Pifor 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 er-

rors 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

Each of the angles provided in the solution can be checked against the produced drawing of the robot to verify

that it is correct.



7/21

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. firstley i will set X=60.


The target position shown in Figure 2.3 gave me the following solution in the terminal.



8/21

Next I will set Y=60.


The target position shown gave me the following solution in the terminal.



9/21

Next I will set =30.





10/21

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


It can be seen that through all of these tests the solutions remain accurate.

3. PAR AL LE LROBOTWORKSPACE

3.1. PLOTTING THEWORKSPACE

There are 2 main ways of plotting a robot workspace. The first 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 thenchange my input variables to match each given target position and check if this position has a valid IK solution for



11/21

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 final product of this process is shown in Figure 3.2 where the rotation of the platform has been locked.

3.2. WORKSAPCE ANALYSIS

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 Pican never reach back to P Bieven ifiis 180 degrees, causing

a small area where the platform cannot reach near each P Bi.



12/21

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 specific angle in order to be reached by the

platform.

Figure 3.3: Workspace with =10 Figure 3.4: Workspace with=20



13/21




When we begin to vary the value for the platform we can see that the workspace begins to shrink. This is be-

cause the distance betweenP BiandP Piis increased when at the same target position, meaning it can travel less

distance before it reaches its limits.



14/21

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 possiblevalue 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. RESEARCHE SSAY

4.1. SAF EWORKING INROBOTICENVIROMENTS

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. CHRI

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.



15/21

4.3. PHR I

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 benefit 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 del-

icate environments where a small scrape or puncture could cause major complications for a patient. Currently

problems with compliance of surgical robots is due to difficultys associated with getting accurate sensory feed-

back at the scale required by these types of robot.

5. CONCLUSION

5.1. INVERSE KINEMATICS

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

final solution which was easily implemented in code (even if it took longer to derive). Due to the fault tolerance of

my IK function, my final IK model provided accurate and reliable solutions.

5.2. WORKSPACE

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 fixed rotation angles as well as plots representing multiple angles. Keyfeatures 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. RESEARCHESSAY

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 proficient 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. OVE RA LL

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.



16/21

A.

MATLAB CODE- MAIN.M

1 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

2 %%%%%%%%%%% Queron Williams %%%%%%%%%%%

3 %%%%%%%%%% Robotic Mechanics %%%%%%%%%%

4 %%%%%%%%%%%% October 2013 %%%%%%%%%%%%%

5 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

6 %% f i r s t c l e a r e v er yt h in g

7 c l c

8 c l ea r a l l

9 c l os e a l l

10 format SHORT

11

12 %% %%%%%%%%%%% Va riab l es %%%%%%%%%%%%%%

13 %%%%%%%%%%%%%%% Globals %%%%%%%%%%%%%%%

14 g lobal l 1 l 2

15 g lobal s o l u t i o n s

16

17

18 %%%%%%%%%%% Program Options %%%%%%%%%%%

19 d oIK = 0 ;

20 plotWorkspace = 1;

21

22 %%%%%%%%% A ju stab le V ariab l e s %%%%%%%%%

23 l 1 = 170 ; %l i nk 1 l en gt h ( base t o f i r s t j o i n t )

24 l 2 = 130 ; %l i n k 2 l en gt h ( f i r s t j o i n t t o p la tf or m )

25

26 baseRadius = 290; %radius of base

27 platformRadius = 130; %radius of platform

28 platformX = 60; %x o f f s e t o f end e f e ct o r 3 0

29 platformY = 60; %y o f f s e t o f end e f e c t o r 2

30 platformR = 30; %r o t a t i o n o f end e f e c t o r 30

31

32 %%%%%%%%% I n t e r n a l V ari ab le s %%%%%%%%%%

33 l e g X = zeros (1 ,3) ;

34 l eg Y = zeros (1 ,3) ;

35 legLinkX1 = zeros (1 ,3) ;

36 legLinkY1 = zeros (1 ,3) ;

37 legLinkX2 = zeros (1 ,3) ;

38 legLinkY2 = zeros (1 ,3) ;

39 legTargetX = zeros (1 ,3) ;

40 legTargetY = zeros (1 ,3) ;

41

42 %% %%%%%%%%%%%%% Main %%%%%%%%%%%%%%%%%

43 hold on

44 d as pe ct ( [ 1 1 1 ] ) % a l l a x i s s c al e d t he same

45 x l a b e l( X (mm) , FontSize , 1 6 )

46 y l a b e l( Y (mm) , FontSize , 1 6 )

47 f o r i =1: 1:3 , %f o r each c or ne r o f b as e t r i a n g l e , c a l c u l a t e p o s it i o n .

48 angle = ( ( i *120) ) ; % 120* between c o rn e rs o f a t r i a n g l e

49 legX ( i ) = baseRadius* s i n d ( angle ) ;



17/21

50 legY ( i ) = baseRadius * cosd( angle ) ;

51 plot ( legX( i ) , legY( i ) , r + ) ; %plot each point

52 end

53 l i n e( [ l e gX ( 1 ) ; l e gX ( 2 ) ] , [ l e gY ( 1 ) ; l e gY ( 2 ) ] ) ; %join base points

54 l i n e( [ l e gX ( 2 ) ; l e gX ( 3 ) ] , [ l e gY ( 2 ) ; l e gY ( 3 ) ] ) ;

55 l i n e( [ l e gX ( 3 ) ; l e gX ( 1 ) ] , [ l e gY ( 3 ) ; l e gY ( 1 ) ] ) ;

56

57 %%%%%%%%% Inverse Kinematics %%%%%%%%%%

58

59 i f(doIK == 1)

60

61 f o r i =1: 1:3 , %f o r e ach c or ne r o f p la tf or m , c a l c u l a t e p o s i t i o n

62 angle = ( ( i *120) platformR) ; %acount for angular rotation

63 legTargetX ( i ) = ( platformRadius * s i n d ( angle ) ) + platformX ;

64 legTargetY ( i ) = ( platformRadius * cosd( angle ) ) + platformY ;

65 p l o t ( legTargetX ( i ) , legTargetY ( i ) , r x ) ; %plot each point

66 end

67 l i n e( [ l e g Ta r ge t X ( 1 ) ; l e gT a rg e tX ( 2 ) ] , [ l eg T a rg e t Y ( 1 ) ; l e g Ta r ge t Y ( 2 ) ] ) ;68 l i n e( [ l e g Ta r ge t X ( 2 ) ; l e gT a rg e tX ( 3 ) ] , [ l eg T a rg e t Y ( 2 ) ; l e g Ta r ge t Y ( 3 ) ] ) ;

69 l i n e( [ l e g Ta r ge t X ( 3 ) ; l e gT a rg e tX ( 1 ) ] , [ l eg T a rg e t Y ( 3 ) ; l e g Ta r ge t Y ( 1 ) ] ) ;

70

71 e r r o r = 0 ; %make s u re e r r or f l a g i s r e s e t

72

73 %print debug info

74 f p r i n t f ( I f the platform is at X=%d and Y=%d , platformX , platformY)

75 f p r i n t f ( w it h a r o t a ti o n a ng le o f %d d eg re es : \ n , platformR )

76

77 f o r i =1: 1:3 , %f o r e ach l e g

78 t a r g e t = [ l eg X ( i ) ; %from base x position79 legY ( i ) ; %from base Y posit ion

80 legTargetX ( i ) ; %t o p l at f ro m x p o s i t i on

81 legTargetY ( i ) ] ; %t o p l at f ro m y p o s i t i o n

82

83 i f ( IK ( t a r g e t ) == 1 ) % i f s ol ou ti on f o r l eg i s p os ib le

84

85 Theta1a = solution s (1 ,1) ; %s e t t he ta 1 f o r s ol ou ti on 1

86 Theta2a = solution s (2 ,1) ; %s e t t he ta 2 f o r s ol ou ti on 1

87 Theta1b = solution s (1 ,2) ; %s e t t he ta 1 f o r s ol ou ti on 2

88 Theta2b = solution s (2 ,2) ; %s e t t he ta 2 f o r s ol ou ti on 2

89

90 %c a l cu l a te x , y p o si ti o n f o r j o i n t i n s ol ou ti on 1

91 angle = Theta1a ;

92 legLinkX1 ( i ) = ( l1 * cosd( angle ) ) + legX( i ) ;

93 legLinkY1 ( i ) = ( l1 * s i n d ( angle ) ) + l e gY ( i ) ;

94

95 %c a l cu l a te x , y p o si ti o n f o r j o i n t i n s ol ou ti on 2

96 angle = Theta1b ;

97 legLinkX2 ( i ) = ( l1 * cosd( angle ) ) + legX( i ) ;

98 legLinkY2 ( i ) = ( l1 * s i n d ( angle ) ) + l e gY ( i ) ;

99

100 %print debug info101 i f( i == 1 )



18/21

102 f p r i n t f ( \ t L eg %d , ( bottom l e f t ) : \ n , i ) % i f s ol ou ti on f or l eg i s

posible

103 end

104 i f( i == 2 )

105 f p r i n t f ( \ t L eg %d , ( bottom r i g h t ) : \ n , i ) % i f s ol ou tio n f or l eg i s

posible

106 end107 i f( i == 3 )

108 f p r i n t f ( \ t L eg %d , ( t op c e nt er ) : \ n , i ) % i f s ol ou ti on f o r l eg i s

posible

109 end

110 f p r i n t f ( \ t \ t S ol u ti o n 1 ( r ed ) : \ t Th et a=%.2 f \ t P s i = %.2 f \n , Theta1a , Theta2a)

111 f p r i n t f ( \ t \ t S o l ut i o n 2 ( p in k ) : \ t Th et a=%.2 f \ t P s i = %.2 f \n ,Theta1b , Theta2b )

112 e l s e % i f s o lo u ti o n f o r l e g i s NOT p os i bl e

113 e r r o r = 1 ; %s e t e r ro r f l a g

114 end

115 end

116

117 i f(e r r o r = = 0 ) % i f e rro r f la g i s c le ar

118 f o r i =1: 1:3 , %draw both soloutions fo each leg

119 OOx = [ le g X ( i ) ; l e gL i nk X 1 ( i ) ; l e g Ta r ge t X ( i ) ] ;

120 OOy = [ l e gY ( i ) ; l e gL i nk Y1 ( i ) ; l e g Ta r ge t Y ( i ) ] ;

121 hold on

122 p l o t (OOx,OOy, r , LineWidth , 2 , Marker , o ) ;

123 OOx = [ le g X ( i ) ; l e gL i nk X 2 ( i ) ; l e g Ta r ge t X ( i ) ] ;

124 OOy = [ l e gY ( i ) ; l e gL i nk Y2 ( i ) ; l e g Ta r ge t Y ( i ) ] ;

125 hold on

126 p l o t (OOx,OOy,m, LineWidth , 2 , Marker , o ) ;

127 end128 e l s e % i f e rr or f l a g i s s et p ri nt e rr or

129 f p r i n t f ( \ n ! ERROR : I K t a r g e t o ut o f bounds ! \ n )

130 end

131 end

132

133 %%%%%%%%%%%%%% Workspace %%%%%%%%%%%%%%

134

135 i f( plotWorkspace == 1)

136 X = [ ] ; %d e cl a re a r ra y s f o r s c a t t e r p l o t

137 Y = [ ] ;

138 Z = [ ] ;139 C = [ ] ;

140

141 h = w a it b ar ( 0 , Please wait . . . ) ;%i n i t i a l i s e w ai tb ar

142 f o r x =180:1:180, %f o r e ach p o s i b le x p o s i t i on

143 f o r y =180:1:180, %f o r eac h p o s i b l e y p o s i t i o n

144 p la tf or mX = x ;

145 p la tf or mY = y ;

146 zTemp=0;

147

148 f o r z = 0 0 : 1 0 : 1 0 , %f o r e ach p o s i b le r o t a t i o n

149 f o r i =1: 1:3 , %calcu late platform edge points150 angle = ( ( i *1 2 0 ) + z ) ;



19/21

151 legTargetX( i ) = ( platformRadius * s i n d ( angle ) ) + platformX ;

152 legTargetY ( i ) = ( platformRadius * cosd( angle ) ) + platformY ;

153 end

154 e r r o r = 0 ; %r e s et e r ro r f l a g

155 f o r i =1: 1:3 ,%c he ck i f e ach l e g p r ov i de s v a l i d s o l ou t i o n

156 t a r g e t = [ l eg X ( i ) ;

157 legY ( i ) ;158 legTargetX ( i ) ;

159 legTargetY ( i ) ] ;

160

161 i f( I K ( t a r g e t ) == 0 ) % i f no v a l i d s o lo u ti o n

162 e r r o r = 1 ; %s e t e r r o r f l a g

163 end

164 end

165 i f(e r r o r == 0 )

166 zTemp = zTemp+ 1; %colour based on posible soloutions

167 %zTemp = z+9 0; %colour based on rot ati on

168 end169 end

170

171 i f zTemp >=1 %i f t h er e were v a l i d s o lo ut i on s , c r ea t e d at a p oi nt

172 X( end+1) = platformX ;

173 Y ( end+1) = platformY ;

174 Z( end+1) = zTemp; %s e t z h ei g h t b ase d on no o f s o l o u ti o n s

175 C( end+1) = zTemp; %s e t c o lo u r b as ed on no o f s o l o ut i o n s

176

177 end

178 end

179 waitbar (( x+181)/361,h) ; %update the waitbar180 end

181 f i g u r e(1) ; %make sure your rendering on fig ure 1

182 s c a t t e r 3 ( X , Y , Z , 3 , C , f i l l e d , s ) ;% f u l l c ol our s c at t e r p lo t

183 %s c a t t e r 3 ( X , Y , Z , 3 , b , f i l l e d , s ) ;

184 colormap (j e t) ; %make co lo ur fu ll

185 hold on

186 end

187

188 %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%



20/21

B.

MATLAB COD E- IK.M

1 function [ s ta te ] = IK ( t ar ge t ) % p erf orm I K c a l c u l a t i o n s

2 g lobal l 1 l 2

3 g lobal s o l u t i o n s

4

5 t a r g e t ;

6

7 x = t a r g e t ( 3 , 1 )targ et (1 ,1) ;%s e t t a r g e t s

8 y = t a r g e t ( 4 , 1 )targ et (2 ,1) ;%s e t t a r g e t s

9

10 co sT2 = ( ( x ^ 2 ) + ( y ^2 )( l 1 ^ 2 )( l 2 ^ 2 ) ) / ( 2 *l 1 * l2 ) ;

11

12 i f(x^2+y^2>=(l1 +l 2 ) ^2) %check i f v a l id s ol ou ti on e x i s i t s

13

14 s o lu t io n s = [ 0 , 0 ; %i f no s o l ou t i on r e t ur n z e ro s

15 0 , 0 ] ;

16

17 % f p r in t f ( \n ! ERROR: IK t ar g e t out of bounds ! \ n )

18 s t at e = 0 ; %c l e ar s u ce s sf u l f l a g

19 return

20 e ls e i f (cosT2


21/21

Bibliography

REFERENCES

[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 human-

robot interactions. InInformation 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. InBiomedical 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. InHuman-Robot Interaction (HRI), 2012 7th ACM/IEEE International Conference on, pages 121122,

2012.

ROBOTS MECHANICS Coursework2: Parallel Manipulator Kinematics

Documents