BÀI TẬP LỚN Kỹ thuật Robot: Robot Vertical Articulated TV800

TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI

BÀI TẬP LỚN MÔN HỌC
Kỹ thuật Robot: Robot Vertical
Articulated TV800
Ngành Kỹ thuật điều khiển và tự động hóa
Chun ngành Tự động hóa cơng nghiệp

Giảng viên hướng dẫn:

TS. Nguyễn Phạm Thục Anh

Bộ mơn:

Tự động hóa cơng nghiệp

Viện:

Điện

Nhóm sinh viên:

Nhóm 9

HÀ NỘI, 6/2018

1


MỤC LỤC

CHƯƠNG 1. YÊU CẦU BÀI TOÁN VÀ ROBOT TV800 ............................... 3
1.1

Yêu cầu bài toán ......................................................................................... 3

1.2

Robot Vertical Articulated TV800 ............................................................. 3

1.3

Ứng dụng trong công nghiệp ..................................................................... 4

1.4

Thông số kĩ thuật ........................................................................................ 4

CHƯƠNG 2. ĐỘNG HỌC THUẬN VỊ TRÍ ..................................................... 7
CHƯƠNG 3. TÍNH TỒN MA TRẬN JACOBY VÀ VIẾT CHƯƠNG
TRÌNH TRÊN MATLAB .................................................................................. 13
3.1

Các bước tính tốn ma trận Jacoby theo 𝑱𝑯 ............................................ 13

3.2

Tính tốn ma trận của robot TV800 ......................................................... 14

CHƯƠNG 4. ĐỘNG HỌC NGƯỢC VỊ TRÍ................................................... 24
4.1

Tổng quan động học ngược vị trí ............................................................. 24

4.2. ........................................................................................................................ 24
CHƯƠNG 5. THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG CỦA CÁC KHỚP
THEO QUỸ ĐẠO DẠNG BẬC 3 ..................................................................... 27
5.1

Thiết kế quỹ đạo PTP (Point to Point) qua 2 điểm đơn ........................... 27

5.2

Thiết kế quỹ đạo PTP qua điểm trung gian .............................................. 29

CHƯƠNG 6. XÂY DỰNG MƠ HÌNH ĐỘNG LỰC HỌC CHO ĐỐI TƯỢNG
TRÊN TOOLBOX SIMSCAPE MULTIBODY/MATLAB ........................... 32
6.1

Thiết kế mơ hình 3D cho cánh tay robot .................................................. 32

6.2

Liên kết với Matlab .................................................................................. 32

6.3

Mô phỏng chuyển động............................................................................ 33

2

CHƯƠNG 1. YÊU CẦU BÀI TOÁN VÀ ROBOT TV800
1.1 Yêu cầu bài tốn
1. Giới thiệu về Robot nhóm nghiên cứu, các ứng dụng trong cơng nghiệp, kết
cấu cơ khí, các thơng số kỹ thuật cơ bản. u cầu có hình ảnh hoặc clip
hoạt động.
2. Tính tốn động học thuận vị trí Robot. Xây dựng chương trình phần mềm
trên MATLAB để nhập dữ liệu, hiển thị kết quả.
3. Tính tốn ma trận Jacoby (thơng qua JH) và viêt chương trình trên
MATLAB.
4. Tính tốn động học đảo vị trí Robot.
5. Thiết kế quỹ đạo chuyển động cho các khớp của Robot theo quỹ đạo dạng
bậc 3.
6. Thiết kế điều khiển chuyển động cho Robot theo thuật tốn PID.
7. Xây dựng mơ hình động lực học cho đối tượng trên ToolBox Simscape/
MATLAB.

1.2 Robot Vertical Articulated TV800
Robot được chọn của báo cáo này là robot TV800 của hãng Toshiba
Robot TV800 là Robot tác động nhanh, linh hoạt, nhỏ gọn và đáng tin cậy. Đây là
một loại Robot hoạt động với nhiều cài đặt. Nó cũng cung cấp rất nhiều ứng dụng,
với hiệu suất sử dụng cao, đảm bảo những yêu cầu về chất lượng, thời gian hoàn
vốn ngắn.
TV800 là một robot tác động tốc độ cao với 6 trục sử dụng hệ thống phát hiện vị
trí tuyệt đối, với động cơ AC mạnh mẽ ở trung tâm, mang lại hiệu suất cao và độ
tin cậy cho việc xử lý thực phẩm, chọn, đóng gói và các ứng dụng xử lý vật liệu
tốc độ cao khác.
TV800 có tổng chiều dài cánh tay là 800mm, tầm với là 892mm và tốc độ tối đa
tổng hợp là 8,06 mét / giây.
Robot có thời gian chu kỳ tối đa từ 0,4 đến 0,5 giây, độ lặp lại ± 0,02mm và trọng

tải tối đa là 5 kg.
3


Hình 1. Robot TV800 ngồi thực tế.

TV800 được thiết kế cứng và thẳng, điều này dẫn đến tiếng ồn khi làm việc thấp,
thời gian bảo trì lâu. Ngồi ra nó còn được thiết kế nhỏ gọn, cổ tay mỏng, hiệu suất
hoạt động cao ngay cả trong những vị trí khó.
1.3 Ứng dụng trong công nghiệp
Robot TV800 được ứng dụng cho các dây chuyền sản xuất tự động, hiện nay các
lĩnh vực phổ biến nhất là:
• Xếp/dỡ máy và lắp ráp
• Cơng nghệ gia cơng lắp ráp.
• Phun sơn.
1.4 Thơng số kĩ thuật
- Số bậc tự do: 6
4


- Kiểu khớp: 6 khớp quay
- Vùng không gian làm việc:

Hình 2. Khơng gian làm việc của robot TV800

Ta có bảng thông số kĩ thuật của robot:
SPECIFICATIONS
Trục

Vùng hoạt

Tốc độ max

Momen

Số trục

6

động

(o/sec)

quán tính

Tải tối đa[kg]

5

Sai số

±0.02
892

cho phép
J1

±170

237

-

Tầm với

J2

+150/−100

240

-

[mm]

J3

+167/−127

288

-

Khối

J4

±190

350.5

0.3kg · m 2

lượng[kg]

45

5


J5

±120

484

0.3kg · m 2

J6

±360

576

0.05kg · m 2

Bảng thông số kĩ thuật robot TV800

6

CHƯƠNG 2. ĐỘNG HỌC THUẬN VỊ TRÍ

Hình 3. Mơ hình robot

Đây là bước cơ sở cho việc thiết kế sơ bộ robot, từ đó có thể giải bài tốn điều
khiển robot theo các quỹ đạo. Từ đây ta mới có đủ các thông số để điều khiển robot
theo một quỹ đạo cho trước hoặc với lực cho trước ta thu được một quỹ đạo chuyển
động nhất định. Dưới đây là phần tính tốn động học cho Robot TV800

Bài tốn động học thuận:
Trục

Giới hạn chuyển động

Tốc độ

±170°

237°/s

-100 ∼ +150°

240°/s

-127 ∼ +167°

288°/s

±190°

350.5°/s

1

2

3

4

7


5
±120°

484°/s

±360°

576°/s

6

1. Bộ thông số D-H
Ta xây dựng mối quan hệ động học thông qua bộ thông số D-H:
Theo Denavit & Hartenberg, hai ông đã đề xuất dùng ma trận thuần nhất 4x4 để
mô tả quan hệ giữa 2 khâu liên tiếp trong cơ cấu không gian. Trước hết, xác định
bộ thông số cơ bản giữa 2 trục quay của 2 khớp động i+1 và i:

- 𝑎𝑖 là độ dài đường vng góc chung giữa 2 trục khớp động i+1 và i
- 𝛼𝑖 là góc chéo giữa 2 trục khớp động i+1 và i,
- 𝑑𝑖 là khoảng cách đo dọc trục khớp động i kể từ đường vng góc chung giữa
trục khớp động i+1 và trục khớp động i tới đường vng góc chung giữa trục khớp
động i và trục khớp động i-1.
- 𝜃𝑖 là góc giữa 2 đường vng góc nói trên.
Biến khớp:
- Nếu khớp động i là khớp quay thì 𝜃𝑖 là biến khớp
- Nếu khớp động i là tịnh tiến thì 𝑑𝑖 là biến khớp
2. Thiết lập hệ tọa độ

8


Thiết lập hệ tọa độ như hình
Khâu

𝜃𝑖 (°)

𝑑𝑖 (mm)

𝑎𝑖 (mm)

𝛼𝑖 (°)

Khớp

1

𝜃1

𝑑1 =116

𝑎1 =0

-90

R

2

−90 + 𝜃2

𝑑2 =0

𝑎2 =380

0

R

3

𝜃3

𝑑3 =0

𝑎3 =100

-90

R

4

𝜃4

𝑑4 =420

𝑎4 =0

90

R

5

−90 + 𝜃5

𝑑5 =0

𝑎5 =0

-90

R

6

𝜃6

𝑑6 =80

𝑎6 =0

0

R

Các ma trận A được xác định bằng ma trận biến đổi tọa độ thuần nhất tổng quát :
9


C i

S
Ai =   i
 0

 0

− S C i
i
C C i
i
S i

S S i
i
−C S i

i
C i

0

0

aC 
i 
aS 
i 
d 



1 

C1

S
A = 1
1
0

0

−S 0 
1

0 C

0
1

−1 0 d1 

0
0
1

C3

S
A = 3
3
0

0

− S a3C 
3
3
0 C
a3 S 
3
3
−1
0
0 

0

0
1 

C4

S
A = 4
4
 0

 0

4
0 −C
4
1
0
0
0

 S5

 −C
A = 5
5
 0

 0

0

5

0 −S
0
5

−1 0
0

0
0 1

C6

S
A = 6
6
 0

 0

−S
0 0
6

−C 0 0 
6

0
1 d6 


0
0 1

0

0

0

C

 S2

 −C
A = 2
2
 0

 0

C

2

S

2
0
0

0

S

0 a2 S 
2
0 a2C 
2
1
0 

0
1 
0

0

d4 

1

Từ các ma trận biến đổi giữa các trục, ta sẽ xác định được hàm truyền RTH của
robot ( hay chính là ma trận chuyển đổi giữa trục 0 và trục 6 của robot)

 nx
n
0
T6 =  y
 nz


0

ox
oy
oz
0

ax
ay
az
0

px 
p y 
= A1. A2 . A3 . A4 . A5 . A6
pz 

