Matrici di Rotazione
-
Upload
valeriotucci -
Category
Documents
-
view
27 -
download
0
description
Transcript of 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
-
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
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
-
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
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
-
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
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
-
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
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
-
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
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
-
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
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
-
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
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
-
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
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 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 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
-
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
E s e m p i o
-
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
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
-
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
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
-
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
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
)
-
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
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
-
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
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 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
)
-
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
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
-
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
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 )
-
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
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
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
-
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
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
-
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
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 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
}
-
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
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
-
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
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
-
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
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 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 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
-
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
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
-
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
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 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 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 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 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
-
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
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
-
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
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
-
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
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
-
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
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 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 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
)
-
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
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
-
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
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
-
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
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
-
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
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
-
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
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
-
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
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
-
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
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
-
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
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
-
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
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
C i n e m a t i c a d i r e t t a
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
-
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
M a n i p o l a t o r e s f e r i c o
-
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
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
-
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
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
-
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
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 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
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
-
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
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
-
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
P o l s o s f e r i c o
-
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
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
-
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
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
-
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
M a n i p o l a t o r e d i S t a n f o r d
-
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
T
0
6
= T
0
3
T
3
6
=
n
0
s
0
a
0
p
0
0 0 0 1
-
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
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
-
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
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
-
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
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
-
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
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
-
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
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
-
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
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
-
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
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
-
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
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
)
C i n e m a t i c a d i r e t t a
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
-
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
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
-
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
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 )
-
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
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
-
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
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
-
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
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
-
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
s p a z i o d i l a v o r o
-
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
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 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 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
)
-
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
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 )
-
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
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
C i n e m a t i c a d i r e t t a
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
-
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
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 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
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
-
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
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
=
-
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
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
)
-
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
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 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
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
-
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
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 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
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
)
-
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
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
-
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
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
)