Matrici di Rotazione

C O N T R O L L O D E I R O B O T P r o f . B r u n o S I C I L I A N O

C I N E M A T I C A

r e l a z i o n i t r a p o s i z i o n i d e i g i u n t i e p o s i z i o n e e o r i e n t a m e n t o

d e l l o r g a n o t e r m i n a l e

M a t r i c e d i r o t a z i o n e

R a p p r e s e n t a z i o n i d e l l o r i e n t a m e n t o

T r a s f o r m a z i o n i o m o g e n e e

C i n e m a t i c a d i r e t t a

S p a z i o d e i g i u n t i e s p a z i o o p e r a t i v o

C a l i b r a z i o n e c i n e m a t i c a

P r o b l e m a c i n e m a t i c o i n v e r s o


P O S A D I U N C O R P O R I G I D O

P o s i z i o n e

o

=

o

x

o

y

o

z

O r i e n t a m e n t o

x

= x

x

x + x

y

y + x

z

z

y

= y

x

x + y

y

y + y

z

z

z

= z

x

x + z

y

y + z

z

z


M A T R I C E D I R O T A Z I O N E

R =

x

y

z

=

x

T

x y

T

x z

T

x

x

T

y y

T

y z

T

y

x

T

z y

T

z z

T

z

R

T

R = I

R

T

= R

1


R o t a z i o n i e l e m e n t a r i

r o t a z i o n e d i i n t o r n o a z

R

z

( ) =

c o s s i n 0

s i n c o s 0

0 0 1


r o t a z i o n e d i i n t o r n o a y

R

y

( ) =

c o s 0 s i n

0 1 0

s i n 0 c o s

r o t a z i o n e d i i n t o r n o a x

R

x

( ) =

1 0 0

0 c o s s i n

0 s i n c o s


R a p p r e s e n t a z i o n e d i u n v e t t o r e

p =

p

x

p

y

p

z

p

=

p

x

p

y

p

z

p =

x

y

z

p

= R p

p

= R

T

p


E s e m p i o

p

x

= p

x

c o s p

y

s i n

p

y

= p

x

s i n + p

y

c o s

p

z

= p

z


R o t a z i o n e d i u n v e t t o r e

p = R p

p

T

p = p

T

R

T

R p

E s e m p i o

p

x

= p

x

c o s p

y

s i n

p

y

= p

x

s i n + p

y

c o s

p

z

= p

z

p = R

z

( ) p


M a t r i c e d i r o t a z i o n e

f o r n i s c e l o r i e n t a m e n t o d i u n a t e r n a d i c o o r d i n a t e r i s p e t t o

a d u n a l t r a : i v e t t o r i c o l o n n a s o n o i c o s e n i d i r e t t o r i d e g l i

a s s i d e l l a t e r n a r u o t a t a r i s p e t t o a l l a t e r n a d i p a r t e n z a

r a p p r e s e n t a u n a t r a s f o r m a z i o n e d i c o o r d i n a t e c h e m e t t e i n

r e l a z i o n e l e c o o r d i n a t e d i u n o s t e s s o p u n t o i n d u e t e r n e

d i f f e r e n t i ( d i o r i g i n e c o m u n e )

`

e l o p e r a t o r e c h e c o n s e n t e d i r u o t a r e u n v e t t o r e i n u n a s t e s s a

t e r n a d i c o o r d i n a t e


C O M P O S I Z I O N E D I M A T R I C I D I

R O T A Z I O N E

p

1

= R

1

2

p

2

p

0

= R

0

1

p

1

p

0

= R

0

2

p

2

R

j

i

= ( R

i

j

)

1

= ( R

i

j

)

T

R o t a z i o n e i n t e r n a c o r r e n t e

R

0

2

= R

0

1

R

1

2

R o t a z i o n e i n t e r n a fi s s a

R

0

2

= R

1

2

R

0

1


E s e m p i o


A N G O L I D I E U L E R O

m a t r i c e d i r o t a z i o n e

9 p a r a m e t r i c o n 6 v i n c o l i

r a p p r e s e n t a z i o n e m i n i m a d e l l o r i e n t a m e n t o

3 p a r a m e t r i i n d i p e n d e n t i


A n g o l i Z Y Z

R ( ) = R

z

( ) R

y

( ) R

z

( )

=

c

c

c

s

s

c

c

s

s

c

c

s

s

c

c

+ c

s

s

c

s

+ c

c

s

s

s

c

s

s

c


P r o b l e m a i n v e r s o

A s s e g n a t a

R =

r

1 1

r

1 2

r

1 3

r

2 1

r

2 2

r

2 3

r

3 1

r

3 2

r

3 3

i 3 a n g o l i Z Y Z s o n o ( ( 0 , ) )

= A t a n 2 ( r

2 3

, r

1 3

)

= A t a n 2

(

r

2

1 3

+ r

2

2 3

, r

3 3

)

= A t a n 2 ( r

3 2

, r

3 1

)

o v v e r o ( ( , 0 ) )

= A t a n 2 ( r

2 3

, r

1 3

)

= A t a n 2

(

r

2

1 3

+ r

2

2 3

, r

3 3

)

= A t a n 2 ( r

3 2

, r

3 1

)


A n g o l i d i R P Y

R ( ) = R

z

( ) R

y

( ) R

x

( )

=

c

c

c

s

s

s

c

c

s

c

+ s

s

s

c

s

s

s

+ c

c

s

s

c

c

s

s

c

s

c

c



A s s e g n a t a

R =

r

1 1

r

1 2

r

1 3

r

2 1

r

2 2

r

2 3

r

3 1

r

3 2

r

3 3

i 3 a n g o l i d i R P Y s o n o ( ( / 2 , / 2 ) )

= A t a n 2 ( r

2 1

, r

1 1

)

= A t a n 2

(

r

3 1

,

r

2

3 2

+ r

2

3 3

)

= A t a n 2 ( r

3 2

, r

3 3

)

o v v e r o ( ( / 2 , 3 / 2 ) )

= A t a n 2 ( r

2 1

, r

1 1

)

= A t a n 2

(

r

3 1

,

r

2

3 2

+ r

2

3 3

)

= A t a n 2 ( r

3 2

, r

3 3

)


A S S E / A N G O L O

R ( , r ) = R

z

( ) R

y

( ) R

z

( ) R

y

( ) R

z

( )

s i n =

r

y

r

2

x

+ r

2

y

c o s =

r

x

r

2

x

+ r

2

y

s i n =

r

2

x

+ r

2

y

c o s = r

z


R ( , r ) =

r

2

x

( 1 c

) + c

r

x

r

y

( 1 c

) r

z

s

r

x

r

y

( 1 c

) + r

z

s

r

2

y

( 1 c

) + c

r

x

r

z

( 1 c

) r

y

s

r

y

r

z

( 1 c

) + r

x

s

r

x

r

z

( 1 c

) + r

y

s

r

y

r

z

( 1 c

) r

x

s

r

2

z

( 1 c

) + c

R ( , r ) = R ( , r )



A s s e g n a t a

R =

r

1 1

r

1 2

r

1 3

r

2 1

r

2 2

r

2 3

r

3 1

r

3 2

r

3 3

l a n g o l o e l a s s e d i r o t a z i o n e s o n o ( s i n 6 = 0 )

= c o s

1

(

r

1 1

+ r

2 2

+ r

3 3

1

2

)

r =

1

2 s i n

r

3 2

r

2 3

r

1 3

r

3 1

r

2 1

r

1 2

c o n

r

2

x

+ r

2

y

+ r

2

z

= 1


Q U A T E R N I O N E U N I T A R I O

r a p p r e s e n t a z i o n e a 4 p a r a m e t r i Q = { , }

= c o s

2

= s i n

2

r

2

+

2

x

+

2

y

+

2

z

= 1

( , r ) e ( , r ) f o r n i s c o n o l o s t e s s o q u a t e r n i o n e

R ( , ) =

2 (

2

+

2

x

) 1 2 (

x

y

z

) 2 (

x

z

+

y

)

2 (

x

y

+

z

) 2 (

2

+

2

y

) 1 2 (

y

z

x

)

2 (

x

z

y

) 2 (

y

z

+

x

) 2 (

2

+

2

z

) 1



A s s e g n a t a

R =

r

1 1

r

1 2

r

1 3

r

2 1

r

2 2

r

2 3

r

3 1

r

3 2

r

3 3

i l q u a t e r n i o n e

`