1

Trong đó :
𝑠1 = 𝑠𝑖𝑛( 𝜃1 ); 𝑐1 = 𝑐𝑜𝑠( 𝜃1 )
𝑠2 = 𝑠𝑖𝑛( 𝜃2 ); 𝑐2 = 𝑐𝑜𝑠( 𝜃2 )
𝑠3 = 𝑠𝑖𝑛( 𝜃3 ); 𝑐3 = 𝑐𝑜𝑠( 𝜃3 )
𝑠4 = 𝑠𝑖𝑛( 𝜃4 ); 𝑐4 = 𝑐𝑜𝑠( 𝜃4 )
𝑠5 = 𝑠𝑖𝑛( 𝜃5 ); 𝑐5 = 𝑐𝑜𝑠( 𝜃5 )
10


𝑠6 = 𝑠𝑖𝑛( 𝜃6 ); 𝑐6 = 𝑐𝑜𝑠( 𝜃6 )

𝑠23 = 𝑠𝑖𝑛( 𝜃2 + 𝜃3 ); 𝑐23 = 𝑐𝑜𝑠( 𝜃2 + 𝜃3 ); 𝑐45 = 𝑐𝑜𝑠( 𝜃4 + 𝜃5 );
𝑐4−5 = 𝑐𝑜𝑠( 𝜃4 − 𝜃5 )

Sử dụng công cụ matlab để nhập và nhân các ma trận. Sau khi dùng hàm simplify
để rút gọn, ta sẽ được thành phần của ma trận như sau :

nx = s6 (c4 s1 - s4 (c1c2 s3 + c1c3 s2 ))

(

)

+ c6 s5 (s1s4 + c4 (c1c2 s3 + c1c3 s2 )) + c5 (c1s2 s3 - c1c2 c3 )

ny = - s6 (c1c4 + s4 (c2 s1s3 + c3 s1s2 ))

(

)

- c6 s5 (c1s4 - c4 (c2 s1s3 + c3 s1s2 )) - c5 (s1s2 s3 - c2 c3 s1 )

nz = c6 (s23c5 + c23c4 s5 ) - c23 s4 s6
o x = c6 (c4 s1 - s4 (c1c2 s3 + c1c3 s2 ))

(

)

- s6 s5 (s1 s4 + c4 (c1c2 s3 + c1c3 s2 )) + c5 (c1s2 s3 - c1c2 c3 )

(

)

oy = s6 s5 (c1s4 - c4 (c2 s1s3 + c3 s1s2 ))- c5 (s1s2 s3 - c2 c3 s1 )
- c6 (c1c4 + s4 (c2 s1s3 + c3 s1s2 ))
oz = - s6 (s23c5 + c23c4 s5 ) - c23c6 s4
ax = c5 (s1 s4 + c4 (c1c2 s3 + c1c3 s2 )) - s5 (c1 s2 s3 - c1c2 c3 )
a y = - c5 (c1 s4 - c4 (c2 s1 s3 + c3 s1 s2 )) - s5 (s1 s2 s3 - c2 c3 s1 )

az = c23c4 c5 - s23 s5

(

)

px = d6 c5 (s1s4 + c4 (c1c2 s3 + c1c3 s2 ))- s5 (c1s2 s3 - c1c2 c3 )
- d 4 (c1s2 s3 - c1c2 c3 )+ a2 c1s2 + a3c1c2 s3 + a3c1c3 s2

11


(

)

p y = a2 s1s2 - d 4 (s1s2 s3 - c2 c3 s1 )- d6 c5 (c1s4 - c4 (c2 s1s3 + c3 s1s2 ))+ s5 (s1s2 s3 - c2c3 s1 )
+ a3c2 s1s3 + a3c3 s1s2
pz = d1 + a3c23 - d4 s23 + a2c2 + 0,5(d6c23c45 )- d6 s23s5 + 0,5(d6c4- 5c23 )

12


CHƯƠNG 3. TÍNH TỒN MA TRẬN JACOBY VÀ VIẾT CHƯƠNG
TRÌNH TRÊN MATLAB
3.1 Các bước tính tốn ma trận Jacoby theo 𝑱𝑯
Thuật toán gồm 3 bước:

Bước 1: Xác định ma trận 𝑇𝑛𝑖 (𝑖 = 0 → 𝑛 − 1)
𝑇𝑛𝑖 =

𝑖

𝑛𝑥
𝑖
𝑛𝑦

𝑖

𝑜𝑥
𝑖
𝑜𝑦

𝑖

𝑎𝑥
𝑖
𝑎𝑦

𝑝𝑥
𝑖
𝑝𝑦

𝑖

𝑖

𝑖

𝑖

𝑛𝑧
[ 0

𝑜𝑧
0

𝑎𝑧
0

𝑖

𝑝𝑧
1 ]

Bước 2: Xác định ma trận 𝐽𝐻

𝐻
𝐽6𝑛

𝜕 𝐻 𝑝𝑥
𝜕𝑞1
𝜕 𝐻 𝑝𝑦
𝜕𝑞1
𝜕 𝐻 𝑝𝑧
𝜕 𝐻 𝑝𝑥
𝜕𝑞
=
= 𝐻 1
𝜕𝑄
𝜕 Φ𝑥
𝜕𝑞1
𝜕 𝐻 Φ𝑦
𝜕𝑞1
𝜕 𝐻 Φ𝑧
[ 𝜕𝑞1

𝜕 𝐻 𝑝𝑥
𝜕𝑞2
𝜕 𝐻 𝑝𝑦
𝜕𝑞2
𝜕 𝐻 𝑝𝑧
𝜕𝑞2
𝜕 𝐻 Φ𝑥
𝜕𝑞2
𝜕 𝐻 Φ𝑦
𝜕𝑞2
𝜕 𝐻 Φ𝑧
𝜕𝑞2

…
…
…
…
…
…

𝜕 𝐻 𝑝𝑥
𝜕𝑞𝑛
𝜕 𝐻 𝑝𝑦
𝜕𝑞𝑛
𝜕 𝐻 𝑝𝑧
𝜕𝑞𝑛
𝜕 𝐻 Φ𝑥
𝜕𝑞𝑛
𝜕 𝐻 Φ𝑦
𝜕𝑞𝑛
𝜕 𝐻 Φ𝑧
𝜕𝑞𝑛 ]

Khi (i+1) là khớp quay, biến khớp 𝜃𝑖+1
13


Sử dụng ma trận 𝑇𝑛𝑖
𝜕 𝐻 𝑝𝑥
= 𝑖𝑛𝑦 𝑖𝑝𝑥 − 𝑖𝑛𝑥 𝑖𝑝𝑦
𝜕𝜃𝑖+1
𝜕 𝐻 𝑝𝑦

= 𝑖𝑜 𝑖𝑝𝑥 − 𝑖𝑜 𝑖𝑝𝑦
𝜕𝜃𝑖+1
𝜕 𝐻 𝑝𝑧
= 𝑖𝑎𝑦 𝑖𝑝𝑥 − 𝑖𝑎𝑥 𝑖𝑝𝑦
𝜕𝜃𝑖+1
𝜕 𝐻 Φ𝑥
= 𝑖 𝑛𝑧
𝜕Φ𝑖+1
𝜕 𝐻 Φ𝑦
= 𝑖𝑜𝑧
𝜕Φ𝑖+1
𝜕 𝐻 Φ𝑧
= 𝑖𝑎𝑧
𝜕Φ𝑖+1
Bước 3: Tính ma trận J
𝐽= [

𝑅𝑛0
0

0
] × 𝐽𝐻
𝑅𝑛0

3.2 Tính tốn ma trận của robot TV800
Robot TV800 có 6 khớp đều là khớp quay, ta áp dụng các bước làm phía trên cùng
các ma trận động học thuận của các khớp để tính ma trận J.
Chương trình Matlab:

Kết quả ma trận Jacoby:

14


𝐽11
𝐽21
𝐽
𝐽 = 31
𝐽41
𝐽51
𝐽61

𝐽12
𝐽22
𝐽32
𝐽42
𝐽52
𝐽62

𝐽13
𝐽23
𝐽33
𝐽43
𝐽53
𝐽63

𝐽14
𝐽24
𝐽34
𝐽44

𝐽54
𝐽64

𝐽15
𝐽25
𝐽35
𝐽45
𝐽55
𝐽65

𝐽16
𝐽26
𝐽36
𝐽46
𝐽56
𝐽66

𝐽11 = (𝑠6 ∗ (𝑐5 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑐3 − 𝑐1 ∗ 𝑠2 ∗ 𝑠3) − 𝑠5 ∗ (𝑠1 ∗ 𝑠4 + 𝑐4 ∗ (𝑐1
∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2))) + 𝑐6 ∗ (𝑐4 ∗ 𝑠1 − 𝑠4 ∗ (𝑐1 ∗ 𝑐2
∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2))) ∗ ((𝑠6 ∗ (𝑐5 ∗ (𝑐2 ∗ 𝑐3 ∗ 𝑠1 − 𝑠1 ∗ 𝑠2
∗ 𝑠3) + 𝑠5 ∗ (𝑐1 ∗ 𝑠4 − 𝑐4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2)))
− 𝑐6 ∗ (𝑐1 ∗ 𝑐4 + 𝑠4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2))) ∗ ((19
∗ 𝑐1 ∗ 𝑠2)/50 + (2 ∗ 𝑠5 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑐3 − 𝑐1 ∗ 𝑠2 ∗ 𝑠3))/25
+ (2 ∗ 𝑐5 ∗ (𝑠1 ∗ 𝑠4 + 𝑐4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2)))/25
+ (21 ∗ 𝑐1 ∗ 𝑐2 ∗ 𝑐3)/50 + 𝑐1 ∗ 𝑐2 ∗ 𝑠3 − (21 ∗ 𝑐1 ∗ 𝑠2
∗ 𝑠3)/50) − (𝑠6 ∗ (𝑐5 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑐3 − 𝑐1 ∗ 𝑠2 ∗ 𝑠3) − 𝑠5
∗ (𝑠1 ∗ 𝑠4 + 𝑐4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2))) + 𝑐6 ∗ (𝑐4
∗ 𝑠1 − 𝑠4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2))) ∗ ((19 ∗ 𝑠1
∗ 𝑠2)/50 + (2 ∗ 𝑠5 ∗ (𝑐2 ∗ 𝑐3 ∗ 𝑠1 − 𝑠1 ∗ 𝑠2 ∗ 𝑠3))/25 − (2
∗ 𝑐5 ∗ (𝑐1 ∗ 𝑠4 − 𝑐4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2)))/25

