Bio-Robotics and Human-Mechatronics (T.

Matsumaru) Lab 2015

ロボット工学 Schedule – 2015 Fall semester

• 30 Sep. Wed 09h00-10h30, S104 01. What robot is?
• 07 Oct. Wed 09h00-10h30, S104 02. Robot history
Robotics •
•
•
14 Oct. Wed 09h00-10h30, S104 03. Robot configuration
21 Oct. Wed 09h00-10h30, S104 04. Robot mechanism
28 Oct. Wed 09h00-10h30, S104 05. Actuator
440111370F • 04 Nov. Wed 09h00-10h30, S104 06. Sensor (1)
• 11 Nov. Wed 09h00-10h30, S104 07. Sensor (2)
• 18 Nov. Wed 09h00-10h30, S104 08. Reduction mech. and driving part design
• 25 Nov. Wed 09h00-10h30, S104 09. Fundamental theory of manipulator (1)
• 02 Dec. Wed 09h00-10h30, S104 10. Fundamental theory of manipulator (2)
松丸隆文 •
•
09 Dec. Wed 09h00-10h30, S104 11. Fundamental theory of manipulator (3)
16 Dec. Wed 09h00-10h30, S104 12. Fundamental theory of manipulator (4)
• 23 Dec. Wed (winter break)
Takafumi Matsumaru •
•
30 Dec. Wed (winter break)
06 Jan. Wed (winter break)
• 13 Jan. Wed 09h00-10h30, S104 13. Manipulator control (1)
• 20 Jan. Wed 09h00-10h30, S104 14. Manipulator control (2)
• 27 Jan. Wed 09h00-10h30, S104 15. Conclusion
1 2
• 03 Feb. Wed (supplement/makeup class, during 30 Jan. and 04 Feb.)

Force balancing between end-tip and Force balancing between end-tip and
Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015

joint (1/2) joint (2/2)

• Condition of force balancing between end-tip and joints # applied moment from end-tip to i th joint (neglecting gravity)
• Acting force to environment : (translational) f and (rotational) m m + ( ph − pi )× f
• End-tip position vector : ph
# generating torque of i th joint : axial-direction component of applied
• Joint orientation vector : ai
moment
τ i = ai ⋅ (m + ( ph − pi )× f )
τ i = ai ⋅ m + (ai × ( ph − pi )) ⋅ f　　　　　　･･･(f1)
# fundamental equation of manipulator force control : using six-
dimensional vectorτ consists of each joint torque τi
 (a1 × ( ph − p1 ))T , a1T 
  f
τ = : :  ⋅   　　　　･･･(f2)
  m
 (a6 × ( ph − p6 ))T , a6 T   
 
3 4
 Practice (2) – Joint torque
Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015

Jacobian of planar 2-d.o.f. arm

# Jacobian (Jacobi matrix) : eq. (e4) • Joint angle : θ1 = 0 deg, θ2 = 90 deg
 a × ( ph − p1 ),･･･, a6 × ( ph − p6 ) • Acting force : translation force f = (f,0,0)T, rotational force m = (0,0,0)T
J =  1 
 a1 , ･･･, a6  • Joint torque : τ1 = ?, τ2 = ?
# End-tip speed joint angle velocity : eq. (e5) y
6 by 6 matrix is Jacobian  •  •
   a × ( p − p ),･･･, a × ( p − p ) 
•  θ1   θ1  f
 ph  =   
1 h 1 6 h 6
 :  = J  :  　･(f3) l
   a1 , ･･･, a6  •  •
 ωh  θ 
 θ6   6
    l x
# End-tip force joint torque : eq. (f2)
6 by 6 matrix is transposed-Jacobian（ヤコビアンの転置行列）
 τ1   (a1 × ( ph − p1 )) , a1 
T T
  f f
 : = : = J Τ   　･･（f4）