e ( 0 )

=

1

2

r

1 1

+ r

2 2

+ r

3 3

+ 1

=

1

2

s g n ( r

3 2

r

2 3

)

r

1 1

r

2 2

r

3 3

+ 1

s g n ( r

1 3

r

3 1

)

r

2 2

r

3 3

r

1 1

+ 1

s g n ( r

2 1

r

1 2

)

r

3 3

r

1 1

r

2 2

+ 1

q u a t e r n i o n e e s t r a t t o d a R

1

= R

T

Q

1

= { , }

p r o d o t t o t r a q u a t e r n i o n i

Q

1

Q

2

= {

1

2

T

1

2

,

1

2

+

2

1

+

1

2

}


T R A S F O R M A Z I O N I O M O G E N E E

T r a s f o r m a z i o n e d i c o o r d i n a t e ( t r a s l a z i o n e + r o t a z i o n e )

p

0

= o

0

1

+ R

0

1

p

1

T r a s f o r m a z i o n e i n v e r s a

p

1

= R

1

0

o

0

1

+ R

1

0

p

0


R a p p r e s e n t a z i o n e o m o g e n e a

p =

p

1

M a t r i c e d i t r a s f o r m a z i o n e o m o g e n e a

A

0

1

=

R

0

1

o

0

1

0

T

1

T r a s f o r m a z i o n e d i c o o r d i n a t e

p

0

= A

0

1

p

1


T r a s f o r m a z i o n e i n v e r s a

p

1

= A

1

0

p

0

=

(

A

0

1

)

1

p

0

o v e

A

1

0

=

R

1

0

R

1

0

o

0

1

0

T

1

A

1

6 = A

T

S u c c e s s i o n e d i t r a s f o r m a z i o n i

p

0

= A

0

1

A

1

2

. . . A

n 1

n

p

n


C I N E M A T I C A D I R E T T A

M a n i p o l a t o r e

i n s i e m e d i b r a c c i c o n n e s s i t r a m i t e g i u n t i

C a t e n a c i n e m a t i c a ( d a l l a b a s e a l l o r g a n o t e r m i n a l e )

a p e r t a ( s e q u e n z a u n i c a )

c h i u s a ( s e q u e n z a f o r m a u n a n e l l o )

G r a d o d i m o b i l i t