+ (21 ∗ 𝑐2 ∗ 𝑐3 ∗ 𝑠1)/50 + 𝑐2 ∗ 𝑠1 ∗ 𝑠3 − (21 ∗ 𝑠1 ∗ 𝑠2
∗ 𝑠3)/50)) + (𝑐6 ∗ (𝑐5 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑐3 − 𝑐1 ∗ 𝑠2 ∗ 𝑠3) − 𝑠5
∗ (𝑠1 ∗ 𝑠4 + 𝑐4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2))) − 𝑠6 ∗ (𝑐4
∗ 𝑠1 − 𝑠4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2))) ∗ ((𝑐6 ∗ (𝑐5 ∗ (𝑐2
∗ 𝑐3 ∗ 𝑠1 − 𝑠1 ∗ 𝑠2 ∗ 𝑠3) + 𝑠5 ∗ (𝑐1 ∗ 𝑠4 − 𝑐4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3
+ 𝑐3 ∗ 𝑠1 ∗ 𝑠2))) + 𝑠6 ∗ (𝑐1 ∗ 𝑐4 + 𝑠4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3
∗ 𝑠1 ∗ 𝑠2))) ∗ ((19 ∗ 𝑐1 ∗ 𝑠2)/50 + (2 ∗ 𝑠5 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑐3 − 𝑐1
∗ 𝑠2 ∗ 𝑠3))/25 + (2 ∗ 𝑐5 ∗ (𝑠1 ∗ 𝑠4 + 𝑐4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1
∗ 𝑐3 ∗ 𝑠2)))/25 + (21 ∗ 𝑐1 ∗ 𝑐2 ∗ 𝑐3)/50 + 𝑐1 ∗ 𝑐2 ∗ 𝑠3
− (21 ∗ 𝑐1 ∗ 𝑠2 ∗ 𝑠3)/50) − (𝑐6 ∗ (𝑐5 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑐3 − 𝑐1
∗ 𝑠2 ∗ 𝑠3) − 𝑠5 ∗ (𝑠1 ∗ 𝑠4 + 𝑐4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3
∗ 𝑠2))) − 𝑠6 ∗ (𝑐4 ∗ 𝑠1 − 𝑠4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2)))
∗ ((19 ∗ 𝑠1 ∗ 𝑠2)/50 + (2 ∗ 𝑠5 ∗ (𝑐2 ∗ 𝑐3 ∗ 𝑠1 − 𝑠1 ∗ 𝑠2
∗ 𝑠3))/25 − (2 ∗ 𝑐5 ∗ (𝑐1 ∗ 𝑠4 − 𝑐4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1
∗ 𝑠2)))/25 + (21 ∗ 𝑐2 ∗ 𝑐3 ∗ 𝑠1)/50 + 𝑐2 ∗ 𝑠1 ∗ 𝑠3 − (21
∗ 𝑠1 ∗ 𝑠2 ∗ 𝑠3)/50)) − (𝑐5 ∗ 𝑠4 ∗ (𝑠5 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑐3 − 𝑐1 ∗ 𝑠2
∗ 𝑠3) + 𝑐5 ∗ (𝑠1 ∗ 𝑠4 + 𝑐4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2)))
∗ (𝑐1^2 + 𝑠1^2) ∗ (19 ∗ 𝑠2 + 21 ∗ 𝑐2 ∗ 𝑐3 + 50 ∗ 𝑐2 ∗ 𝑠3
− 21 ∗ 𝑠2 ∗ 𝑠3))/50

15


𝐽12 = ((𝑠6 ∗ (𝑐5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2) + 𝑐4 ∗ 𝑠5 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3))
+ 𝑐6 ∗ 𝑠4 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3)) ∗ ((19 ∗ 𝑠2)/50 + (21 ∗ 𝑐2
∗ 𝑐3)/50 + 𝑐2 ∗ 𝑠3 − (21 ∗ 𝑠2 ∗ 𝑠3)/50 + (2 ∗ 𝑠5 ∗ (𝑐2 ∗ 𝑐3
− 𝑠2 ∗ 𝑠3))/25 + (2 ∗ 𝑐4 ∗ 𝑐5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2))/25)
− (𝑠6 ∗ (𝑐5 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3) − 𝑐4 ∗ 𝑠5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3
∗ 𝑠2)) − 𝑐6 ∗ 𝑠4 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2)) ∗ ((21 ∗ 𝑐2 ∗ 𝑠3)/50
− (19 ∗ 𝑐2)/50 + (21 ∗ 𝑐3 ∗ 𝑠2)/50 + 𝑠2 ∗ 𝑠3 + (2 ∗ 𝑠5

∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2))/25 − (2 ∗ 𝑐4 ∗ 𝑐5 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2
∗ 𝑠3))/25)) ∗ (𝑠6 ∗ (𝑐5 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑐3 − 𝑐1 ∗ 𝑠2 ∗ 𝑠3) − 𝑠5
∗ (𝑠1 ∗ 𝑠4 + 𝑐4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2))) + 𝑐6 ∗ (𝑐4
∗ 𝑠1 − 𝑠4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2))) − ((𝑐6 ∗ (𝑐5 ∗ (𝑐2
∗ 𝑐3 − 𝑠2 ∗ 𝑠3) − 𝑐4 ∗ 𝑠5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2)) + 𝑠4 ∗ 𝑠6
∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2)) ∗ ((21 ∗ 𝑐2 ∗ 𝑠3)/50 − (19 ∗ 𝑐2)/50
+ (21 ∗ 𝑐3 ∗ 𝑠2)/50 + 𝑠2 ∗ 𝑠3 + (2 ∗ 𝑠5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3
∗ 𝑠2))/25 − (2 ∗ 𝑐4 ∗ 𝑐5 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3))/25) − (𝑐6
∗ (𝑐5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2) + 𝑐4 ∗ 𝑠5 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3))
− 𝑠4 ∗ 𝑠6 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3)) ∗ ((19 ∗ 𝑠2)/50 + (21 ∗ 𝑐2
∗ 𝑐3)/50 + 𝑐2 ∗ 𝑠3 − (21 ∗ 𝑠2 ∗ 𝑠3)/50 + (2 ∗ 𝑠5 ∗ (𝑐2 ∗ 𝑐3
− 𝑠2 ∗ 𝑠3))/25 + (2 ∗ 𝑐4 ∗ 𝑐5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2))/25))
∗ (𝑐6 ∗ (𝑐5 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑐3 − 𝑐1 ∗ 𝑠2 ∗ 𝑠3) − 𝑠5 ∗ (𝑠1 ∗ 𝑠4
+ 𝑐4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2))) − 𝑠6 ∗ (𝑐4 ∗ 𝑠1 − 𝑠4
∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2))) + ((𝑠5 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑐3 − 𝑐1
∗ 𝑠2 ∗ 𝑠3) + 𝑐5 ∗ (𝑠1 ∗ 𝑠4 + 𝑐4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3
∗ 𝑠2))) ∗ (𝑐2^2 + 𝑠2^2) ∗ (19 ∗ 𝑐3 ∗ 𝑠5 + 50 ∗ 𝑠3^2 ∗ 𝑠5
+ 19 ∗ 𝑐4 ∗ 𝑐5 ∗ 𝑠3 − 21 ∗ 𝑐3^2 ∗ 𝑐4 ∗ 𝑐5 − 21 ∗ 𝑐4 ∗ 𝑐5
∗ 𝑠3^2 − 50 ∗ 𝑐3 ∗ 𝑐4 ∗ 𝑐5 ∗ 𝑠3))/50
𝐽13 = − (𝑠6 ∗ (𝑐5 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑐3 − 𝑐1 ∗ 𝑠2 ∗ 𝑠3) − 𝑠5 ∗ (𝑠1 ∗ 𝑠4 + 𝑐4 ∗ (𝑐1
∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2))) + 𝑐6 ∗ (𝑐4 ∗ 𝑠1 − 𝑠4 ∗ (𝑐1 ∗ 𝑐2
∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2))) ∗ ((𝑠6 ∗ (𝑐3 ∗ 𝑐5 − 𝑐4 ∗ 𝑠3 ∗ 𝑠5) − 𝑐6
∗ 𝑠3 ∗ 𝑠4) ∗ ((21 ∗ 𝑠3)/50 + (2 ∗ 𝑠3 ∗ 𝑠5)/25 − (2 ∗ 𝑐3 ∗ 𝑐4
∗ 𝑐5)/25) − (𝑠6 ∗ (𝑐5 ∗ 𝑠3 + 𝑐3 ∗ 𝑐4 ∗ 𝑠5) + 𝑐3 ∗ 𝑐6 ∗ 𝑠4)
∗ ((21 ∗ 𝑐3)/50 + 𝑠3 + (2 ∗ 𝑐3 ∗ 𝑠5)/25 + (2 ∗ 𝑐4 ∗ 𝑐5
∗ 𝑠3)/25)) − (𝑐6 ∗ (𝑐5 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑐3 − 𝑐1 ∗ 𝑠2 ∗ 𝑠3) − 𝑠5
∗ (𝑠1 ∗ 𝑠4 + 𝑐4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2))) − 𝑠6 ∗ (𝑐4
∗ 𝑠1 − 𝑠4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2))) ∗ ((𝑐6 ∗ (𝑐3 ∗ 𝑐5
− 𝑐4 ∗ 𝑠3 ∗ 𝑠5) + 𝑠3 ∗ 𝑠4 ∗ 𝑠6) ∗ ((21 ∗ 𝑠3)/50 + (2 ∗ 𝑠3
∗ 𝑠5)/25 − (2 ∗ 𝑐3 ∗ 𝑐4 ∗ 𝑐5)/25) − (𝑐6 ∗ (𝑐5 ∗ 𝑠3 + 𝑐3 ∗ 𝑐4

∗ 𝑠5) − 𝑐3 ∗ 𝑠4 ∗ 𝑠6) ∗ ((21 ∗ 𝑐3)/50 + 𝑠3 + (2 ∗ 𝑐3
∗ 𝑠5)/25 + (2 ∗ 𝑐4 ∗ 𝑐5 ∗ 𝑠3)/25)) − (𝑠5 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑐3 − 𝑐1
∗ 𝑠2 ∗ 𝑠3) + 𝑐5 ∗ (𝑠1 ∗ 𝑠4 + 𝑐4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3
∗ 𝑠2))) ∗ ((21 ∗ 𝑐3^2 ∗ 𝑐4 ∗ 𝑐5)/50 − 𝑠3^2 ∗ 𝑠5 + (21 ∗ 𝑐4
∗ 𝑐5 ∗ 𝑠3^2)/50 + 𝑐3 ∗ 𝑐4 ∗ 𝑐5 ∗ 𝑠3)
16