:  ⋅   　
 τ   (a × ( p − p ))T , a T   m  m
 6  6 h 6 6  5 6

Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015

joint torque of planar 2-d.o.f. arm Chapter 12

• #1 rotational direction vector a1 = Basic principles of manipulator : vector approach for kinetics
• #2 rotational direction vector a2 = – part IV : Inverse kinematics and end-tip orientation
• End-tip to #1 displacement vector ph − p1 = 1. Inverse kinematics（逆運動学）
• End-tip to #2 displacement vector ph − p2 = 1. Inverse problem
1. Practice (1) – Inverse kinematics of planar 2-d.o.f. arm
from eq. (f1) of force balancing at i th joint 2. ex.) Inverse kinematics (position) of articulated manipulator
2. Inverse kinematics (velocity) end-tip joint
• #1 rotation torque
1. ex.) Inverse kinematics (velocity) of articulated manipulator
τ1 =
2. Description of end-tip orientation
1. Direction cosine
• #2 rotation torque 2. End-tip direction angle
τ2 = 3. Euler angle
4. Roll-pitch-yaw

7 8
 Practice – Inverse kinematics
Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015

Inverse problem（逆問題） of planar 2-d.o.f. arm

# Inverse Kinematics（逆運動学） # Position : End-tip to joint
Position : from end-tip position and orientation to each joint angle. (trigonometric function ) A
Velocity : from end-tip translational and rotational speed to each
c b
joint revolution speed. B C
# Conversion equation about position or speed
a
No general expression.
Dependent on individual manipulator structure (joint configuration, Low of cosine
d.o.f. arrangement). a 2 = b 2 + c 2 − 2bc cos A
Here, property of trigonometric ratio
Number of d.o.f. is six. = number of joint is six (rotational joint). cos(π − A) = − cos A
Rotation axes of three d.o.f. at end-tip side are concentrated at a point
(wrist).
Solution must be specified（解が一意に求められるはず）.
For example, articulated manipulator, Stanford manipulator, etc. 9 10

Inverse kinematics Ex.) Inverse kinematics (position)

Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015

of planar 2-d.o.f. arm of articulated manipulator

# Joint-1 angle √(px2+py2) l2 Articulated manipulator with six rotational joints
θ1 = U θ2 From end-tip position / orientation [X6, Y6, Z6, P6]
T to joint angles [θ1, θ2, θ3, θ4, θ5, θ6]
S θ1 l1

# Joint-2 angle
θ2 = A
c b
B C
a
a 2 = b 2 + c 2 − 2bc cos A
cos(π − A) = − cos A
11 12
Inverse kinematics (position) Inverse kinematics (position)
Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015

of articulated manipulator (1/7) of articulated manipulator (2/7)

# End-tip position : P6 P6 － l4 Z6 ＝ l1 Z0＋l2 Z2＋l3 Z3 ･･･(d1)
On extension（延長線上） of joint-6 rotation axis. To calculate X0-direction component, X0 is scalar produced both sides
End-tip link is consistent with joint-6 rotation axis. of eq. (d1).
(P6 － l4 Z6)･X0 ＝ (l1 Z0＋l2 Z2＋l3 Z3)･X0
# Wrist position : PW l2 Z2＋l3 Z3 ＝ ｛l2 s2 ＋ l3 (s3 c2 ＋ c3 s2) ｝X1
Calculated from end-tip position. ＋｛l2 c2＋l3 (c2 s2－s3 c2)｝Z1,
PW ＝ P6 － l4 Z6 X1･X0 ＝ cosθ1 , Z1･X0 ＝ 0, Z0･X0 ＝ 0,
Calculated from 3 d.o.f. at base side. ∴ (P6 － l4 Z6)･X0 ＝ ｛l2 s2 ＋ l3 (s3 c2 ＋ c3 s2) ｝c1
P4 ＝ l1 Z0 ＋ l2 Z2 ＋l3 Z3 In similar way, Y0 is scalar produced on both sides of eq. (d1).
Equal from both sides. ∴ (P6 － l4 Z6)･Y0 ＝ ｛l2 s2 ＋ l3 (s3 c2 ＋ c3 s2) ｝s1
P6 － l4 Z6 ＝ l1 Z0＋l2 Z2＋l3 Z3 ･･･(d1) Therefore,