`

a

t i p i c a m e n t e a s s o c i a t o a u n a a r t i c o l a z i o n e = v a r i a b i l e d i

g i u n t o


T e r n a b a s e e t e r n a u t e n s i l e

E q u a z i o n e c i n e m a t i c a d i r e t t a

T

b

e

( q ) =

n

b

e

( q ) s

b

e

( q ) a

b

e

( q ) p

b

e

( q )

0 0 0 1


M a n i p o l a t o r e p l a n a r e a d u e b r a c c i

T

b

e

( q ) =

n

b

e

s

b

e

a

b

e

p

b

e

0 0 0 1

=

0 s

1 2

c

1 2

a

1

c

1

+ a

2

c

1 2

0 c

1 2

s

1 2

a

1

s

1

+ a

2

s

1 2

1 0 0 0

0 0 0 1


C a t e n a a p e r t a

T

0

n

( q ) = A

0

1

( q

1

) A

1

2

( q

2

) . . . A

n 1

n

( q

n

)

T

b

e

( q ) = T

b

0

T

0

n

( q ) T

n

e


C o n v e n z i o n e d i D e n a v i t - H a r t e n b e r g

s i s c e g l i e l a s s e z

i

g i a c e n t e l u n g o l a s s e d e l g i u n t o i + 1

s i i n d i v i d u a O

i

a l l i n t e r s e z i o n e d e l l a s s e z

i

c o n l a n o r m a l e

c o m u n e a g l i a s s i z

i 1

e z

i

, e c o n O

i

s i i n d i c a l i n t e r s e z i o n e

d e l l a n o r m a l e c o m u n e c o n z

i 1

s i a s s u m e l a s s e x

i

d i r e t t o l u n g o l a n o r m a l e c o m u n e a g l i

a s s i z

i 1

e z

i

c o n v e r s o p o s i t i v o d a l g i u n t o i a l g i u n t o i + 1

s i s c e g l i e l a s s e y

i

i n m o d o d a c o m p l e t a r e u n a t e r n a l e v o g i r a


D e fi n i z i o n e n o n u n i v o c a d e l l a t e r n a :

c o n r i f e r i m e n t o a l l a t e r n a 0 , p e r l a q u a l e l a s o l a d i r e z i o n e

d e l l a s s e z

0

r i s u l t a s p e c i fi c a t a : s i p o s s o n o q u i n d i s c e g l i e r e

a r b i t r a r i a m e n t e O

0

e d x

0

c o n r i f e r i m e n t o a l l a t e r n a n , p e r l a q u a l e i l s o l o

a s s e x

n

r i s u l t a s o g g e t t o a v i n c o l o ( d e v e e s s e r e n o r m a l e

a l l a s s e z

n 1

) : i n f a t t i n o n v i

`

e g i u n t o n + 1 , p e r c u i n o n

`

e

d e fi n i t o z

n

e l o s i p u

`

o s c e g l i e r e a r b i t r a r i a m e n t e

q u a n d o d u e a s s i c o n s e c u t i v i s o n o p a r a l l e l i , i n q u a n t o l a

n o r m a l e c o m u n e t r a d i e s s i n o n

`

e u n i v o c a m e n t e d e fi n i t a

q u a n d o d u e a s s i c o n s e c u t i v i s i i n t e r s e c a n o , i n q u a n t o i l v e r s o

d i x

i

`

e a r b i t r a r i o

q u a n d o i l g i u n t o i

`

e p r i s m a t i c o , n e l q u a l c a s o l a s o l a

d i r e z i o n e d e l l a s s e z

i 1

`

e d e t e r m i n a t a


P a r a m e t r i d i D e n a v i t - H a r t e n b e r g

a

i

d i s t a n z a d i O

i

d a O

i

;

d

i

c o o r d i n a t a s u z

i 1

d i O

i

;

i

a n g o l o i n t o r n o a l l a s s e x

i

t r a l a s s e z

i 1

e l a s s e z

i

v a l u t a t o

p o s i t i v o i n s e n s o a n t i o r a r i o ;

i

a n g o l o i n t o r n o a l l a s s e z

i 1

t r a l a s s e x

i 1

e l a s s e x

i

v a l u t a t o

p o s i t i v o i n s e n s o a n t i o r a r i o .

a

i

e

i

s o n o s e m p r e c o s t a n t i

s e i l g i u n t o

`

e r o t o i d a l e l a v a r i a b i l e

`

e

i

s e i l g i u n t o

`

e p r i s m a t i c o l a v a r i a b i l e

`

e d

i


T r a s f o r m a z i o n e d i c o o r d i n a t e

A

i 1

i

=

c

i

s

i

0 0

s

i

c

i

0 0

0 0 1 d

i

0 0 0 1

A

i

i

=

1 0 0 a

i

0 c

i

s

i

0

0 s

i

c

i

0

0 0 0 1

A

i 1

i

( q

i

) = A

i 1

i

A

i

i

=

c

i

s

i

c

i

s

i

s

i

a

i

c

i

s

i

c

i

c

i

c

i

s

i

a

i

s

i

0 s

i

c

i

d

i

0 0 0 1


P r o c e d u r a o p e r a t i v a

1 . I n d i v i d u a r e e n u m e r a r e c o n s e c u t i v a m e n t e g l i a s s i d e i g i u n t i ;

a s s e g n a r e , r i s p e t t i v a m e n t e , l e d i r e z i o n i a g l i a s s i z

0

, . . . , z

n 1

2 . F i s s a r e l a t e r n a b a s e p o s i z i o n a n d o n e l o r i g i n e s u l l a s s e z

0

; g l i

a s s i x

0

e y

0

s o n o s c e l t i i n m a n i e r a t a l e d a o t t e n e r e u n a t e r n a

l e v o g i r a

E s e g u i r e i p a s s i d a 3 a 5 p e r i = 1 , . . . , n 1 :

3 . I n d i v i d u a r e l o r i g i n e O

i

a l l i n t e r s e z i o n e d i z

i

c o n l a n o r m a l e

c o m u n e a g l i a s s i z

i 1

e z

i

. S e g l i a s s i z

i 1

e z

i

s o n o p a r a l l e l i e

i l g i u n t o i

`

e r o t o i d a l e , p o s i z i o n a r e O

i

i n m o d o d a a n n u l l a r e d

i

;

s e i l g i u n t o i

`

e p r i s m a t i c o , s c e g l i e r e O

i

i n c o r r i s p o n d e n z a d i

u n a p o s i z i o n e d i r i f e r i m e n t o p e r l a c o r s a d e l g i u n t o ( a d e s e m p i o

u n fi n e - c o r s a )

4 . F i s s a r e l a s s e x

i

d i r e t t o l u n g o l a n o r m a l e c o m u n e a g l i a s s i z

i 1

e z

i

c o n v e r s o p o s i t i v o d a l g i u n t o i a l g i u n t o i + 1

5 . F i s s a r e l a s s e y

i

i n m o d o d a o t t e n e r e u n a t e r n a l e v o g i r a

P e r c o m p l e t a r e :

6 . F i s s a r e l a t e r n a n , a l l i n e a n d o z

n