𝐽14 = −(2 ∗ 𝑐5 ∗ (𝑐4^2 + 𝑠4^2) ∗ (𝑐6^2 + 𝑠6^2) ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 ∗ 𝑠4 − 𝑐4
∗ 𝑠1 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2 ∗ 𝑠4))/25
𝐽15 = −(2 ∗ (𝑐5^2 + 𝑠5^2) ∗ (𝑐6^2 + 𝑠6^2) ∗ (𝑠1 ∗ 𝑠4 ∗ 𝑠5 + 𝑐1 ∗ 𝑐5 ∗ 𝑠2
∗ 𝑠3 − 𝑐1 ∗ 𝑐2 ∗ 𝑐3 ∗ 𝑐5 + 𝑐1 ∗ 𝑐2 ∗ 𝑐4 ∗ 𝑠3 ∗ 𝑠5 + 𝑐1 ∗ 𝑐3 ∗ 𝑐4
∗ 𝑠2 ∗ 𝑠5))/25
𝐽16 = 0
𝐽21 = ((𝑐6 ∗ (𝑐5 ∗ (𝑐2 ∗ 𝑐3 ∗ 𝑠1 − 𝑠1 ∗ 𝑠2 ∗ 𝑠3) + 𝑠5 ∗ (𝑐1 ∗ 𝑠4 − 𝑐4 ∗ (𝑐2
∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2))) + 𝑠6 ∗ (𝑐1 ∗ 𝑐4 + 𝑠4 ∗ (𝑐2 ∗ 𝑠1
∗ 𝑠3 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2))) ∗ ((19 ∗ 𝑐1 ∗ 𝑠2)/50 + (2 ∗ 𝑠5 ∗ (𝑐1 ∗ 𝑐2
∗ 𝑐3 − 𝑐1 ∗ 𝑠2 ∗ 𝑠3))/25 + (2 ∗ 𝑐5 ∗ (𝑠1 ∗ 𝑠4 + 𝑐4 ∗ (𝑐1 ∗ 𝑐2
∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2)))/25 + (21 ∗ 𝑐1 ∗ 𝑐2 ∗ 𝑐3)/50 + 𝑐1 ∗ 𝑐2
∗ 𝑠3 − (21 ∗ 𝑐1 ∗ 𝑠2 ∗ 𝑠3)/50) − (𝑐6 ∗ (𝑐5 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑐3
− 𝑐1 ∗ 𝑠2 ∗ 𝑠3) − 𝑠5 ∗ (𝑠1 ∗ 𝑠4 + 𝑐4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3
∗ 𝑠2))) − 𝑠6 ∗ (𝑐4 ∗ 𝑠1 − 𝑠4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2)))
∗ ((19 ∗ 𝑠1 ∗ 𝑠2)/50 + (2 ∗ 𝑠5 ∗ (𝑐2 ∗ 𝑐3 ∗ 𝑠1 − 𝑠1 ∗ 𝑠2
∗ 𝑠3))/25 − (2 ∗ 𝑐5 ∗ (𝑐1 ∗ 𝑠4 − 𝑐4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1
∗ 𝑠2)))/25 + (21 ∗ 𝑐2 ∗ 𝑐3 ∗ 𝑠1)/50 + 𝑐2 ∗ 𝑠1 ∗ 𝑠3 − (21
∗ 𝑠1 ∗ 𝑠2 ∗ 𝑠3)/50)) ∗ (𝑐6 ∗ (𝑐5 ∗ (𝑐2 ∗ 𝑐3 ∗ 𝑠1 − 𝑠1 ∗ 𝑠2 ∗ 𝑠3)
+ 𝑠5 ∗ (𝑐1 ∗ 𝑠4 − 𝑐4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2))) + 𝑠6
∗ (𝑐1 ∗ 𝑐4 + 𝑠4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2))) + ((𝑠6 ∗ (𝑐5
∗ (𝑐2 ∗ 𝑐3 ∗ 𝑠1 − 𝑠1 ∗ 𝑠2 ∗ 𝑠3) + 𝑠5 ∗ (𝑐1 ∗ 𝑠4 − 𝑐4 ∗ (𝑐2 ∗ 𝑠1
∗ 𝑠3 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2))) − 𝑐6 ∗ (𝑐1 ∗ 𝑐4 + 𝑠4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3

+ 𝑐3 ∗ 𝑠1 ∗ 𝑠2))) ∗ ((19 ∗ 𝑐1 ∗ 𝑠2)/50 + (2 ∗ 𝑠5 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑐3
− 𝑐1 ∗ 𝑠2 ∗ 𝑠3))/25 + (2 ∗ 𝑐5 ∗ (𝑠1 ∗ 𝑠4 + 𝑐4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3
+ 𝑐1 ∗ 𝑐3 ∗ 𝑠2)))/25 + (21 ∗ 𝑐1 ∗ 𝑐2 ∗ 𝑐3)/50 + 𝑐1 ∗ 𝑐2 ∗ 𝑠3
− (21 ∗ 𝑐1 ∗ 𝑠2 ∗ 𝑠3)/50) − (𝑠6 ∗ (𝑐5 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑐3 − 𝑐1
∗ 𝑠2 ∗ 𝑠3) − 𝑠5 ∗ (𝑠1 ∗ 𝑠4 + 𝑐4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3
∗ 𝑠2))) + 𝑐6 ∗ (𝑐4 ∗ 𝑠1 − 𝑠4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2)))
∗ ((19 ∗ 𝑠1 ∗ 𝑠2)/50 + (2 ∗ 𝑠5 ∗ (𝑐2 ∗ 𝑐3 ∗ 𝑠1 − 𝑠1 ∗ 𝑠2
∗ 𝑠3))/25 − (2 ∗ 𝑐5 ∗ (𝑐1 ∗ 𝑠4 − 𝑐4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1
∗ 𝑠2)))/25 + (21 ∗ 𝑐2 ∗ 𝑐3 ∗ 𝑠1)/50 + 𝑐2 ∗ 𝑠1 ∗ 𝑠3 − (21
∗ 𝑠1 ∗ 𝑠2 ∗ 𝑠3)/50)) ∗ (𝑠6 ∗ (𝑐5 ∗ (𝑐2 ∗ 𝑐3 ∗ 𝑠1 − 𝑠1 ∗ 𝑠2 ∗ 𝑠3)
+ 𝑠5 ∗ (𝑐1 ∗ 𝑠4 − 𝑐4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2))) − 𝑐6
∗ (𝑐1 ∗ 𝑐4 + 𝑠4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2))) − (𝑐5 ∗ 𝑠4
∗ (𝑠5 ∗ (𝑐2 ∗ 𝑐3 ∗ 𝑠1 − 𝑠1 ∗ 𝑠2 ∗ 𝑠3) − 𝑐5 ∗ (𝑐1 ∗ 𝑠4 − 𝑐4
∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2))) ∗ (𝑐1^2 + 𝑠1^2) ∗ (19 ∗ 𝑠2
+ 21 ∗ 𝑐2 ∗ 𝑐3 + 50 ∗ 𝑐2 ∗ 𝑠3 − 21 ∗ 𝑠2 ∗ 𝑠3))/50

17