θ1 = tan −1
(P6 − l4 Z6 )⋅ Y0 
 (P6 − l4 Z6 )⋅ X 0 
13 14

Inverse kinematics (position) Inverse kinematics (position)

Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015

of articulated manipulator (3/7) of articulated manipulator (4/7)

# θ3 may be calculated from distance between PW and P1. # θ2 may be calculated from Z-direction component of PW.
l1 Z0 (＝P1 ) is reduced from both sides of eq. (d1). l3 θ3 Z0 is scalar produced on both sides of eq. (d1).
PW
P6 － l4 Z6 － l1 Z0 ＝ l2 Z2＋l3 Z3 (P6 － l4 Z6)･Z0 ＝ (l1 Z0＋l2 Z2＋l3 Z3)･Z0
Vector length of both sides is made square product. ＝ [l1 Z0＋｛l3 s3 c2＋(l3 c2＋l2) s2｝X1
|P6 － l4 Z6 － l1 Z0|2 l2 ＋｛－l3 s3 s2＋(l3 c3＋l2) c2｝Z1 ] Z0
＝ |l2 Z2＋l3 Z3|2 where, Z0･Z0＝Z1･Z0＝1, X1･Z0＝0
＝ l22 (Z2･Z2) ＋ l32 (Z3･Z3) ＋ 2 l2 l3 (Z2･Z3) ∴ (P6 － l4 Z6)･Z0 －l1 ＝－l3 s3 s2＋(l3 c3＋l2) c2
P1 PW
＝ l 2 2 ＋ l 3 2 ＋ 2 l 2 l 3 c3 while,
∵ Z2･Z2＝Z3･Z3＝1, A　 = (l3 s3 ) + (l3 c3 + l2 )
2 2

Z2･Z3＝Z2 (s3 X2＋c3 Z2)＝s3 Z2 X2＋c3 Z2 Z2 ＝ c3 l c +l

