2.

36 Forward Kinematic by Systematic Method

# θ D R α
l3
0-1 0 0 0
θ1 l4
1-2 0 90
θ2 l5
2-3 0 -90
l 6 +l 7
3-H 180+θ 3 0 0

[ ]
1 0 0 0
0 1 0 0
A1 =
0 0 1 l3
0 0 0 1

[ ]
cos( t 1) 0 sin(t 1) 0
sin(t 1) 0 −c os (t 1) 0
A2=
0 1 0 l4
0 0 0 0

[ ]
cos( t 2) 0 −sin (t 2) l5∗cos (t 2)
A3 = sin(t 2) 0 cos (t 2) l 5∗sin (t 2)
0 −1 0 0
0 0 0 1

[ ]
−cos (t 3) sin (t 3) 0 0
A 4= −sin( t 3) −cos (t 3) 0 0
0 0 1 l 6+l 7
0 0 0 1
 [
s(t 1)∗s (t 3)−c( t 1)∗c (t 2)∗c(t 3) c (t 3)∗s (t 1)+c (t 1)∗c (t 2)∗s (t 3) −c (t 1)∗s(t 2) l 5∗c (t 1)∗c (t 2
−c(t 1)∗s (t 3)−c(t 2)∗c (t 3)∗s (t 1) c (t 2)∗s (t 1)∗s (t 3)−c (t 1)∗c (t 3) −s(t 1)∗s (t 2) l 5∗c (t 2)∗s(t 1
A1∗A 2∗A3∗A 4 =
−cos (t 3)∗sin(t 2) sin(t 2)∗sin(t 3) cos(t 2) l 3+l 4+cos (t 2
0 0 0

[ ]
1 0 0 l1
0 1 0 l2 ∗A ∗A ∗A ∗A
T H =T 0 *T H =
U U 0
1 2 3 4
0 0 1 0
0 0 0 1

[ ]
−1 0 0 l1+l 5
0 −1 0 l2
T H=
U
0 0 1 l 3+l 4+l 6+l 7
0 0 0 1

2.39 Inverse Kinematic

T 0H = A1∗A 2∗A3∗A 4 = A1 2∗A3∗A4

A1 2
−1
*T = A ∗A
0
H 3 4

[ ][ ][ ]
C1 S1 0 0 Nx Ox Ax Px cos(t 2) 0 −sin (t 2) l5∗cos (t 2)
0 0 1 −L3−L 4 ∗ Ny Oy Ay Py sin(t 2) 0 cos (t 2) l 5∗sin (t 2)
= *
S 1 −C 1 0 0 Nz Oz Az Pz 0 −1 0 0
0 0 0 1 0 0 0 1 0 0 0 1

[ ]
−cos (t 3) sin (t 3) 0 0
−s ∈(t 3) −cos (t 3) 0 0
0 0 1 l 6+l7
0 0 0 1

[ ][
C 1 Nx+ S 1 Ny C 1∗Ox+ S 1 Oy C 1 Ax +S 1 Ay C 1 Px+ S 1 Py −C 2C 3 C 2 S 3 −S 2 −S 2 ( L6 + L7 )
Nx Oz Az Pz−L 3−L 4 C 2 ( L 6+ L 7 ) +
= −S 2 C 3 S 2 S 3 C 2
S 1 Nx−C 1 Ny S 1Ox−C 1∗Oy S 1 Ax−C 1 Ay S 1 Px−C 1 Py S3 C3 0 0
0 0 0 1 0 0 0 1
 Ay
S1Ax_C1Ay=0; t1= tan
−1
( )
Ax

Py
S1Px-C1Py=0; t1= tan
−1
( )
Px

S2=-(C1Ax+S1Ay);
C2=Az; t2=ATAN2(-C1Ax-S1Ay,Az)
S3=S1Nx-C1Ny;
C3=S1Ox-C1Oy;
t3=ATAN2(S1Nx-C1Ny,S1Ox-C1Oy)

2-37:Forward Kinematics by Systematic Method

[ ]
sin ⁡( t 1) 0 cos ⁡(t 1) 0
−cos ⁡(t 1) 0 sin ⁡(t 1) 0
A1= 0 −1 0 −l3−l 4
0 0 0 1

[ ]
sin( t 2) cos (t 2) 0 0
s−cos (t 2) sin(t 2) 0 0
A2= 0 0 1 l5+ l6+l 7
0 0 0 1

[ ]
sin(t 1)∗sin (t 2) cos(t 2)∗sin(t 1) cos(t 1) cos (t 1)∗(l 5+l 6+l 7)
−cos(t 1)∗sin(t 2) −cos (t 1)∗cos(t 2) sin(t 1) sin(t 1)∗(l5+l 6+l 7)
A1∗A 2=
cos (t 2) −sin(t 2) 0 −l 3−l 4
0 0 0 1
 [ ]
1 0 0 l1
0 1 0 −l 2 ∗A ∗A
T UH =T U0 *T 0H = 1 2
0 0 1 0
0 0 0 1

[ ]
0 0 1 l1+l 5+l 6+l 7
¿ 0 −1 0 −l2
1 0 0 −l 3−l 4
0 0 0 1

2_40: Inverse Kinematic

0
T H = A 1∗A2

[ ][ ]
Nx Ox Ax Px sin (t 1)∗sin(t 2) cos (t 2)∗sin (t 1) cos (t 1) cos (t 1)∗(l5+l 6+l 7)
Ny Oy Ay Py −cos (t 1)∗sin (t 2) −cos (t 1)∗cos (t 2) sin (t 1) sin (t 1)∗(l 5+l 6+l 7)
=
Nz Oz Az Pz cos (t 2) −sin (t 2) 0 −l 3−l 4
0 0 0 1 0 0 0 1

S1=Ay
C1=Ax;
t1=ATAN2(Ay,Ax)
S2=-Nz;
C2=-Oz;
t2=ATAN2(-Nz,-Oz)