l u n g o l a d i r e z i o n e d i z

n 1

s e i l

g i u n t o n

`

e r o t o i d a l e , o v v e r o s c e g l i e n d o z

n

i n m a n i e r a a r b i t r a r i a

s e i l g i u n t o n

`

e p r i s m a t i c o ; fi s s a r e l a s s e x

n

i n a c c o r d o a l p u n t o 4

7 . C o s t r u i r e p e r i = 1 , . . . , n l a t a b e l l a d e i p a r a m e t r i a

i

, d

i

,

i

,

i

8 . C a l c o l a r e s u l l a b a s e d e i p a r a m e t r i d i c u i a l p u n t o 7 l e m a t r i c i

d i t r a s f o r m a z i o n e o m o g e n e a A

i 1

i

( q

i

) p e r i = 1 , . . . , n

9 . C a l c o l a r e T

0

n

( q ) = A

0

1

. . . A

n 1

n

c h e f o r n i s c e p o s i z i o n e e

o r i e n t a m e n t o d e l l a t e r n a n r i s p e t t o a l l a t e r n a 0

1 0 . A s s e g n a t e T

b

0

e T

n

e

, c a l c o l a r e l a f u n z i o n e c i n e m a t i c a d i r e t t a

T

b

e

( q ) = T

b

0

T

0

n

T

n

e

c h e f o r n i s c e p o s i z i o n e e o r i e n t a m e n t o d e l l a

t e r n a u t e n s i l e r i s p e t t o a l l a t e r n a b a s e


C a t e n a c h i u s a

C o n n e s s i o n e d i u n s i n g o l o b r a c c i o c o n d u e b r a c c i

g i u n t o v i r t u a l e d i t a g l i o

A

i

j

( q

) = A

i

i + 1

( q

i + 1

) . . . A

j 1

j

( q

j

)

A

i

k

( q

) = A

i

i + 1

( q

i + 1

) . . . A

k 1

k

( q

k

)


V i n c o l i

g i u n t o j + 1 r o t o i d a l e