tan α = 3 3 2
Therefore, − l 3 s3 P1
( ))
therefore,
(
 1  θ2
θ 3 = cos−1 2
P6 − l4 Z 6 − l1Z 0 − l2 2 + l32  (P6 − l4 Z6 ) ⋅ Z0 − l1 　 = sin (θ2 + α )
 2l2l3  A
(P − l Z ) ⋅ Z 0 − l1  − tan −1  l3c3 + l2
θ 2 = sin −1  6 4 6 

15  A   − l
16 s
3 3
Inverse kinematics (position) Inverse kinematics (position)
Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015

of articulated manipulator (5/7) of articulated manipulator (6/7)

[X3, Y3, Z3] is calculated from θ1, θ2, θ3. # θ4 is calculated from X3-direction component and Y3-direction
θ4, θ5, θ6 is calculated from relationship between [X6, Y6, Z6] and [X3, Y3, component of Z5.
Z3]. X4 ＝X3 cosθ4＋Y3sinθ4 and Z4＝Z3 are assigned to joint-5 recurrence
equation: Z5 ＝ X4 sinθ5＋Z4 cosθ5.
# θ5 is calculated from scalar product of Z4 and Z5. Z5 ＝ (X3 cosθ4＋Y3 sinθ4) sinθ5＋Z3cosθ5
• Z4 is scalar produced on both sides of joint-5 recurrence equation: Z5 X3 is scalar produced on both sides.
＝ X4 sinθ5＋Z4 cosθ5 PW Z Z5･X3 ＝ (X3･X3 cosθ4 ＋ Y3･X3 sinθ4) sinθ5 ＋ Z3･X3 cosθ5
4
Z5･Z4 ＝ (X4 sinθ5＋Z4 cosθ5)･Z4 θ5 ＝ cosθ4 sinθ5
＝ (X4･Z4) sinθ5＋(Z4･Z4) cosθ5 Z5 Y3 is scalar produced on both sides.
＝ 0 ×sinθ5＋ 1 ×cosθ5 ＝ cosθ5 Z5･Y3 ＝ (X3･Y3 cosθ4 ＋ Y3･Y3 sinθ4) sinθ5 ＋ Z3･Y3 cosθ5
here, Z5＝Z6, Z4＝Z3, ＝ sinθ4 sinθ5
Therefore,
Therefore,
∴ θ4 ＝ tan-1(Z6･Y3 / Z6･X3)
∴ θ5 ＝ cos-1 (Z6･Z3)

17 18

Inverse kinematics (position) Inverse kinematics (velocity)

Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015

of articulated manipulator (7/7) end-tip joint

# θ6 is calculated from X5-direction component and Y5-direction # Inverse Kinematics (velocity)
component of X6. From end-tip d
Joint-6 recurrence equation: X6 ＝ X5 cosθ6＋Y5 sinθ6. Positional time change (translational, 3-dimension) PE
dt
X5 is scalar produced on both sides. Postural time change (rotational, 3-dimension) ωE
X6･X5 ＝ X6･X5 cosθ6＋Y5･X5 sinθ6 ＝ cosθ6 To joints
Y5 is scalar produced on both sides. Time change of joint angle (N-dimensional vector) ⋅ ⋅ ⋅
X6･Y5 ＝ X6･Y5 cosθ6＋Y5･Y5 sinθ6 ＝ sinθ6 θ1 , θ 2 ,･････, θ N
Therefore,
∴ θ6 ＝ tan-1(X6･Y5 / X6･X5) # Conversion equation about position or speed
No general expression.
Dependent on individual manipulator structure
(joint configuration, d.o.f. arrangement).

19 20
Ex.) Inverse kinematics (velocity) Inverse kinematics (velocity)
Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015

of articulated manipulator of articulated manipulator (1/3)

Articulated manipulator with six rotational joints In this case，
From end-tip speed to joint speed Three rotation axes at wrist are concentrated at a point (wrist).
p6 and P6 − P5 = P6 − P4 are the same direction (rotate around long
axis of end-tip link)
P6 − ω6 × ( P6 − P4 ) is function only of • • •
d (translation speed of origin of wrist coordinate system)
dt θ1, θ 2, θ 3
• • • • • •
ω6 = Z1 θ1 + Y2 θ 2 + Y3 θ 3 + Z 4 θ 4 + Y5 θ 5 + Z 6 　
θ 6 　･･(g1)
• • •
P6 = Z1 × ( P6 − P1 )θ1 + Y2 × ( P6 − P2 )θ 2 + Y3 × ( P6 − P3 )θ 3
d
dt
• •
+ Z 4 × ( P6 − P4 )θ 4 + Y5 × (P6 − P5 )θ 5
　　　

∴ 　 P6 − ω6 × ( P6 − P4 )
d
dt
• • •
= Z1 × ( P4 − P1 )θ1 + Y2 × ( P4 − P2 )θ 2 + Y3 × ( P4 − P3 )θ 3 　･･(g2)
21 22
　

Inverse kinematics (velocity) Inverse kinematics (velocity)

Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015

of articulated manipulator (2/3) of articulated manipulator (3/3)

• • •
Y2 = Y3 is scalar produced on both sides of eq. (g2). θ1, θ2, θ3 are assigned to eq. (g2), then move to the left side.
•
d  • • • • • •
 P6 − ω6 × ( P6 − P4 ) ⋅ Y2 = ( Z1 × ( P4 − P1 )) ⋅ Y2 θ1 ω6 − Z1 θ1 − Y2 θ2 − Y3 θ3 = Z 4 θ4 + Y5 θ5 + Z 6 θ6
 dt 