𝐽22 = ((𝑠6 ∗ (𝑐5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2) + 𝑐4 ∗ 𝑠5 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3))
+ 𝑐6 ∗ 𝑠4 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3)) ∗ ((19 ∗ 𝑠2)/50 + (21 ∗ 𝑐2
∗ 𝑐3)/50 + 𝑐2 ∗ 𝑠3 − (21 ∗ 𝑠2 ∗ 𝑠3)/50 + (2 ∗ 𝑠5 ∗ (𝑐2 ∗ 𝑐3
− 𝑠2 ∗ 𝑠3))/25 + (2 ∗ 𝑐4 ∗ 𝑐5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2))/25)
− (𝑠6 ∗ (𝑐5 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3) − 𝑐4 ∗ 𝑠5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3
∗ 𝑠2)) − 𝑐6 ∗ 𝑠4 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2)) ∗ ((21 ∗ 𝑐2 ∗ 𝑠3)/50
− (19 ∗ 𝑐2)/50 + (21 ∗ 𝑐3 ∗ 𝑠2)/50 + 𝑠2 ∗ 𝑠3 + (2 ∗ 𝑠5
∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2))/25 − (2 ∗ 𝑐4 ∗ 𝑐5 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2
∗ 𝑠3))/25)) ∗ (𝑠6 ∗ (𝑐5 ∗ (𝑐2 ∗ 𝑐3 ∗ 𝑠1 − 𝑠1 ∗ 𝑠2 ∗ 𝑠3) + 𝑠5
∗ (𝑐1 ∗ 𝑠4 − 𝑐4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2))) − 𝑐6 ∗ (𝑐1
∗ 𝑐4 + 𝑠4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2))) − ((𝑐6 ∗ (𝑐5 ∗ (𝑐2

∗ 𝑐3 − 𝑠2 ∗ 𝑠3) − 𝑐4 ∗ 𝑠5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2)) + 𝑠4 ∗ 𝑠6
∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2)) ∗ ((21 ∗ 𝑐2 ∗ 𝑠3)/50 − (19 ∗ 𝑐2)/50
+ (21 ∗ 𝑐3 ∗ 𝑠2)/50 + 𝑠2 ∗ 𝑠3 + (2 ∗ 𝑠5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3
∗ 𝑠2))/25 − (2 ∗ 𝑐4 ∗ 𝑐5 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3))/25) − (𝑐6
∗ (𝑐5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2) + 𝑐4 ∗ 𝑠5 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3))
− 𝑠4 ∗ 𝑠6 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3)) ∗ ((19 ∗ 𝑠2)/50 + (21 ∗ 𝑐2
∗ 𝑐3)/50 + 𝑐2 ∗ 𝑠3 − (21 ∗ 𝑠2 ∗ 𝑠3)/50 + (2 ∗ 𝑠5 ∗ (𝑐2 ∗ 𝑐3
− 𝑠2 ∗ 𝑠3))/25 + (2 ∗ 𝑐4 ∗ 𝑐5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2))/25))
∗ (𝑐6 ∗ (𝑐5 ∗ (𝑐2 ∗ 𝑐3 ∗ 𝑠1 − 𝑠1 ∗ 𝑠2 ∗ 𝑠3) + 𝑠5 ∗ (𝑐1 ∗ 𝑠4
− 𝑐4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2))) + 𝑠6 ∗ (𝑐1 ∗ 𝑐4 + 𝑠4
∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2))) + ((𝑠5 ∗ (𝑐2 ∗ 𝑐3 ∗ 𝑠1 − 𝑠1
∗ 𝑠2 ∗ 𝑠3) − 𝑐5 ∗ (𝑐1 ∗ 𝑠4 − 𝑐4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1
∗ 𝑠2))) ∗ (𝑐2^2 + 𝑠2^2) ∗ (19 ∗ 𝑐3 ∗ 𝑠5 + 50 ∗ 𝑠3^2 ∗ 𝑠5
+ 19 ∗ 𝑐4 ∗ 𝑐5 ∗ 𝑠3 − 21 ∗ 𝑐3^2 ∗ 𝑐4 ∗ 𝑐5 − 21 ∗ 𝑐4 ∗ 𝑐5
∗ 𝑠3^2 − 50 ∗ 𝑐3 ∗ 𝑐4 ∗ 𝑐5 ∗ 𝑠3))/50
𝐽23 = − ((𝑐6 ∗ (𝑐3 ∗ 𝑐5 − 𝑐4 ∗ 𝑠3 ∗ 𝑠5) + 𝑠3 ∗ 𝑠4 ∗ 𝑠6) ∗ ((21 ∗ 𝑠3)/50
+ (2 ∗ 𝑠3 ∗ 𝑠5)/25 − (2 ∗ 𝑐3 ∗ 𝑐4 ∗ 𝑐5)/25) − (𝑐6 ∗ (𝑐5 ∗ 𝑠3
+ 𝑐3 ∗ 𝑐4 ∗ 𝑠5) − 𝑐3 ∗ 𝑠4 ∗ 𝑠6) ∗ ((21 ∗ 𝑐3)/50 + 𝑠3 + (2
∗ 𝑐3 ∗ 𝑠5)/25 + (2 ∗ 𝑐4 ∗ 𝑐5 ∗ 𝑠3)/25)) ∗ (𝑐6 ∗ (𝑐5 ∗ (𝑐2 ∗ 𝑐3
∗ 𝑠1 − 𝑠1 ∗ 𝑠2 ∗ 𝑠3) + 𝑠5 ∗ (𝑐1 ∗ 𝑠4 − 𝑐4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3
∗ 𝑠1 ∗ 𝑠2))) + 𝑠6 ∗ (𝑐1 ∗ 𝑐4 + 𝑠4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1
∗ 𝑠2))) − ((𝑠6 ∗ (𝑐3 ∗ 𝑐5 − 𝑐4 ∗ 𝑠3 ∗ 𝑠5) − 𝑐6 ∗ 𝑠3 ∗ 𝑠4)
∗ ((21 ∗ 𝑠3)/50 + (2 ∗ 𝑠3 ∗ 𝑠5)/25 − (2 ∗ 𝑐3 ∗ 𝑐4 ∗ 𝑐5)/25)
− (𝑠6 ∗ (𝑐5 ∗ 𝑠3 + 𝑐3 ∗ 𝑐4 ∗ 𝑠5) + 𝑐3 ∗ 𝑐6 ∗ 𝑠4) ∗ ((21
∗ 𝑐3)/50 + 𝑠3 + (2 ∗ 𝑐3 ∗ 𝑠5)/25 + (2 ∗ 𝑐4 ∗ 𝑐5 ∗ 𝑠3)/25))
∗ (𝑠6 ∗ (𝑐5 ∗ (𝑐2 ∗ 𝑐3 ∗ 𝑠1 − 𝑠1 ∗ 𝑠2 ∗ 𝑠3) + 𝑠5 ∗ (𝑐1 ∗ 𝑠4
− 𝑐4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2))) − 𝑐6 ∗ (𝑐1 ∗ 𝑐4 + 𝑠4
∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2))) − (𝑠5 ∗ (𝑐2 ∗ 𝑐3 ∗ 𝑠1 − 𝑠1
∗ 𝑠2 ∗ 𝑠3) − 𝑐5 ∗ (𝑐1 ∗ 𝑠4 − 𝑐4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1
∗ 𝑠2))) ∗ ((21 ∗ 𝑐3^2 ∗ 𝑐4 ∗ 𝑐5)/50 − 𝑠3^2 ∗ 𝑠5 + (21 ∗ 𝑐4

∗ 𝑐5 ∗ 𝑠3^2)/50 + 𝑐3 ∗ 𝑐4 ∗ 𝑐5 ∗ 𝑠3)
18


𝐽24 = −(2 ∗ 𝑐5 ∗ (𝑐4^2 + 𝑠4^2) ∗ (𝑐6^2 + 𝑠6^2) ∗ (𝑐1 ∗ 𝑐4 + 𝑐2 ∗ 𝑠1 ∗ 𝑠3
∗ 𝑠4 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2 ∗ 𝑠4))/25
𝐽25 = −(2 ∗ (𝑐5^2 + 𝑠5^2) ∗ (𝑐6^2 + 𝑠6^2) ∗ (𝑐5 ∗ 𝑠1 ∗ 𝑠2 ∗ 𝑠3 − 𝑐1 ∗ 𝑠4
∗ 𝑠5 − 𝑐2 ∗ 𝑐3 ∗ 𝑐5 ∗ 𝑠1 + 𝑐2 ∗ 𝑐4 ∗ 𝑠1 ∗ 𝑠3 ∗ 𝑠5 + 𝑐3 ∗ 𝑐4 ∗ 𝑠1
∗ 𝑠2 ∗ 𝑠5))/25
𝐽26 = 0
𝐽31 = (𝑐5 ∗ 𝑠4 ∗ (𝑠5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2) − 𝑐4 ∗ 𝑐5 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3))
∗ (𝑐1^2 + 𝑠1^2) ∗ (19 ∗ 𝑠2 + 21 ∗ 𝑐2 ∗ 𝑐3 + 50 ∗ 𝑐2 ∗ 𝑠3
− 21 ∗ 𝑠2 ∗ 𝑠3))/50 − (𝑐6 ∗ (𝑐5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2) + 𝑐4
∗ 𝑠5 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3)) − 𝑠4 ∗ 𝑠6 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3))
∗ ((𝑐6 ∗ (𝑐5 ∗ (𝑐2 ∗ 𝑐3 ∗ 𝑠1 − 𝑠1 ∗ 𝑠2 ∗ 𝑠3) + 𝑠5 ∗ (𝑐1 ∗ 𝑠4
− 𝑐4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2))) + 𝑠6 ∗ (𝑐1 ∗ 𝑐4 + 𝑠4
∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2))) ∗ ((19 ∗ 𝑐1 ∗ 𝑠2)/50 + (2 ∗ 𝑠5
∗ (𝑐1 ∗ 𝑐2 ∗ 𝑐3 − 𝑐1 ∗ 𝑠2 ∗ 𝑠3))/25 + (2 ∗ 𝑐5 ∗ (𝑠1 ∗ 𝑠4 + 𝑐4
∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2)))/25 + (21 ∗ 𝑐1 ∗ 𝑐2 ∗ 𝑐3)/50
+ 𝑐1 ∗ 𝑐2 ∗ 𝑠3 − (21 ∗ 𝑐1 ∗ 𝑠2 ∗ 𝑠3)/50) − (𝑐6 ∗ (𝑐5 ∗ (𝑐1
∗ 𝑐2 ∗ 𝑐3 − 𝑐1 ∗ 𝑠2 ∗ 𝑠3) − 𝑠5 ∗ (𝑠1 ∗ 𝑠4 + 𝑐4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3
+ 𝑐1 ∗ 𝑐3 ∗ 𝑠2))) − 𝑠6 ∗ (𝑐4 ∗ 𝑠1 − 𝑠4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1
∗ 𝑐3 ∗ 𝑠2))) ∗ ((19 ∗ 𝑠1 ∗ 𝑠2)/50 + (2 ∗ 𝑠5 ∗ (𝑐2 ∗ 𝑐3 ∗ 𝑠1 − 𝑠1
∗ 𝑠2 ∗ 𝑠3))/25 − (2 ∗ 𝑐5 ∗ (𝑐1 ∗ 𝑠4 − 𝑐4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3
∗ 𝑠1 ∗ 𝑠2)))/25 + (21 ∗ 𝑐2 ∗ 𝑐3 ∗ 𝑠1)/50 + 𝑐2 ∗ 𝑠1 ∗ 𝑠3
− (21 ∗ 𝑠1 ∗ 𝑠2 ∗ 𝑠3)/50)) − (𝑠6 ∗ (𝑐5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2)
+ 𝑐4 ∗ 𝑠5 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3)) + 𝑐6 ∗ 𝑠4 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2
∗ 𝑠3)) ∗ ((𝑠6 ∗ (𝑐5 ∗ (𝑐2 ∗ 𝑐3 ∗ 𝑠1 − 𝑠1 ∗ 𝑠2 ∗ 𝑠3) + 𝑠5 ∗ (𝑐1
∗ 𝑠4 − 𝑐4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2))) − 𝑐6 ∗ (𝑐1 ∗ 𝑐4
+ 𝑠4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2))) ∗ ((19 ∗ 𝑐1 ∗ 𝑠2)/50

+ (2 ∗ 𝑠5 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑐3 − 𝑐1 ∗ 𝑠2 ∗ 𝑠3))/25 + (2 ∗ 𝑐5 ∗ (𝑠1
∗ 𝑠4 + 𝑐4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2)))/25 + (21 ∗ 𝑐1 ∗ 𝑐2
∗ 𝑐3)/50 + 𝑐1 ∗ 𝑐2 ∗ 𝑠3 − (21 ∗ 𝑐1 ∗ 𝑠2 ∗ 𝑠3)/50) − (𝑠6 ∗ (𝑐5
∗ (𝑐1 ∗ 𝑐2 ∗ 𝑐3 − 𝑐1 ∗ 𝑠2 ∗ 𝑠3) − 𝑠5 ∗ (𝑠1 ∗ 𝑠4 + 𝑐4 ∗ (𝑐1 ∗ 𝑐2
∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2))) + 𝑐6 ∗ (𝑐4 ∗ 𝑠1 − 𝑠4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3
+ 𝑐1 ∗ 𝑐3 ∗ 𝑠2))) ∗ ((19 ∗ 𝑠1 ∗ 𝑠2)/50 + (2 ∗ 𝑠5 ∗ (𝑐2 ∗ 𝑐3 ∗ 𝑠1
− 𝑠1 ∗ 𝑠2 ∗ 𝑠3))/25 − (2 ∗ 𝑐5 ∗ (𝑐1 ∗ 𝑠4 − 𝑐4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3
+ 𝑐3 ∗ 𝑠1 ∗ 𝑠2)))/25 + (21 ∗ 𝑐2 ∗ 𝑐3 ∗ 𝑠1)/50 + 𝑐2 ∗ 𝑠1 ∗ 𝑠3
− (21 ∗ 𝑠1 ∗ 𝑠2 ∗ 𝑠3)/50))