{

R

j

i

( q

)

(

p

i

j

( q

) p

i

k

( q

)

)

= [ 0 0 d

j k

]

T

z

i

j

( q

) = z

i

k

( q

)

g i u n t o j + 1 p r i s m a t i c o

[

x

i T

j

( q

)

y

i T

j

( q

)

]

(

p

i

j

( q

) p

i

k

( q

)

)

=

[

0

0

]

z

i

j

( q

) = z

i

k

( q

)

x

i T

j

( q

) x

i

k

( q

) = c o s

j k

r i s o l t i i n t e r m i n i d i q . . .

T

0

n

( q ) = A

0

i

A

i

j

A

j

n


P r o c e d u r a o p e r a t i v a

1 . S e l e z i o n a r e u n g i u n t o n o n a t t u a t o d e l l a c a t e n a c h i u s a . I p o t i z z a r e

d i a p r i r e t a l e g i u n t o i n m o d o d a o t t e n e r e u n a c a t e n a a p e r t a c o n

s t r u t t u r a a d a l b e r o

2 . C a l c o l a r e l e t r a s f o r m a z i o n i o m o g e n e e s e c o n d o l a c o n v e n z i o n e

d i D e n a v i t - H a r t e n b e r g

3 . T r o v a r e i v i n c o l i d i u g u a g l i a n z a p e r l e d u e t e r n e c o n n e s s e d a l

g i u n t o d i t a g l i o

4 . R i s o l v e r e i v i n c o l i i n t e r m i n i d i u n n u m e r o r i d o t t o d i v a r i a b i l i

d i g i u n t o

5 . E s p r i m e r e l e t r a s f o r m a z i o n i o m o g e n e e i n f u n z i o n e d i t a l i

v a r i a b i l i d i g i u n t o e c a l c o l a r e l a f u n z i o n e c i n e m a t i c a d i r e t t a

p e r c o m p o s i z i o n e d e l l a v a r i e t r a s f o r m a z i o n i d a l l a t e r n a b a s e

a l l a t e r n a u t e n s i l e


M a n i p o l a t o r e p l a n a r e a t r e b r a c c i


B r a c c i o a

i

i

d

i

i

1 a

1

0 0

1

2 a

2

0 0

2

3 a

3

0 0

3


A

i 1

i

=

c

i

s

i

0 a

i

c

i

s

i

c

i

0 a

i

s

i

0 0 1 0

0 0 0 1

i = 1 , 2 , 3

T

0

3

= A

0

1

A

1

2

A

2

3

=

c

1 2 3

s

1 2 3

0 a

1

c

1

+ a

2

c

1 2

+ a

3

c

1 2 3

s

1 2 3

c

1 2 3

0 a

1

s

1

+ a

2

s

1 2

+ a

3

s

1 2 3

0 0 1 0

0 0 0 1


M a n i p o l a t o r e a p a r a l l e l o g r a m m a


B r a c c i o a

i

i

d

i

i

1

a

1

0 0

1

2

a

2

0 0

2

3

a

3

0 0

3

1

a

1

0 0

1

4 a

4

0 0 0


A

0

3

( q

) = A

0

1

A

1

2

A

2

3

=

c

1

2

3

s

1

2

3

0 a

1

c

1

+ a

2

c

1

2

+ a

3

c

1

2

3

s

1

2

3

c

1

2

3

0 a

1

s

1

+ a

2

s

1

2

+ a

3

s

1

2

3

0 0 1 0

0 0 0 1

A

0

1

( q

) =

c

1

s

1

0 a

1

c

1

s

1

c

1

0 a

1

s

1

0 0 1 0

0 0 0 1

A

3

4

=

1 0 0 a

4

0 1 0 0

0 0 1 0

0 0 0 1


R i s o l u z i o n e v i n c o l i

o r i e n t a m e n t o ( O K )

p o s i z i o n e

R

3

0

( q

)

(

p

0

3

( q

) p

0

1

( q

)

)

=

0

0

0

a

1

( c

1

+ c

1

2

3

) + a

1

( c

1

2

c

1

) = 0

a

1

( s

1

+ s

1

2

3

) + a

1

( s

1

2

s

1

) = 0

2

=

1

1

3

=

2

=

1

+

1


T

0

4

( q ) = A

0

3

( q ) A

3

4

=

c

1

s

1

0 a

1

c

1

a

4

c

1

s

1

c

1

0 a

1

s

1

a

4

s

1

0 0 1 0

0 0 0 1


M a n i p o l a t o r e s f e r i c o


B r a c c i o a

i

i

d

i

i

1 0 / 2 0

1

2 0 / 2 d

2

2

3 0 0 d

3

0


A

0

1

=

c

1

0 s

1

0

s

1

0 c

1

0

0 1 0 0

0 0 0 1

A

1

2

=

c

2

0 s

2

0

s

2

0 c

2

0

0 1 0 d

2

0 0 0 1

A

2

3

=

1 0 0 0

0 1 0 0

0 0 1 d

3

0 0 0 1

T

0

3

= A

0

1

A

1

2

A

2

3

=

c

1

c

2

s

1

c

1

s

2

c

1

s

2

d

3

s

1

d

2

s

1

c

2

c

1

s

1

s

2

s

1

s

2

d

3

+ c

1

d

2

s

2

0 c

2

c

2

d

3

0 0 0 1


M a n i p o l a t o r e a n t r o p o m o r f o


B r a c c i o a

i

i

d

i

i

1 0 / 2 0

1

2 a

2

0 0

2

3 a

3

0 0

3


A

0

1

=

c

1

0 s

1

0

s

1

0 c

1

0

0 1 0 0

0 0 0 1

A

i 1

i

=

c

i

s

i

0 a

i

c

i

s

i

c

i

0 a

i

s

i

0 0 1 0

0 0 0 1

i = 2 , 3

T

0

3

= A

0

1

A

1

2

A

2

3

=

c

1

c

2 3

c

1

s

2 3

s

1

c

1

( a

2

c

2

+ a

3

c

2 3

)

s

1

c

2 3

s

1

s

2 3

c

1

s

1

( a

2

c

2

+ a

3

c

2 3

)

s

2 3

c

2 3

0 a

2

s

2

+ a

3

s

2 3

0 0 0 1


P o l s o s f e r i c o


B r a c c i o a

i

i

d

i

i

4 0 / 2 0

4

5 0 / 2 0

5

6 0 0 d

6

6


A

3

4

=

c

4

0 s

4

0

s

4

0 c

4

0

0 1 0 0

0 0 0 1

A

4

5

=

c

5

0 s

5

0

s

5

0 c

5

0

0 1 0 0

0 0 0 1

A

5

6

=

c

6

s

6

0 0

s

6

c

6

0 0

0 0 1 d

6

0 0 0 1

T

3

6

= A

3

4

A

4

5

A

5

6

=

c

4

c

5

c

6

s

4

s

6

c

4

c

5

s

6

s

4

c

6

c

4

s

5

c

4

s

5

d

6

s

4

c

5

c

6

+ c

4

s

6

s

4

c

5

s

6

+ c

4

c

6

s

4

s

5

s

4

s

5

d

6

s

5

c

6

s

5

s

6

c

5

c

5

d

6

0 0 0 1


M a n i p o l a t o r e d i S t a n f o r d


T

0

6

= T

0

3

T

3

6

=

n

0

s

0

a

0

p

0

0 0 0 1


p

0

=

c

1

s

2

d

3

s

1

d

2

+

(

c

1

( c

2

c

4

s

5

+ s

2

c

5

) s

1

s

4

s

5

)

d

6

s

1

s

2

d

3

+ c

1

d

2

+

(

s

1

( c

2

c

4

s

5

+ s

2

c

5

) + c

1

s

4

s

5

)

d

6

c

2

d

3

+ ( s

2

c

4

s

5

+ c

2

c

5

) d

6

n

0

=

c

1

(

c

2

( c

4

c

5

c

6

s

4

s

6

) s

2

s

5

c

6

)

s

1

( s

4

c

5

c

6

+ c

4

s

6

)

s

1

(

c

2

( c

4

c

5

c

6

s

4

s

6

) s

2

s

5

c

6

)

+ c

1

( s

4

c

5

c

6

+ c

4

s

6

)

s

2

( c

4

c

5

c

6

s

4

s

6

) c

2

s

5

c

6

s

0

=

c

1

(

c

2

( c

4

c

5

s

6

+ s

4

c

6

) + s

2

s

5

s

6

)

s

1

( s

4

c

5

s

6

+ c

4

c

6

)

s

1

(

c

2

( c

4

c

5

s

6

+ s

4

c

6

) + s

2

s

5

s

6

)

+ c

1

( s

4

c

5

s

6

+ c

4

c

6

)

s

2

( c

4

c

5

s

6

+ s

4

c

6

) + c

2

s

5

s

6

a

0

=

c

1

( c

2

c

4

s

5

+ s

2

c

5

) s

1

s

4

s

5

s

1

( c

2

c

4

s

5

+ s

2

c

5

) + c

1

s

4

s

5

s

2

c

4

s

5

+ c

2

c

5


M a n i p o l a t o r e a n t r o p o m o r f o c o n p o l s o s f e r i c o


B r a c c i o a

i

i

d

i

i

1 0 / 2 0

1

2 a

2

0 0

2

3 0 / 2 0

3

4 0 / 2 d

4

4

5 0 / 2 0

5

6 0 0 d

6

6


A

2

3

=

c

3

0 s

3

0

s

3

0 c

3

0

0 1 0 0

0 0 0 1

A

3

4

=

c

4

0 s

4

0

s

4

0 c

4

0

0 1 0 d

4

0 0 0 1

p

0

=

a

2

c

1

c

2

+ d

4

c

1

s

2 3

+ d

6

(

c

1

( c

2 3

c

4

s

5

+ s

2 3

c

5

) + s

1

s

4

s

5

)

a

2

s

1

c

2

+ d

4

s

1

s

2 3

+ d

6

(

s

1

( c

2 3

c

4

s

5

+ s

2 3

c

5

) c

1

s

4

s

5

)

a

2

s

2

d

4

c

2 3

+ d

6

( s

2 3

c

4

s

5

c

2 3

c

5

)

n

0

=

c

1

(

c

2 3

( c

4

c

5

c

6

s

4

s

6

) s

2 3

s

5

c

6

)

+ s

1

( s

4

c

5

c

6

+ c

4

s

6

)

s

1

(

c

2 3

( c

4

c

5

c

6

s

4

s

6

) s

2 3

s

5

c

6

)

c

1

( s

4

c

5

c

6

+ c

4

s

6

)

s

2 3

( c

4

c

5

c

6

s

4

s

6

) + c

2 3

s

5

c

6

s

0

=

c

1

(

c

2 3

( c

4

c

5

s

6

+ s

4

c

6

) + s

2 3

s

5

s

6

)

+ s

1

( s

4

c

5

s

6

+ c

4

c

6

)

s

1

(

c

2 3

( c

4

c

5

s

6

+ s

4

c

6

) + s

2 3

s

5

s

6

)

c

1

( s

4

c

5

s

6

+ c

4

c

6

)

s

2 3

( c

4

c

5

s

6

+ s

4

c

6

) c

2 3

s

5

s

6

a

0

=

c

1

( c

2 3

c

4

s

5

+ s

2 3

c

5

) + s

1

s

4

s

5

s

1

( c

2 3

c

4

s

5

+ s

2 3

c

5

) c

1

s

4

s

5

s

2 3

c

4

s

5

c

2 3

c

5


M a n i p o l a t o r e d e l D L R

B r a c c i o a

i

i

d

i

i

1 0 / 2 0

1

2 0 / 2 0

2

3 0 / 2 d

3

3

4 0 / 2 0

4

5 0 / 2 d

5

5

6 0 / 2 0

6

7 0 0 d

7

7


A

i 1

i

=

c

i

0 s

i

0

s

i

0 c

i

0

0 1 0 d

i

0 0 0 1

i = 1 , . . . , 6

A

6

7

=

c

7

s

7

0 0

s

7

c

7

0 0

0 0 1 d

7

0 0 0 1

p

0

7

=

d

3

x

d

3

+ d

5

x

d

5

+ d

7

x

d

7

d

3

y

d

3

+ d

5

y

d

5

+ d

7

y

d

7

d

3

z

d

3

+ d

5

z

d

5

+ d

7

z

d

7

x

d

3

= c

1

s

2

x

d

5

= c

1

( c

2

c

3

s

4

s

2

c

4

) + s

1

s

3

s

4

x

d

7

= c

1

( c

2

k

1

+ s

2

k

2

) + s

1

k

3

y

d

3

= s

1

s

2

y

d

5

= s

1

( c

2

c

3

s

4

s

2

c

4

) c

1

s

3

s

4

y

d

7

= s

1

( c

2

k

1

+ s

2

k

2

) c

1

k

3

z

d

3

= c

2

z

d

5

= c

2

c

4

+ s

2

c

3

s

4

z

d

7

= s

2

( c

3

( c

4

c

5

s

6

s

4

c

6

) + s

3

s

5

s

6

) c

2

k

2

k

1

= c

3

( c

4

c

5

s

6

s

4

c

6

) + s

3

s

5

s

6

k

2

= s

4

c

5

s

6

+ c

4

c

6

k

3

= s

3

( c

4

c

5

s

6

s

4

c

6

) c

3

s

5

s

6


n

0

7

=

( ( x

a

c

5

+ x

c

s

5

) c

6

+ x

b

s

6

) c

7

+ ( x

a

s

5

x

c

c

5

) s

7

( ( y

a

c

5

+ y

c

s

5

) c

6

+ y

b

s

6

) c

7

+ ( y

a

s

5

y

c

c

5

) s

7

( z

a

c

6

+ z

c

s

6

) c

7

+ z

b

s

7

s

0

7

=

( ( x

a

c

5

+ x

c

s

5

) c

6

+ x

b

s

6

) s

7

+ ( x

a

s

5

x

c

c

5

) c

7

( ( y

a

c

5

+ y

c

s

5

) c

6

+ y

b

s

6

) s

7

+ ( y

a

s

5

y

c

c

5

) c

7

( z

a

c

6

+ z

c

s

6

) s

7

+ z

b

c

7

a

0

7

=

( x

a

c

5

+ x

c

s

5

) s

6

x

b

c

6

( y

a

c

5

+ y

c

s

5

) s

6

y

b

c

6

z

a

s

6

z

c

c

6

x

a

= ( c

1

c

2

c

3

+ s

1

s

3

) c

4

+ c

1

s

2

s

4

x

b

= ( c

1

c

2

c

3

+ s

1

s

3

) s

4

c

1

s

2

c

4

x

c

= c

1

c

2

s

3

s

1

c

3

y

a

= ( s

1

c

2

c

3

c

1

s

3

) c

4

+ s

1

s

2

s

4

y

b

= ( s

1

c

2

c

3

c

1

s

3

) s

4

s

1

s

2

c

4

y

c

= s

1

c

2

s

3

+ c

1

c

3

z

a

= ( s

2

c

3

c

4

c

2

s

4

) c

5

+ s

2

s

3

s

5

z

b

= ( s

2

c

3

s

4

+ c

2

c

4

) s

5

s

2

s

3

c

5

z

c

= s

2

c

3

s

4

+ c

2

c

4

s e

7

= / 2

A

6

7

=

c

7

0 s

7

a

7

c

7

s

7

0 c

7

a

7

s

7

0 0 1 0

0 0 0 1


M a n i p o l a t o r e u m a n o i d e

B r a c c i a c o s t i t u i t e d a d u e m a n i p o l a t o r i d e l D L R (

7

= / 2 )

S t r u t t u r a d i c o l l e g a m e n t o t r a l o r g a n o t e r m i n a l e d e l t o r s o

a n t r o p o m o r f o e l e t e r n e b a s e d e i m a n i p o l a t o r i c h e f u n g o n o

d a b r a c c i a

m e c c a n i s m o c h e p e r m e t t e d i m a n t e n e r e i l p e t t o d e l

m a n i p o l a t o r e u m a n o i d e s e m p r e o r t o g o n a l e a l s u o l o (

4

=

2

3

)


T

0

r h

= T

0

3

T

3

t

T

t

r

T

r

r h

T

0

l h

= T

0

3

T

3

t

T

t

l

T

l

l h


T

3

t

=

c

2 3

s

2 3

0 0

s

2 3

c

2 3

0 0

0 0 1 0

0 0 0 1

T

0

3

c o m e n e l m a n i p o l a t o r e a n t r o p o m o r f o

T

t

r

e T

t

l

d i p e n d o n o d a

T

r

r h

e T

r

l h

c o m e n e l m a n i p o l a t o r e d e l D L R


S P A Z I O D E I G I U N T I E S P A Z I O O P E R A T I V O

S p a z i o d e i g i u n t i

q =

q

1

.

.

.

q

n

q

i

=

i

( g i u n t o r o t o i d a l e )

q

i

= d

i

( g i u n t o p r i s m a t i c o )

S p a z i o o p e r a t i v o

x =

[

p

]

p ( p o s i z i o n e )

( o r i e n t a m e n t o )

E q u a z i o n e c i n e m a t i c a d i r e t t a

x = k ( q )


E s e m p i o

x =

p

x

p

y

= k ( q ) =

a

1

c

1

+ a

2

c

1 2

+ a

3

c

1 2 3

a

1

s

1

+ a

2

s

1 2

+ a

3

s

1 2 3

1

+

2

+

3


S p a z i o d i l a v o r o

S p a z i o d i l a v o r o r a g g i u n g i b i l e

p = p ( q ) q

i m

q

i

q

i M

i = 1 , . . . , n

e l e m e n t i d i s u p e r fi c i e p l a n a r e , s f e r i c a , t o r o i d a l e e c i l i n d r i c a

S p a z i o d i l a v o r o d e s t r o

o r i e n t a m e n t i d i v e r s i


E s e m p i o

c o n fi g u r a z i o n i a m m i s s i b i l i


s p a z i o d i l a v o r o


A c c u r a t e z z a

s c o s t a m e n t o t r a p o s i z i o n e c o n s e g u i t a c o n l a p o s t u r a

a s s e g n a t a e p o s i z i o n e c a l c o l a t a t r a m i t e l a c i n e m a t i c a d i r e t t a

v a l o r i t i p i c i : ( 0 . 2 , 1 ) m m

R i p e t i b i l i t

`

a

c a p a c i t

`

a d e l m a n i p o l a t o r e d i t o r n a r e i n u n a p o s i z i o n e

p r e c e d e n t e m e n t e r a g g i u n t a

v a l o r i t i p i c i : ( 0 . 0 2 , 0 . 2 ) m m

R i d o n d a n z a c i n e m a t i c a

m < n ( i n t r i n s e c a )

r < m = n ( f u n z i o n a l e )


C A L I B R A Z I O N E C I N E M A T I C A

V a l o r i p r e c i s i d e i p a r a m e t r i D H p e r m i g l i o r a r e l a c c u r a t e z z a d i

u n m a n i p o l a t o r e

E q u a z i o n e c i n e m a t i c a d i r e t t a i n f u n z i o n e d i t u t t i i p a r a m e t r i

x = k ( a , , d , )

x

m

l o c a z i o n e m i s u r a t a

x

n

l o c a z i o n e n o m i n a l e ( p a r a m e t r i fi s s i + v a r i a b i l i g i u n t o )

x =

k

a

a +

k

+

k

d

d +

k

= (

n

)


l m i s u r e ( l m 4 n )

x =

x

1

.

.

.

x

l

=

1

.

.

.

l

=

S o l u z i o n e

= (

T

)

1

T

x

=

n

+

. . . fi n c h

e c o n v e r g e

s t i m e p i

`

u a c c u r a t e d e i p a r a m e t r i fi s s i

c o r r e z i o n i a l l e m i s u r e d e i t r a s d u t t o r i

I n i z i a l i z z a z i o n e

p o s t u r a d i r i f e r i m e n t o ( h o m e )


P R O B L E M A C I N E M A T I C O I N V E R S O


q = T

q = x

C i n e m a t i c a i n v e r s a

T = q

x = q

C o m p l e s s i t

`

a

s o l u z i o n e a n a l i t i c a ( i n f o r m a c h i u s a ) ?

s o l u z i o n i m u l t i p l e

i n fi n i t e s o l u z i o n i

n o n e s i s t o n o s o l u z i o n i a m m i s s i b i l i

I n t u i z i o n e

a l g e b r i c a

g e o m e t r i c a

T e c n i c h e n u m e r i c h e


S o l u z i o n e d e l m a n i p o l a t o r e p l a n a r e a t r e b r a c c i

S o l u z i o n e a l g e b r i c a

=

1

+

2

+

3

p

W x

= p

x

a

3

c

= a

1

c

1

+ a

2

c

1 2

p

W y

= p

y

a

3

s

= a

1

s

1

+ a

2

s

1 2


c

2

=

p

2

W x

+ p

2

W y

a

2

1

a

2

2

2 a

1

a

2

s

2

=

1 c

2

2

2

= A t a n 2 ( s

2

, c

2

)

s

1

=

( a

1

+ a

2

c

2

) p

W y

a

2

s

2

p

W x

p

2

W x

+ p

2

W y

c

1

=

( a

1

+ a

2

c

2

) p

W x

+ a

2

s

2

p

W y

p

2

W y

+ p

2

W y

1

= A t a n 2 ( s

1

, c

1

)

3

=

1

2


S o l u z i o n e g e o m e t r i c a

c

2

=

p

2

W x

+ p

2

W y

a

2

1

a

2

2

2 a

1

a

2

.

2

= c o s

1

( c

2

)

= A t a n 2 ( p

W y

, p

W x

)

c

p

2

W x

+ p

2

W y

= a

1

+ a

2

c

2

= c o s

1

p

2

W x

+ p

2

W y

+ a

2

1

a

2

2

2 a

1

p

2

W x

+ p

2

W y

1

=


S o l u z i o n e d i m a n i p o l a t o r i c o n p o l s o s f e r i c o

p

W

= p d

6

a

S o l u z i o n e d i s a c c o p p i a t a

c a l c o l a r e l a p o s i z i o n e d e l p o l s o p

W

( q

1

, q

2

, q

3

)

r i s o l v e r e l a c i n e m a t i c a i n v e r s a p e r ( q

1

, q

2

, q

3

)

c a l c o l a r e R

0

3

( q

1

, q

2

, q

3

)

c a l c o l a r e R

3

6

(

4

,

5

,

6

) = R

0

3

T

R

r i s o l v e r e l a c i n e m a t i c a i n v e r s a p e r l o r i e n t a m e n t o (

4

,

5

,

6

)


S o l u z i o n e d e l m a n i p o l a t o r e s f e r i c o

( A

0

1

)

1

T

0

3

= A

1

2

A

2

3

p

1

W

=

p

W x

c

1

+ p

W y

s

1

p

W z

p

W x

s

1

+ p

W y

c

1

=

d

3

s

2

d

3

c

2

d

2


c

1

=

1 t

2

1 + t

2

s

1

=

2 t

1 + t

2

( d

2

+ p

W y

) t

2

+ 2 p

W x

t + d

2

p

W y

= 0

1

= 2 A t a n 2

(

p

W x

p

2

W x

+ p

2

W y

d

2

2

, d

2

+ p

W y

)

p

W x

c

1

+ p

W y

s

1

p

W z

=

d

3

s

2

d

3

c

2

2

= A t a n 2 ( p

W x

c

1

+ p

W y

s

1

, p

W z

)

d

3

=

( p

W x

c

1

+ p

W y

s

1

)

2

+ p

2

W z


S o l u z i o n e d e l m a n i p o l a t o r e a n t r o p o m o r f o

p

W x

= c

1

( a

2

c

2

+ a

3

c

2 3

)

p

W y

= s

1

( a

2

c

2

+ a

3

c

2 3

)

p

W z

= a

2

s

2

+ a

3

s

2 3

c

3

=

p

2

W x

+ p

2

W y

+ p

2

W z

a

2

2

a

2

3

2 a

2

a

3

s

3

=

1 c

2

3

3

= A t a n 2 ( s

3

, c

3

)

3 , I

[ , ]

3 , I I

=

3 , I


c

2

=

p

2

W x

+ p

2

W y

( a

2

+ a

3

c

3

) + p

W z

a

3

s

3

a

2

2

+ a

2

3

+ 2 a

2

a

3

c

3

s

2

=

p

W z

( a

2

+ a

3

c

3

)

p

2

W x

+ p

2

W y

a

3

s

3

a

2

2

+ a

2

3

+ 2 a

2

a

3

c

3

2

= A t a n 2 ( s

2

, c

2

)

p e r s

+

3

=

1 c

2

3

:

2 , I

= A t a n 2

(

( a

2

+ a

3

c

3

) p

W z

a

3

s

+

3

p

2

W x

+ p

2

W y

,

( a

2

+ a

3

c

3

)

p

2

W x

+ p

2

W y

+ a

3

s

+

3

p

W z

)

2 , I I

= A t a n 2

(

( a

2

+ a

3

c

3

) p

W z

+ a

3

s

+

3

p

2

W x

+ p

2

W y

,

( a

2

+ a

3

c

3

)

p

2

W x

+ p

2

W y

+ a

3

s

+

3

p

W z

)

p e r s

3

=

1 c

2

3

:

2 , I I I

= A t a n 2

(

( a

2

+ a

3

c

3

) p

W z

a

3

s

3

p

2

W x

+ p

2

W y

,

( a

2

+ a

3

c

3

)

p

2

W x

+ p

2

W y

+ a

3

s

3

p

W z

)

2 , I V

= A t a n 2

(

( a

2

+ a

3

c

3

) p

W z

+ a

3

s

3

p

2

W x

+ p

2

W y

,

( a

2

+ a

3

c

3

)

p

2

W x

+ p

2

W y

+ a

3

s

3

p

W z

)


1 , I

= A t a n 2 ( p

W y

, p

W x

)

1 , I I

= A t a n 2 ( p

W y

, p

W x

)

Q u a t t r o c o n fi g u r a z i o n i a m m i s s i b i l i

(

1 , I

,

2 , I

,

3 , I

) (

1 , I

,

2 , I I I

,

3 , I I

)

(

1 , I I

,

2 , I I

,

3 , I

) (

1 , I I

,

2 , I V

,

3 , I I

)

s o l u z i o n e u n i v o c a s o l o s e

p

W x

6 = 0 p

W y

6 = 0


S o l u z i o n e d e l p o l s o s f e r i c o

R

3

6

=

n

3

x

s

3

x

a

3

x

n

3

y

s

3

y

a

3

y

n

3

z

s

3

z

a

3

z

4

= A t a n 2 ( a

3

y

, a

3

x

)

5

= A t a n 2

(

( a

3

x

)

2

+ ( a

3

y

)

2

, a

3

z

)

6

= A t a n 2 ( s

3

z

, n

3

z

)

4

= A t a n 2 ( a

3

y

, a

3

x

)

5

= A t a n 2

(

( a

3

x

)

2

+ ( a

3

y

)

2

, a

3

z

)

6

= A t a n 2 ( s

3

z

, n

3

z

)

Matrici di Rotazione

Documents

Transcript of Matrici di Rotazione