•
d  Y5 is scalar produced on both sides, and calculate θ ：Z4･Y5=Z6･Y5=0.
•  P6 − ω6 × ( P6 − P4 ) ⋅ Y2 •  • • •  5
θ1 =  dt  θ5 =  ω6 − Z1 θ1 − Y2 θ2 − Y3 θ3  ⋅ Y5
(Z1 × (P4 − P1 )) ⋅ Y2  
• •
θ1 item of eq. (g2) is moved to the left side, then make scalar product X5 is scalar produced on both sides, and calculate θ 4：X4･Y5=X6･Y5=0.
with (P4 - P3).  d •   • • • 
 P6 − ω6 × ( P6 − P4 ) − Z1 × ( P4 − P1 )θ1  ⋅ ( P4 − P3 ) •
 ω6 − Z1 θ1 − Y2 θ2 − Y3 θ3  ⋅ X 5
• dt
θ2 =   θ4 =  
Z4 ⋅ X5
(Y3 × (P4 − P2 )) ⋅ (P4 − P3 )
•
(P4 - P2) is scalar produced on both sides of eq. (g2). All items are assigned, and calculate θ 6 .
•  •  • • • • • 
d
 P6 − ω6 × ( P6 − P4 ) − Z1 × (P4 − P1 )θ1  ⋅ (P4 − P2 ) θ6 =  ω6 − Z1 θ1 − Y2 θ2 − Y3 θ3 − Z 4 θ4 − Y5 θ5  ⋅ Z 6
• dt  
θ3 =  
(Y3 × (P4 − P3 )) ⋅ (P4 − P2 ) 23 24
Description of end-tip orientation
Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015

of manipulator Direction cosine (1/2)

# Direction cosine 方向余弦 Direction of coordinate axes
Orientation of coordinate axes of end-tip coordinate system to of coordinate system fixed at end-tip H
basement coordinate system = column vector of translation matrix C : CXH z0
to coordinate system fixed at shoulder S
# End-tip direction angle (α, β, γ) 手先方向角 : CXO
Intuitive : α to right side, β from horizontal plane, and γ rotate from y0
initial posture.
x0
# Euler angle (φ, θ, ψ) オイラー角 In case that all joints are
To basement coordinate system : φ around Z-axis, θ around Y-axis, rotational joint, it means CCH.
and ψ around Z-axis.  h11 h12 h13 

# roll, pitch, yaw (φ, θ, ψ) ロール・ピッチ・ヨー

[
r C
]
XO = CH C
[
r C
XH ] = h21 h22 h23 
r
[
C
]
X H 　･･(c1)
 h31 h32 h33 
To basement coordinate system : φ around Z-axis, θ around Y-axis,
and ψ around X-axis. Coordinate axes of CXH : xH, yH, zH
25 26
Coordinate axes of CXO : xO, yO, zO

Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015

Direction cosine (2/2) End-tip direction angle (1/3)

Direction cosine of xH axis of coordinate CXH to coordinate CXO (α, β, γ) intuitive method
[lH1,mΗ1,nH1 ] T
= C H [1,0,0] = [h11, h21, h31 ]
T T α to right side, β from horizontal plane, γ rotate from initial.
Initial state (0)
Direction cosine of yH axis of coordinate CXH to coordinate CXO -π/2 rotate on the spot (rot-Z)
[lH2,mH2,nH2 ]T = C H [0,1,0]T = [h12 , h22 , h32 ]T π/2 up on horizontal plane (rot-X)
Direction cosine of zH axis of coordinate CXH to coordinate CXO State (i) : on horizontal plane
[lH3,mH3,nH3 ]T = C H [0,0,1]T = [h13 , h23 , h33 ]T angle α (rot-Y)
State (ii) : on horizontal plane
angle -β (rot-X)
To sum up, end-tip direction matrix
State (iii) : under horizontal plane
 l H1 l H2 l H3 