19


𝐽32 = ((𝑐6 ∗ (𝑐5 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3) − 𝑐4 ∗ 𝑠5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2))
+ 𝑠4 ∗ 𝑠6 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2)) ∗ ((21 ∗ 𝑐2 ∗ 𝑠3)/50 − (19
∗ 𝑐2)/50 + (21 ∗ 𝑐3 ∗ 𝑠2)/50 + 𝑠2 ∗ 𝑠3 + (2 ∗ 𝑠5 ∗ (𝑐2 ∗ 𝑠3
+ 𝑐3 ∗ 𝑠2))/25 − (2 ∗ 𝑐4 ∗ 𝑐5 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3))/25)
− (𝑐6 ∗ (𝑐5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2) + 𝑐4 ∗ 𝑠5 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2
∗ 𝑠3)) − 𝑠4 ∗ 𝑠6 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3)) ∗ ((19 ∗ 𝑠2)/50 + (21
∗ 𝑐2 ∗ 𝑐3)/50 + 𝑐2 ∗ 𝑠3 − (21 ∗ 𝑠2 ∗ 𝑠3)/50 + (2 ∗ 𝑠5 ∗ (𝑐2
∗ 𝑐3 − 𝑠2 ∗ 𝑠3))/25 + (2 ∗ 𝑐4 ∗ 𝑐5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3
∗ 𝑠2))/25)) ∗ (𝑐6 ∗ (𝑐5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2) + 𝑐4 ∗ 𝑠5 ∗ (𝑐2
∗ 𝑐3 − 𝑠2 ∗ 𝑠3)) − 𝑠4 ∗ 𝑠6 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3)) − ((𝑠6
∗ (𝑐5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2) + 𝑐4 ∗ 𝑠5 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3))
+ 𝑐6 ∗ 𝑠4 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3)) ∗ ((19 ∗ 𝑠2)/50 + (21 ∗ 𝑐2
∗ 𝑐3)/50 + 𝑐2 ∗ 𝑠3 − (21 ∗ 𝑠2 ∗ 𝑠3)/50 + (2 ∗ 𝑠5 ∗ (𝑐2 ∗ 𝑐3
− 𝑠2 ∗ 𝑠3))/25 + (2 ∗ 𝑐4 ∗ 𝑐5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2))/25)
− (𝑠6 ∗ (𝑐5 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3) − 𝑐4 ∗ 𝑠5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3
∗ 𝑠2)) − 𝑐6 ∗ 𝑠4 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2)) ∗ ((21 ∗ 𝑐2 ∗ 𝑠3)/50
− (19 ∗ 𝑐2)/50 + (21 ∗ 𝑐3 ∗ 𝑠2)/50 + 𝑠2 ∗ 𝑠3 + (2 ∗ 𝑠5

∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2))/25 − (2 ∗ 𝑐4 ∗ 𝑐5 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2
∗ 𝑠3))/25)) ∗ (𝑠6 ∗ (𝑐5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2) + 𝑐4 ∗ 𝑠5 ∗ (𝑐2
∗ 𝑐3 − 𝑠2 ∗ 𝑠3)) + 𝑐6 ∗ 𝑠4 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3)) − ((𝑠5
∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2) − 𝑐4 ∗ 𝑐5 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3)) ∗ (𝑐2^2
+ 𝑠2^2) ∗ (19 ∗ 𝑐3 ∗ 𝑠5 + 50 ∗ 𝑠3^2 ∗ 𝑠5 + 19 ∗ 𝑐4 ∗ 𝑐5 ∗ 𝑠3
− 21 ∗ 𝑐3^2 ∗ 𝑐4 ∗ 𝑐5 − 21 ∗ 𝑐4 ∗ 𝑐5 ∗ 𝑠3^2 − 50 ∗ 𝑐3 ∗ 𝑐4
∗ 𝑐5 ∗ 𝑠3))/50
𝐽33 = (𝑠5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2) − 𝑐4 ∗ 𝑐5 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3)) ∗ ((21
∗ 𝑐3^2 ∗ 𝑐4 ∗ 𝑐5)/50 − 𝑠3^2 ∗ 𝑠5 + (21 ∗ 𝑐4 ∗ 𝑐5 ∗ 𝑠3^2)/50
+ 𝑐3 ∗ 𝑐4 ∗ 𝑐5 ∗ 𝑠3) + (𝑠6 ∗ (𝑐5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2) + 𝑐4
∗ 𝑠5 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3)) + 𝑐6 ∗ 𝑠4 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3))
∗ ((𝑠6 ∗ (𝑐3 ∗ 𝑐5 − 𝑐4 ∗ 𝑠3 ∗ 𝑠5) − 𝑐6 ∗ 𝑠3 ∗ 𝑠4) ∗ ((21
∗ 𝑠3)/50 + (2 ∗ 𝑠3 ∗ 𝑠5)/25 − (2 ∗ 𝑐3 ∗ 𝑐4 ∗ 𝑐5)/25) − (𝑠6
∗ (𝑐5 ∗ 𝑠3 + 𝑐3 ∗ 𝑐4 ∗ 𝑠5) + 𝑐3 ∗ 𝑐6 ∗ 𝑠4) ∗ ((21 ∗ 𝑐3)/50
+ 𝑠3 + (2 ∗ 𝑐3 ∗ 𝑠5)/25 + (2 ∗ 𝑐4 ∗ 𝑐5 ∗ 𝑠3)/25)) + (𝑐6
∗ (𝑐5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2) + 𝑐4 ∗ 𝑠5 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3))
− 𝑠4 ∗ 𝑠6 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3)) ∗ ((𝑐6 ∗ (𝑐3 ∗ 𝑐5 − 𝑐4 ∗ 𝑠3
∗ 𝑠5) + 𝑠3 ∗ 𝑠4 ∗ 𝑠6) ∗ ((21 ∗ 𝑠3)/50 + (2 ∗ 𝑠3 ∗ 𝑠5)/25 − (2
∗ 𝑐3 ∗ 𝑐4 ∗ 𝑐5)/25) − (𝑐6 ∗ (𝑐5 ∗ 𝑠3 + 𝑐3 ∗ 𝑐4 ∗ 𝑠5) − 𝑐3 ∗ 𝑠4
∗ 𝑠6) ∗ ((21 ∗ 𝑐3)/50 + 𝑠3 + (2 ∗ 𝑐3 ∗ 𝑠5)/25 + (2 ∗ 𝑐4 ∗ 𝑐5
∗ 𝑠3)/25))
𝐽34 = −(2 ∗ 𝑐5 ∗ 𝑠4 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3) ∗ (𝑐4^2 + 𝑠4^2) ∗ (𝑐6^2
+ 𝑠6^2))/25
𝐽35 = −(2 ∗ (𝑐5^2 + 𝑠5^2) ∗ (𝑐6^2 + 𝑠6^2) ∗ (𝑐2 ∗ 𝑐5 ∗ 𝑠3 + 𝑐3 ∗ 𝑐5 ∗ 𝑠2
− 𝑐4 ∗ 𝑠2 ∗ 𝑠3 ∗ 𝑠5 + 𝑐2 ∗ 𝑐3 ∗ 𝑐4 ∗ 𝑠5))/25
20