angle -γ (rot-Z)
K ≡ mH1 mH2 mH3  = C H 　　･･･(c2) Final state
 nH1 nH2 nH3 

27 28
 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015

End-tip direction angle (2/3) End-tip direction angle (3/3)

Coordinate transformation matrix Eq. (c3) is equal to eq. (c4), …………………..then,
 π  π  − cα ⋅ cβ = c13  c23 
Az  −  Ax   Ay (α )Ax (− β )Az (− γ ) α = tan −1 
 2 2 − sα ⋅ cβ = c23  c13 
 0 1 0 1 0 0   cα 0 sα  1 0 0   cγ sγ 0 sβ = c33 β = sin −1 (c33 )
= − 10 0 0 0 − 1  0 1 0  0 cβ sβ  − sγ cγ 0 − cβ ⋅ sγ = c31  c31 
 0 0 1 0 1 0  − sα 0 cα  0 − sβ cβ   0 0 1 γ = tan −1 − 
cβ ⋅ cγ = c32
 c32 
 sα ⋅ cγ − cα ⋅ sβ ⋅ sγ sα ⋅ sγ + cα ⋅ sβ ⋅ cγ − cα ⋅ cβ 
= − cα ⋅ cγ − sα ⋅ sβ ⋅ sγ − cα ⋅ sγ + sα ⋅ sβ ⋅ cγ − sα ⋅ cβ  　･･(c3) Therefore, end-tip direction angle D (α, β, γ) is,
T
 − cβ ⋅ sγ cβ ⋅ cγ sβ   c   c 
D(α , β , γ ) =  tan −1  23 , sin −1 (c33 ), tan −1  − 31 
It must be equal to transformation (a5) by rotation angle θ1･･･θn   c13   c32 
 c11 c12 c13 
1
C n = C1 ⋅ ⋅ ⋅ C n = c21 c22 c23  　　　･･(c4)
c31 c32 c33  29 30

Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015

Euler angle (1/2) Euler angle (2/2)

(Φ, θ, ψ) in case of z-y-z system (Φ, θ, ψ)
 X  around − z  x'  around − y '  x"  around − z"  x 
       
 Y  →→→  y '  →→→  y"  →→→  y 
Z φ  z'  θ  z"  ψ z
       

 X   cφ - sφ 0  cθ 0 sθ cψ - sψ 0  x 
      
 Y  =  sφ cφ 0  0 1 0  sψ cψ 0  y 
z-x-z system
Z  0 0 1  - sθ 0 cθ 0 0 1  z 
  
 cφcθcψ- sφsψ - cφcθsψ- sφcψ cφsθ x 
  
　　　 =  sφcθcψ+ cφsψ - sφcθsψ+ cφcψ sφsθ y 
 - sθcψ sθsψ cθ  z 
 31 32
 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015 Bio-Robotics and Human-Mechatronics (T. Matsumaru) Lab 2015

Roll-pitch-yaw (1/2) Roll-pitch-yaw (2/2)

(Φ, θ, ψ) Ship sailing along Z-axis Hand extending along Z-axis

 X  around − z  x'  around − y '  x"  around − x"  x 

       
 Y  →→→  y '  →→→  y" →→→  y 
Z φ  z'  θ  z"  ψ z
       
 X   cφ - sφ 0  cθ 0 sθ 1 0 0  x 
      
 Y  =  sφ cφ 0  0 1 0  0 cψ - sψ y 
Z  0 0 1  - sθ 0 cθ 0 sψ cψ  z 
  
 cφcθ cφsθsψ- sφcψ cφsθcψ+ sφsψ x 
  
　　　 =  sφcθ sφsθsψ+ cφcψ sφsθcψ- cφsψ y 
 - sθ cθsψ cθcψ  z  33 34
  