𝐽36 = 0
𝐽41 = − (𝑠5 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑐3 − 𝑐1 ∗ 𝑠2 ∗ 𝑠3) + 𝑐5 ∗ (𝑠1 ∗ 𝑠4 + 𝑐4 ∗ (𝑐1 ∗ 𝑐2

∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2))) ∗ (𝑠5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2) − 𝑐4 ∗ 𝑐5
∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3)) − (𝑠6 ∗ (𝑐5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2) + 𝑐4
∗ 𝑠5 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3)) + 𝑐6 ∗ 𝑠4 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3))
∗ (𝑠6 ∗ (𝑐5 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑐3 − 𝑐1 ∗ 𝑠2 ∗ 𝑠3) − 𝑠5 ∗ (𝑠1 ∗ 𝑠4
+ 𝑐4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2))) + 𝑐6 ∗ (𝑐4 ∗ 𝑠1 − 𝑠4
∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2))) − (𝑐6 ∗ (𝑐5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3
∗ 𝑠2) + 𝑐4 ∗ 𝑠5 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3)) − 𝑠4 ∗ 𝑠6 ∗ (𝑐2 ∗ 𝑐3
− 𝑠2 ∗ 𝑠3)) ∗ (𝑐6 ∗ (𝑐5 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑐3 − 𝑐1 ∗ 𝑠2 ∗ 𝑠3) − 𝑠5
∗ (𝑠1 ∗ 𝑠4 + 𝑐4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2))) − 𝑠6 ∗ (𝑐4
∗ 𝑠1 − 𝑠4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2)))
𝐽42 = (𝑐4 ∗ 𝑠6 + 𝑐6 ∗ 𝑠4 ∗ 𝑠5) ∗ (𝑐6 ∗ (𝑐5 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑐3 − 𝑐1 ∗ 𝑠2 ∗ 𝑠3)
− 𝑠5 ∗ (𝑠1 ∗ 𝑠4 + 𝑐4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2))) − 𝑠6
∗ (𝑐4 ∗ 𝑠1 − 𝑠4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2))) − (𝑐4 ∗ 𝑐6
− 𝑠4 ∗ 𝑠5 ∗ 𝑠6) ∗ (𝑠6 ∗ (𝑐5 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑐3 − 𝑐1 ∗ 𝑠2 ∗ 𝑠3) − 𝑠5
∗ (𝑠1 ∗ 𝑠4 + 𝑐4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2))) + 𝑐6 ∗ (𝑐4
∗ 𝑠1 − 𝑠4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2))) − 𝑐5 ∗ 𝑠4 ∗ (𝑠5
∗ (𝑐1 ∗ 𝑐2 ∗ 𝑐3 − 𝑐1 ∗ 𝑠2 ∗ 𝑠3) + 𝑐5 ∗ (𝑠1 ∗ 𝑠4 + 𝑐4 ∗ (𝑐1 ∗ 𝑐2
∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2)))
𝐽43 = (𝑐4 ∗ 𝑠6 + 𝑐6 ∗ 𝑠4 ∗ 𝑠5) ∗ (𝑐6 ∗ (𝑐5 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑐3 − 𝑐1 ∗ 𝑠2 ∗ 𝑠3)
− 𝑠5 ∗ (𝑠1 ∗ 𝑠4 + 𝑐4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2))) − 𝑠6
∗ (𝑐4 ∗ 𝑠1 − 𝑠4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2))) − (𝑐4 ∗ 𝑐6
− 𝑠4 ∗ 𝑠5 ∗ 𝑠6) ∗ (𝑠6 ∗ (𝑐5 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑐3 − 𝑐1 ∗ 𝑠2 ∗ 𝑠3) − 𝑠5
∗ (𝑠1 ∗ 𝑠4 + 𝑐4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2))) + 𝑐6 ∗ (𝑐4
∗ 𝑠1 − 𝑠4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2))) − 𝑐5 ∗ 𝑠4 ∗ (𝑠5
∗ (𝑐1 ∗ 𝑐2 ∗ 𝑐3 − 𝑐1 ∗ 𝑠2 ∗ 𝑠3) + 𝑐5 ∗ (𝑠1 ∗ 𝑠4 + 𝑐4 ∗ (𝑐1 ∗ 𝑐2
∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2)))
𝐽44 = 𝑠5 ∗ (𝑠5 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑐3 − 𝑐1 ∗ 𝑠2 ∗ 𝑠3) + 𝑐5 ∗ (𝑠1 ∗ 𝑠4 + 𝑐4 ∗ (𝑐1
∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2))) + 𝑐5 ∗ 𝑐6 ∗ (𝑐6 ∗ (𝑐5 ∗ (𝑐1 ∗ 𝑐2
∗ 𝑐3 − 𝑐1 ∗ 𝑠2 ∗ 𝑠3) − 𝑠5 ∗ (𝑠1 ∗ 𝑠4 + 𝑐4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1
∗ 𝑐3 ∗ 𝑠2))) − 𝑠6 ∗ (𝑐4 ∗ 𝑠1 − 𝑠4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3

∗ 𝑠2))) + 𝑐5 ∗ 𝑠6 ∗ (𝑠6 ∗ (𝑐5 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑐3 − 𝑐1 ∗ 𝑠2 ∗ 𝑠3)
− 𝑠5 ∗ (𝑠1 ∗ 𝑠4 + 𝑐4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2))) + 𝑐6
∗ (𝑐4 ∗ 𝑠1 − 𝑠4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2)))
𝐽45 = (𝑐6^2 + 𝑠6^2) ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 ∗ 𝑠4 − 𝑐4 ∗ 𝑠1 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2 ∗ 𝑠4)
𝐽46 = 𝑠5 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑐3 − 𝑐1 ∗ 𝑠2 ∗ 𝑠3) + 𝑐5 ∗ (𝑠1 ∗ 𝑠4 + 𝑐4 ∗ (𝑐1 ∗ 𝑐2
∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2))

21


𝐽51 = − (𝑠6 ∗ (𝑐5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2) + 𝑐4 ∗ 𝑠5 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3))
+ 𝑐6 ∗ 𝑠4 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3)) ∗ (𝑠6 ∗ (𝑐5 ∗ (𝑐2 ∗ 𝑐3 ∗ 𝑠1
− 𝑠1 ∗ 𝑠2 ∗ 𝑠3) + 𝑠5 ∗ (𝑐1 ∗ 𝑠4 − 𝑐4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1
∗ 𝑠2))) − 𝑐6 ∗ (𝑐1 ∗ 𝑐4 + 𝑠4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2)))
− (𝑐6 ∗ (𝑐5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2) + 𝑐4 ∗ 𝑠5 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2
∗ 𝑠3)) − 𝑠4 ∗ 𝑠6 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3)) ∗ (𝑐6 ∗ (𝑐5 ∗ (𝑐2 ∗ 𝑐3
∗ 𝑠1 − 𝑠1 ∗ 𝑠2 ∗ 𝑠3) + 𝑠5 ∗ (𝑐1 ∗ 𝑠4 − 𝑐4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3
∗ 𝑠1 ∗ 𝑠2))) + 𝑠6 ∗ (𝑐1 ∗ 𝑐4 + 𝑠4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1
∗ 𝑠2))) − (𝑠5 ∗ (𝑐2 ∗ 𝑐3 ∗ 𝑠1 − 𝑠1 ∗ 𝑠2 ∗ 𝑠3) − 𝑐5 ∗ (𝑐1 ∗ 𝑠4
− 𝑐4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2))) ∗ (𝑠5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3
∗ 𝑠2) − 𝑐4 ∗ 𝑐5 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3))
𝐽52 = (𝑐4 ∗ 𝑠6 + 𝑐6 ∗ 𝑠4 ∗ 𝑠5) ∗ (𝑐6 ∗ (𝑐5 ∗ (𝑐2 ∗ 𝑐3 ∗ 𝑠1 − 𝑠1 ∗ 𝑠2 ∗ 𝑠3)
+ 𝑠5 ∗ (𝑐1 ∗ 𝑠4 − 𝑐4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2))) + 𝑠6
∗ (𝑐1 ∗ 𝑐4 + 𝑠4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2))) − (𝑐4 ∗ 𝑐6
− 𝑠4 ∗ 𝑠5 ∗ 𝑠6) ∗ (𝑠6 ∗ (𝑐5 ∗ (𝑐2 ∗ 𝑐3 ∗ 𝑠1 − 𝑠1 ∗ 𝑠2 ∗ 𝑠3) + 𝑠5
∗ (𝑐1 ∗ 𝑠4 − 𝑐4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2))) − 𝑐6 ∗ (𝑐1
∗ 𝑐4 + 𝑠4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2))) − 𝑐5 ∗ 𝑠4 ∗ (𝑠5
∗ (𝑐2 ∗ 𝑐3 ∗ 𝑠1 − 𝑠1 ∗ 𝑠2 ∗ 𝑠3) − 𝑐5 ∗ (𝑐1 ∗ 𝑠4 − 𝑐4 ∗ (𝑐2 ∗ 𝑠1
∗ 𝑠3 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2)))
𝐽53 = (𝑐4 ∗ 𝑠6 + 𝑐6 ∗ 𝑠4 ∗ 𝑠5) ∗ (𝑐6 ∗ (𝑐5 ∗ (𝑐2 ∗ 𝑐3 ∗ 𝑠1 − 𝑠1 ∗ 𝑠2 ∗ 𝑠3)

+ 𝑠5 ∗ (𝑐1 ∗ 𝑠4 − 𝑐4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2))) + 𝑠6
∗ (𝑐1 ∗ 𝑐4 + 𝑠4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2))) − (𝑐4 ∗ 𝑐6
− 𝑠4 ∗ 𝑠5 ∗ 𝑠6) ∗ (𝑠6 ∗ (𝑐5 ∗ (𝑐2 ∗ 𝑐3 ∗ 𝑠1 − 𝑠1 ∗ 𝑠2 ∗ 𝑠3) + 𝑠5
∗ (𝑐1 ∗ 𝑠4 − 𝑐4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2))) − 𝑐6 ∗ (𝑐1
∗ 𝑐4 + 𝑠4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2))) − 𝑐5 ∗ 𝑠4 ∗ (𝑠5
∗ (𝑐2 ∗ 𝑐3 ∗ 𝑠1 − 𝑠1 ∗ 𝑠2 ∗ 𝑠3) − 𝑐5 ∗ (𝑐1 ∗ 𝑠4 − 𝑐4 ∗ (𝑐2 ∗ 𝑠1
∗ 𝑠3 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2)))
𝐽54 = 𝑠5 ∗ (𝑠5 ∗ (𝑐2 ∗ 𝑐3 ∗ 𝑠1 − 𝑠1 ∗ 𝑠2 ∗ 𝑠3) − 𝑐5 ∗ (𝑐1 ∗ 𝑠4 − 𝑐4 ∗ (𝑐2
∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2))) + 𝑐5 ∗ 𝑐6 ∗ (𝑐6 ∗ (𝑐5 ∗ (𝑐2 ∗ 𝑐3
∗ 𝑠1 − 𝑠1 ∗ 𝑠2 ∗ 𝑠3) + 𝑠5 ∗ (𝑐1 ∗ 𝑠4 − 𝑐4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3
∗ 𝑠1 ∗ 𝑠2))) + 𝑠6 ∗ (𝑐1 ∗ 𝑐4 + 𝑠4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1
∗ 𝑠2))) + 𝑐5 ∗ 𝑠6 ∗ (𝑠6 ∗ (𝑐5 ∗ (𝑐2 ∗ 𝑐3 ∗ 𝑠1 − 𝑠1 ∗ 𝑠2 ∗ 𝑠3)
+ 𝑠5 ∗ (𝑐1 ∗ 𝑠4 − 𝑐4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2))) − 𝑐6
∗ (𝑐1 ∗ 𝑐4 + 𝑠4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2)))
𝐽55 = (𝑐6^2 + 𝑠6^2) ∗ (𝑐1 ∗ 𝑐4 + 𝑐2 ∗ 𝑠1 ∗ 𝑠3 ∗ 𝑠4 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2 ∗ 𝑠4)
𝐽56 = 𝑠5 ∗ (𝑐2 ∗ 𝑐3 ∗ 𝑠1 − 𝑠1 ∗ 𝑠2 ∗ 𝑠3) − 𝑐5 ∗ (𝑐1 ∗ 𝑠4 − 𝑐4 ∗ (𝑐2 ∗ 𝑠1
∗ 𝑠3 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2))
𝐽61 = (𝑠5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2) − 𝑐4 ∗ 𝑐5 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3))^2 + (𝑠6
∗ (𝑐5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2) + 𝑐4 ∗ 𝑠5 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3))
+ 𝑐6 ∗ 𝑠4 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3))^2 + (𝑐6 ∗ (𝑐5 ∗ (𝑐2 ∗ 𝑠3
+ 𝑐3 ∗ 𝑠2) + 𝑐4 ∗ 𝑠5 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3)) − 𝑠4 ∗ 𝑠6 ∗ (𝑐2
∗ 𝑐3 − 𝑠2 ∗ 𝑠3))^2
22


𝐽62 = (𝑠6 ∗ (𝑐5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2) + 𝑐4 ∗ 𝑠5 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3)) + 𝑐6
∗ 𝑠4 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3)) ∗ (𝑐4 ∗ 𝑐6 − 𝑠4 ∗ 𝑠5 ∗ 𝑠6) − (𝑐6
∗ (𝑐5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2) + 𝑐4 ∗ 𝑠5 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3))
− 𝑠4 ∗ 𝑠6 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3)) ∗ (𝑐4 ∗ 𝑠6 + 𝑐6 ∗ 𝑠4 ∗ 𝑠5)
+ 𝑐5 ∗ 𝑠4 ∗ (𝑠5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2) − 𝑐4 ∗ 𝑐5 ∗ (𝑐2 ∗ 𝑐3

− 𝑠2 ∗ 𝑠3))
𝐽63 = (𝑠6 ∗ (𝑐5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2) + 𝑐4 ∗ 𝑠5 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3)) + 𝑐6
∗ 𝑠4 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3)) ∗ (𝑐4 ∗ 𝑐6 − 𝑠4 ∗ 𝑠5 ∗ 𝑠6) − (𝑐6
∗ (𝑐5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2) + 𝑐4 ∗ 𝑠5 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3))
− 𝑠4 ∗ 𝑠6 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3)) ∗ (𝑐4 ∗ 𝑠6 + 𝑐6 ∗ 𝑠4 ∗ 𝑠5)
+ 𝑐5 ∗ 𝑠4 ∗ (𝑠5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2) − 𝑐4 ∗ 𝑐5 ∗ (𝑐2 ∗ 𝑐3
− 𝑠2 ∗ 𝑠3))
𝐽64 = − 𝑠5 ∗ (𝑠5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2) − 𝑐4 ∗ 𝑐5 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3))
− 𝑐5 ∗ 𝑐6 ∗ (𝑐6 ∗ (𝑐5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2) + 𝑐4 ∗ 𝑠5 ∗ (𝑐2
∗ 𝑐3 − 𝑠2 ∗ 𝑠3)) − 𝑠4 ∗ 𝑠6 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3)) − 𝑐5 ∗ 𝑠6
∗ (𝑠6 ∗ (𝑐5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2) + 𝑐4 ∗ 𝑠5 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2
∗ 𝑠3)) + 𝑐6 ∗ 𝑠4 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3))
𝐽65 = 𝑠4 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3) ∗ (𝑐6^2 + 𝑠6^2)
𝐽66 = 𝑐4 ∗ 𝑐5 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3) − 𝑠5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2)

23


CHƯƠNG 4. ĐỘNG HỌC NGƯỢC VỊ TRÍ
4.1 Tổng quan động học ngược vị trí
Động học ngược vị trí để xác định giá trị các biến khớp tương ứng với vị trí
mong muốn của tay trong khơng gian. Tức là xác định ma trận các biến khớp Q =
[q1, q2,…, qn]T từ vị trí của khâu tác động cuối đã biết, giả sử 𝑇𝑛0 có giá trị:
𝑛𝑥 𝑜𝑥 𝑎𝑥 𝑝𝑥
𝑛
𝑜𝑦 𝑎𝑦 𝑝𝑦
]
𝑇𝑛0 = [ 𝑦
𝑛𝑧 𝑜𝑧 𝑎𝑧 𝑝𝑧
0 0 0 1

Ta có các phương pháp giải vị trí các động học ngược vị trí:
• Phép đảo vị trí: áp dụng cho các robot có hai bậc tự do,
• Phép đảo hướng: áp dụng cho phép quay cố định,
• Phương pháp phân ly biến: áp dụng cho các robot có số bậc tự do >3.
➔ Trong bài toán mày, ta sử dụng phương pháp phân ly biến để tính tốn động
học ngược vị trí cho robot.
4.2. Tính tốn động học ngược vị trí cho TV800
Các bước thực hiện phương pháp phân ly biến được thể hiện ở Hình 4.1

Hình 4.1. Phương pháp phân ly biến
Ta có:
𝑛𝑥 𝑜𝑥 𝑎𝑥 𝑝𝑥
𝑛
𝑜𝑦 𝑎𝑦 𝑝𝑦
] = 𝐴1 (𝑞1 )𝐴2 (𝑞2 )𝐴3 (𝑞3 )𝐴4 (𝑞4 )𝐴5 (𝑞5 )𝐴6 (𝑞6 ) (∗)
𝑇60 = [ 𝑦
𝑛𝑧 𝑜𝑧 𝑎𝑧 𝑝𝑧
0 0 0 1
❖
Nhân hai vế của phương trình trên với A1-1(q1) ta được:
𝐴1−1 (𝑞1 )𝑇60 = 𝐴1−1 (𝑞1 )𝐴1 (𝑞1 )𝐴2 (𝑞2 )𝐴3 (𝑞3 )𝐴4 (𝑞4 )𝐴5 (𝑞5 )𝐴6 (𝑞6 )
24


𝐴1−1 (𝑞1 )𝑇50 = 𝐴2 (𝑞2 )𝐴3 (𝑞3 )𝐴4 (𝑞4 )𝐴5 (𝑞5 )𝐴6 (𝑞6 ) = 𝑇61
Sử dụng Matlab để thực hiện tính tốn ta thu được kết quả:
VT1=𝐴1−1 (𝑞1 )𝑇60
=
𝑛𝑥 ∗ 𝐶1 + 𝑛𝑦 ∗ 𝑆1 𝑜𝑥 ∗ 𝐶1 + 𝑜𝑦 ∗ 𝑆1 𝑎𝑥 ∗ 𝐶1 + 𝑎𝑦 ∗ 𝑆1 𝑝𝑥 ∗ 𝐶1 + 𝑝𝑦 ∗ 𝑆1
𝑛𝑥 ∗ S1 − nyC1

𝑜𝑥 ∗ 𝑆1 − 𝑜𝑦 ∗ 𝐶1 𝑎𝑥 ∗ 𝑆1 − 𝑎𝑦 ∗ 𝐶1 𝑝𝑥 ∗ 𝑆1 − 𝑝𝑦 ∗ 𝐶1
[
]
−𝑛𝑧
−𝑜𝑧
−𝑎𝑧
𝑑1 − 𝑝𝑧
0
0
0
1
1
VP1=𝐴2 (𝑞2 )𝐴3 (𝑞3 )𝐴4 (𝑞4 )𝐴5 (𝑞5 )𝐴6 (𝑞6 ) = 𝑇6
𝐶23(−4)5(−6) 𝑆23(−4)5(−6)
0 −𝑎3 ∗ 𝐶23 − 𝑎2 ∗ 𝐶2
𝑆23(−4)5(−6) −𝐶23(−4)5(−6) 0 −𝑎3 ∗ 𝑆23 − 𝑎2 ∗ 𝑆2
]
=[
0
0
−1
−𝑑4 − 𝑑6
0
0
0
1
Với 𝐶23(−4)5(−6) = 𝑐𝑜𝑠(𝑞2 + 𝑞3 − 𝑞4 + 𝑞5 − 𝑞6 )
Cân bằng phần tử hàng 1 cột 3 ở cả hai vế VT1 và VP1 ta thu được:
𝑎𝑥 ∗ 𝐶1 + 𝑎𝑦 ∗ 𝑆1 = 0
Áp dụng các trường hợp trong động học giải tích ta có:

𝜃1 = 𝑞1 = 𝐴𝑇𝐴𝑁2(−𝑎𝑥, 𝑎𝑦)
𝜃1 = 𝑞1 = 𝐴𝑇𝐴𝑁2(𝑎𝑥, −𝑎𝑦)

hoặc

Cân bằng các phần tử cột 4 hàng 1 và 2 ta thu được:
𝑎3 ∗ 𝐶23 + 𝑎2 ∗ 𝐶2 = 𝑚 (1)
{
𝑎3 ∗ 𝑆23 + 𝑎2 ∗ 𝑆2 = 𝑛 (2)
Với
{

𝑚 = −(𝑝𝑥 ∗ 𝐶1 + 𝑝𝑦 ∗ 𝑆1)
𝑛 = −(𝑝𝑥 ∗ 𝑆1 − 𝑝𝑦 ∗ 𝐶1)

Bình phương hai vế của (1) và (2) rồi cộng lại ta được:
𝑎32 + 𝑎22 + 2𝑎2 𝑎3 (𝐶23 ∗ 𝐶2 + 𝑆23 ∗ 𝑆2) = 𝑚2 + 𝑛2
𝐶3 =

𝑚2 + 𝑛2 − 𝑎32 − 𝑎22
2𝑎2 𝑎3

➔ 𝜃3 = 𝐴𝑇𝐴𝑁2 (±√1 − (

𝑚2 +𝑛2 −𝑎32 −𝑎22
2𝑎2 𝑎3

2

) ,

𝑚2 +𝑛2 −𝑎32 −𝑎22
2𝑎2 𝑎3

)

Có được 𝜃3 ta thay lại vào (1) và (2) thu được:
(𝑎3 ∗ 𝐶3 + 𝑎2)𝐶2 − 𝑎3 ∗ 𝑆3 ∗ 𝑆2 = 𝑚
{
(𝑎3 ∗ 𝐶3 + 𝑎2)𝑆2 + 𝑎3 ∗ 𝑆3 ∗ 𝐶2 = 𝑛
Áp dụng các trường hợp trong động học giải tích ta có:
𝜃2 = 𝐴𝑇𝐴𝑁2((𝑎3 ∗ 𝐶3 + 𝑎2) ∗ 𝑛 − 𝑎3 ∗ 𝑆3 ∗ 𝑚, (𝑎3 ∗ 𝐶3 + 𝑎2) ∗ 𝑚 + 𝑎3 ∗ 𝑆3
∗ 𝑛)
❖
Nhân cả hai vế của (*) với A1-1(q1)* A2-1(q2)* A3-1(q3)* A4-1(q4) ta
được:

25