\( \DeclareMathOperator{\abs}{abs} \newcommand{\ensuremath}[1]{\mbox{$#1$}} \)
1 Bonusaufgabe 3
(%i1) | ratprint : false ; |
\[\operatorname{ }\mbox{%default false}\]
1.1 Angabe
(%i2) | phi_0 : − %pi / 6 ; |
\[\operatorname{ }-\frac{\ensuremath{\pi} }{6}\]
(%i6) |
d_0_1
:
matrix
(
[
2
]
,
[
1
]
,
[
3
]
)
$
d_1_2 : matrix ( [ 4 ] , [ 2 ] , [ 1 ] ) $ d_2_3 : matrix ( [ 3 ] , [ 1 ] , [ 4 ] ) $ d_3_4 : matrix ( [ 3 ] , [ 4 ] , [ 1 ] ) $ |
(%i11) |
p_1_s1
:
matrix
(
[
2
]
,
[
1
]
,
[
1
/
2
]
)
$
p_2_s2 : matrix ( [ 3 / 2 ] , [ 1 / 2 ] , [ 2 ] ) $ p_3_s3 : matrix ( [ 3 / 2 ] , [ 2 ] , [ 1 / 2 ] ) $ p_4_s4 : matrix ( [ 2 ] , [ 1 / 2 ] , [ 1 ] ) $ p_4_E : matrix ( [ 4 ] , [ 1 ] , [ 2 ] ) $ |
1.2 Aufgabe 1
(%i17) |
R_0_1_z
:
matrix
(
[
cos
(
phi_0
)
,
−
sin
(
phi_0
)
,
0
]
,
[
sin
(
phi_0
)
,
cos
(
phi_0
)
,
0
]
,
[
0
,
0
,
1
]
)
$
H_0_1_z : addrow ( addcol ( R_0_1_z , [ 0 , 0 , 0 ] ) , [ 0 , 0 , 0 , 1 ] ) $ R_0_1_x : matrix ( [ 1 , 0 , 0 ] , [ 0 , cos ( q1 ) , − sin ( q1 ) ] , [ 0 , sin ( q1 ) , cos ( q1 ) ] ) $ H_0_1_x : addrow ( addcol ( R_0_1_x , [ 0 , 0 , 0 ] ) , [ 0 , 0 , 0 , 1 ] ) $ H_0_1_v : matrix ( [ 0 , 0 , 0 , d_0_1 [ 1 , 1 ] ] , [ 0 , 0 , 0 , d_0_1 [ 2 , 1 ] ] , [ 0 , 0 , 0 , d_0_1 [ 3 , 1 ] ] , [ 0 , 0 , 0 , 0 ] ) $ H_0_1 : H_0_1_z . H_0_1_x + H_0_1_v ; |
\[\operatorname{ }\begin{pmatrix}\frac{\sqrt{3}}{2} & \frac{\cos{\left( \ensuremath{\mathrm{q1}}\right) }}{2} & -\frac{\sin{\left( \ensuremath{\mathrm{q1}}\right) }}{2} & 2\\ -\frac{1}{2} & \frac{\sqrt{3} \cos{\left( \ensuremath{\mathrm{q1}}\right) }}{2} & -\frac{\sqrt{3} \sin{\left( \ensuremath{\mathrm{q1}}\right) }}{2} & 1\\ 0 & \sin{\left( \ensuremath{\mathrm{q1}}\right) } & \cos{\left( \ensuremath{\mathrm{q1}}\right) } & 3\\ 0 & 0 & 0 & 1\end{pmatrix}\]
(%i21) |
R_1_2_x
:
matrix
(
[
1
,
0
,
0
]
,
[
0
,
cos
(
q2
)
,
−
sin
(
q2
)
]
,
[
0
,
sin
(
q2
)
,
cos
(
q2
)
]
)
$
H_1_2_x : addrow ( addcol ( R_1_2_x , [ 0 , 0 , 0 ] ) , [ 0 , 0 , 0 , 1 ] ) $ H_1_2_v : matrix ( [ 0 , 0 , 0 , d_1_2 [ 1 , 1 ] ] , [ 0 , 0 , 0 , d_1_2 [ 2 , 1 ] ] , [ 0 , 0 , 0 , d_1_2 [ 3 , 1 ] ] , [ 0 , 0 , 0 , 0 ] ) $ H_1_2 : H_1_2_x + H_1_2_v ; |
\[\operatorname{ }\begin{pmatrix}1 & 0 & 0 & 4\\ 0 & \cos{\left( \ensuremath{\mathrm{q2}}\right) } & -\sin{\left( \ensuremath{\mathrm{q2}}\right) } & 2\\ 0 & \sin{\left( \ensuremath{\mathrm{q2}}\right) } & \cos{\left( \ensuremath{\mathrm{q2}}\right) } & 1\\ 0 & 0 & 0 & 1\end{pmatrix}\]
(%i25) |
R_2_3_x
:
matrix
(
[
1
,
0
,
0
]
,
[
0
,
cos
(
q3
)
,
−
sin
(
q3
)
]
,
[
0
,
sin
(
q3
)
,
cos
(
q3
)
]
)
$
H_2_3_x : addrow ( addcol ( R_2_3_x , [ 0 , 0 , 0 ] ) , [ 0 , 0 , 0 , 1 ] ) $ H_2_3_v : matrix ( [ 0 , 0 , 0 , d_2_3 [ 1 , 1 ] ] , [ 0 , 0 , 0 , d_2_3 [ 2 , 1 ] ] , [ 0 , 0 , 0 , d_2_3 [ 3 , 1 ] ] , [ 0 , 0 , 0 , 0 ] ) $ H_2_3 : H_2_3_x + H_2_3_v ; |
\[\operatorname{ }\begin{pmatrix}1 & 0 & 0 & 3\\ 0 & \cos{\left( \ensuremath{\mathrm{q3}}\right) } & -\sin{\left( \ensuremath{\mathrm{q3}}\right) } & 1\\ 0 & \sin{\left( \ensuremath{\mathrm{q3}}\right) } & \cos{\left( \ensuremath{\mathrm{q3}}\right) } & 4\\ 0 & 0 & 0 & 1\end{pmatrix}\]
(%i29) |
R_3_4
:
matrix
(
[
1
,
0
,
0
]
,
[
0
,
1
,
0
]
,
[
0
,
0
,
1
]
)
$
H_3_4_tx : matrix ( [ 1 , 0 , 0 , q4 ] , [ 0 , 1 , 0 , 0 ] , [ 0 , 0 , 1 , 0 ] , [ 0 , 0 , 0 , 1 ] ) $ H_3_4_v : matrix ( [ 0 , 0 , 0 , d_3_4 [ 1 , 1 ] ] , [ 0 , 0 , 0 , d_3_4 [ 2 , 1 ] ] , [ 0 , 0 , 0 , d_3_4 [ 3 , 1 ] ] , [ 0 , 0 , 0 , 0 ] ) $ H_3_4 : H_3_4_tx + H_3_4_v ; |
\[\operatorname{ }\begin{pmatrix}1 & 0 & 0 & \ensuremath{\mathrm{q4}}+3\\ 0 & 1 & 0 & 4\\ 0 & 0 & 1 & 1\\ 0 & 0 & 0 & 1\end{pmatrix}\]
(%i31) |
H_0_2
:
H_0_1
.
H_1_2
$
trigsimp ( % ) ; |
\[\operatorname{ }\begin{pmatrix}\frac{\sqrt{3}}{2} & -\frac{\sin{\left( \ensuremath{\mathrm{q1}}\right) } \sin{\left( \ensuremath{\mathrm{q2}}\right) }-\cos{\left( \ensuremath{\mathrm{q1}}\right) } \cos{\left( \ensuremath{\mathrm{q2}}\right) }}{2} & -\frac{\cos{\left( \ensuremath{\mathrm{q1}}\right) } \sin{\left( \ensuremath{\mathrm{q2}}\right) }+\sin{\left( \ensuremath{\mathrm{q1}}\right) } \cos{\left( \ensuremath{\mathrm{q2}}\right) }}{2} & -\frac{\sin{\left( \ensuremath{\mathrm{q1}}\right) }-2 \cos{\left( \ensuremath{\mathrm{q1}}\right) }-4 \sqrt{3}-4}{2}\\ -\frac{1}{2} & -\frac{\sqrt{3} \sin{\left( \ensuremath{\mathrm{q1}}\right) } \sin{\left( \ensuremath{\mathrm{q2}}\right) }-\sqrt{3} \cos{\left( \ensuremath{\mathrm{q1}}\right) } \cos{\left( \ensuremath{\mathrm{q2}}\right) }}{2} & -\frac{\sqrt{3} \cos{\left( \ensuremath{\mathrm{q1}}\right) } \sin{\left( \ensuremath{\mathrm{q2}}\right) }+\sqrt{3} \sin{\left( \ensuremath{\mathrm{q1}}\right) } \cos{\left( \ensuremath{\mathrm{q2}}\right) }}{2} & -\frac{\sqrt{3} \sin{\left( \ensuremath{\mathrm{q1}}\right) }-2 \sqrt{3} \cos{\left( \ensuremath{\mathrm{q1}}\right) }+2}{2}\\ 0 & \cos{\left( \ensuremath{\mathrm{q1}}\right) } \sin{\left( \ensuremath{\mathrm{q2}}\right) }+\sin{\left( \ensuremath{\mathrm{q1}}\right) } \cos{\left( \ensuremath{\mathrm{q2}}\right) } & \cos{\left( \ensuremath{\mathrm{q1}}\right) } \cos{\left( \ensuremath{\mathrm{q2}}\right) }-\sin{\left( \ensuremath{\mathrm{q1}}\right) } \sin{\left( \ensuremath{\mathrm{q2}}\right) } & 2 \sin{\left( \ensuremath{\mathrm{q1}}\right) }+\cos{\left( \ensuremath{\mathrm{q1}}\right) }+3\\ 0 & 0 & 0 & 1\end{pmatrix}\]
(%i33) |
H_0_3
:
H_0_2
.
H_2_3
$
trigsimp ( % ) ; |
\[\operatorname{ }\begin{pmatrix}\frac{\sqrt{3}}{2} & -\frac{\left( \cos{\left( \ensuremath{\mathrm{q1}}\right) } \sin{\left( \ensuremath{\mathrm{q2}}\right) }+\sin{\left( \ensuremath{\mathrm{q1}}\right) } \cos{\left( \ensuremath{\mathrm{q2}}\right) }\right) \sin{\left( \ensuremath{\mathrm{q3}}\right) }+\left( \sin{\left( \ensuremath{\mathrm{q1}}\right) } \sin{\left( \ensuremath{\mathrm{q2}}\right) }-\cos{\left( \ensuremath{\mathrm{q1}}\right) } \cos{\left( \ensuremath{\mathrm{q2}}\right) }\right) \cos{\left( \ensuremath{\mathrm{q3}}\right) }}{2} & \frac{\left( \sin{\left( \ensuremath{\mathrm{q1}}\right) } \sin{\left( \ensuremath{\mathrm{q2}}\right) }-\cos{\left( \ensuremath{\mathrm{q1}}\right) } \cos{\left( \ensuremath{\mathrm{q2}}\right) }\right) \sin{\left( \ensuremath{\mathrm{q3}}\right) }+\left( -\cos{\left( \ensuremath{\mathrm{q1}}\right) } \sin{\left( \ensuremath{\mathrm{q2}}\right) }-\sin{\left( \ensuremath{\mathrm{q1}}\right) } \cos{\left( \ensuremath{\mathrm{q2}}\right) }\right) \cos{\left( \ensuremath{\mathrm{q3}}\right) }}{2} & -\frac{\left( \sin{\left( \ensuremath{\mathrm{q1}}\right) }+4 \cos{\left( \ensuremath{\mathrm{q1}}\right) }\right) \sin{\left( \ensuremath{\mathrm{q2}}\right) }+\left( 4 \sin{\left( \ensuremath{\mathrm{q1}}\right) }-\cos{\left( \ensuremath{\mathrm{q1}}\right) }\right) \cos{\left( \ensuremath{\mathrm{q2}}\right) }+\sin{\left( \ensuremath{\mathrm{q1}}\right) }-2 \cos{\left( \ensuremath{\mathrm{q1}}\right) }-7 \sqrt{3}-4}{2}\\ -\frac{1}{2} & -\frac{\left( \sqrt{3} \cos{\left( \ensuremath{\mathrm{q1}}\right) } \sin{\left( \ensuremath{\mathrm{q2}}\right) }+\sqrt{3} \sin{\left( \ensuremath{\mathrm{q1}}\right) } \cos{\left( \ensuremath{\mathrm{q2}}\right) }\right) \sin{\left( \ensuremath{\mathrm{q3}}\right) }+\left( \sqrt{3} \sin{\left( \ensuremath{\mathrm{q1}}\right) } \sin{\left( \ensuremath{\mathrm{q2}}\right) }-\sqrt{3} \cos{\left( \ensuremath{\mathrm{q1}}\right) } \cos{\left( \ensuremath{\mathrm{q2}}\right) }\right) \cos{\left( \ensuremath{\mathrm{q3}}\right) }}{2} & \frac{\left( \sqrt{3} \sin{\left( \ensuremath{\mathrm{q1}}\right) } \sin{\left( \ensuremath{\mathrm{q2}}\right) }-\sqrt{3} \cos{\left( \ensuremath{\mathrm{q1}}\right) } \cos{\left( \ensuremath{\mathrm{q2}}\right) }\right) \sin{\left( \ensuremath{\mathrm{q3}}\right) }+\left( -\sqrt{3} \cos{\left( \ensuremath{\mathrm{q1}}\right) } \sin{\left( \ensuremath{\mathrm{q2}}\right) }-\sqrt{3} \sin{\left( \ensuremath{\mathrm{q1}}\right) } \cos{\left( \ensuremath{\mathrm{q2}}\right) }\right) \cos{\left( \ensuremath{\mathrm{q3}}\right) }}{2} & -\frac{\left( \sqrt{3} \sin{\left( \ensuremath{\mathrm{q1}}\right) }+4 \sqrt{3} \cos{\left( \ensuremath{\mathrm{q1}}\right) }\right) \sin{\left( \ensuremath{\mathrm{q2}}\right) }+\left( 4 \sqrt{3} \sin{\left( \ensuremath{\mathrm{q1}}\right) }-\sqrt{3} \cos{\left( \ensuremath{\mathrm{q1}}\right) }\right) \cos{\left( \ensuremath{\mathrm{q2}}\right) }+\sqrt{3} \sin{\left( \ensuremath{\mathrm{q1}}\right) }-2 \sqrt{3} \cos{\left( \ensuremath{\mathrm{q1}}\right) }+5}{2}\\ 0 & \left( \cos{\left( \ensuremath{\mathrm{q1}}\right) } \cos{\left( \ensuremath{\mathrm{q2}}\right) }-\sin{\left( \ensuremath{\mathrm{q1}}\right) } \sin{\left( \ensuremath{\mathrm{q2}}\right) }\right) \sin{\left( \ensuremath{\mathrm{q3}}\right) }+\left( \cos{\left( \ensuremath{\mathrm{q1}}\right) } \sin{\left( \ensuremath{\mathrm{q2}}\right) }+\sin{\left( \ensuremath{\mathrm{q1}}\right) } \cos{\left( \ensuremath{\mathrm{q2}}\right) }\right) \cos{\left( \ensuremath{\mathrm{q3}}\right) } & \left( -\cos{\left( \ensuremath{\mathrm{q1}}\right) } \sin{\left( \ensuremath{\mathrm{q2}}\right) }-\sin{\left( \ensuremath{\mathrm{q1}}\right) } \cos{\left( \ensuremath{\mathrm{q2}}\right) }\right) \sin{\left( \ensuremath{\mathrm{q3}}\right) }+\left( \cos{\left( \ensuremath{\mathrm{q1}}\right) } \cos{\left( \ensuremath{\mathrm{q2}}\right) }-\sin{\left( \ensuremath{\mathrm{q1}}\right) } \sin{\left( \ensuremath{\mathrm{q2}}\right) }\right) \cos{\left( \ensuremath{\mathrm{q3}}\right) } & \left( \cos{\left( \ensuremath{\mathrm{q1}}\right) }-4 \sin{\left( \ensuremath{\mathrm{q1}}\right) }\right) \sin{\left( \ensuremath{\mathrm{q2}}\right) }+\left( \sin{\left( \ensuremath{\mathrm{q1}}\right) }+4 \cos{\left( \ensuremath{\mathrm{q1}}\right) }\right) \cos{\left( \ensuremath{\mathrm{q2}}\right) }+2 \sin{\left( \ensuremath{\mathrm{q1}}\right) }+\cos{\left( \ensuremath{\mathrm{q1}}\right) }+3\\ 0 & 0 & 0 & 1\end{pmatrix}\]
(%i35) |
H_0_4
:
H_0_3
.
H_3_4
$
trigsimp ( % ) ; |
\[\operatorname{ }\begin{pmatrix}\frac{\sqrt{3}}{2} & -\frac{\left( \cos{\left( \ensuremath{\mathrm{q1}}\right) } \sin{\left( \ensuremath{\mathrm{q2}}\right) }+\sin{\left( \ensuremath{\mathrm{q1}}\right) } \cos{\left( \ensuremath{\mathrm{q2}}\right) }\right) \sin{\left( \ensuremath{\mathrm{q3}}\right) }+\left( \sin{\left( \ensuremath{\mathrm{q1}}\right) } \sin{\left( \ensuremath{\mathrm{q2}}\right) }-\cos{\left( \ensuremath{\mathrm{q1}}\right) } \cos{\left( \ensuremath{\mathrm{q2}}\right) }\right) \cos{\left( \ensuremath{\mathrm{q3}}\right) }}{2} & \frac{\left( \sin{\left( \ensuremath{\mathrm{q1}}\right) } \sin{\left( \ensuremath{\mathrm{q2}}\right) }-\cos{\left( \ensuremath{\mathrm{q1}}\right) } \cos{\left( \ensuremath{\mathrm{q2}}\right) }\right) \sin{\left( \ensuremath{\mathrm{q3}}\right) }+\left( -\cos{\left( \ensuremath{\mathrm{q1}}\right) } \sin{\left( \ensuremath{\mathrm{q2}}\right) }-\sin{\left( \ensuremath{\mathrm{q1}}\right) } \cos{\left( \ensuremath{\mathrm{q2}}\right) }\right) \cos{\left( \ensuremath{\mathrm{q3}}\right) }}{2} & \\ -\frac{1}{2} & -\frac{\left( \sqrt{3} \cos{\left( \ensuremath{\mathrm{q1}}\right) } \sin{\left( \ensuremath{\mathrm{q2}}\right) }+\sqrt{3} \sin{\left( \ensuremath{\mathrm{q1}}\right) } \cos{\left( \ensuremath{\mathrm{q2}}\right) }\right) \sin{\left( \ensuremath{\mathrm{q3}}\right) }+\left( \sqrt{3} \sin{\left( \ensuremath{\mathrm{q1}}\right) } \sin{\left( \ensuremath{\mathrm{q2}}\right) }-\sqrt{3} \cos{\left( \ensuremath{\mathrm{q1}}\right) } \cos{\left( \ensuremath{\mathrm{q2}}\right) }\right) \cos{\left( \ensuremath{\mathrm{q3}}\right) }}{2} & \frac{\left( \sqrt{3} \sin{\left( \ensuremath{\mathrm{q1}}\right) } \sin{\left( \ensuremath{\mathrm{q2}}\right) }-\sqrt{3} \cos{\left( \ensuremath{\mathrm{q1}}\right) } \cos{\left( \ensuremath{\mathrm{q2}}\right) }\right) \sin{\left( \ensuremath{\mathrm{q3}}\right) }+\left( -\sqrt{3} \cos{\left( \ensuremath{\mathrm{q1}}\right) } \sin{\left( \ensuremath{\mathrm{q2}}\right) }-\sqrt{3} \sin{\left( \ensuremath{\mathrm{q1}}\right) } \cos{\left( \ensuremath{\mathrm{q2}}\right) }\right) \cos{\left( \ensuremath{\mathrm{q3}}\right) }}{2} & -\\ 0 & \left( \cos{\left( \ensuremath{\mathrm{q1}}\right) } \cos{\left( \ensuremath{\mathrm{q2}}\right) }-\sin{\left( \ensuremath{\mathrm{q1}}\right) } \sin{\left( \ensuremath{\mathrm{q2}}\right) }\right) \sin{\left( \ensuremath{\mathrm{q3}}\right) }+\left( \cos{\left( \ensuremath{\mathrm{q1}}\right) } \sin{\left( \ensuremath{\mathrm{q2}}\right) }+\sin{\left( \ensuremath{\mathrm{q1}}\right) } \cos{\left( \ensuremath{\mathrm{q2}}\right) }\right) \cos{\left( \ensuremath{\mathrm{q3}}\right) } & \left( -\cos{\left( \ensuremath{\mathrm{q1}}\right) } \sin{\left( \ensuremath{\mathrm{q2}}\right) }-\sin{\left( \ensuremath{\mathrm{q1}}\right) } \cos{\left( \ensuremath{\mathrm{q2}}\right) }\right) \sin{\left( \ensuremath{\mathrm{q3}}\right) }+\left( \cos{\left( \ensuremath{\mathrm{q1}}\right) } \cos{\left( \ensuremath{\mathrm{q2}}\right) }-\sin{\left( \ensuremath{\mathrm{q1}}\right) } \sin{\left( \ensuremath{\mathrm{q2}}\right) }\right) \cos{\left( \ensuremath{\mathrm{q3}}\right) } & \left( \left( -4 \sin{\left( \ensuremath{\mathrm{q1}}\right) }-\cos{\left( \ensuremath{\mathrm{q1}}\right) }\right) \sin{\left( \ensuremath{\mathrm{q2}}\right) }+\left( 4 \cos{\left( \ensuremath{\mathrm{q1}}\right) }-\sin{\left( \ensuremath{\mathrm{q1}}\right) }\right) \cos{\left( \ensuremath{\mathrm{q2}}\right) }\right) \sin{\left( \ensuremath{\mathrm{q3}}\right) }+\left( \left( 4 \cos{\left( \ensuremath{\mathrm{q1}}\right) }-\sin{\left( \ensuremath{\mathrm{q1}}\right) }\right) \sin{\left( \ensuremath{\mathrm{q2}}\right) }+\left( 4 \sin{\left( \ensuremath{\mathrm{q1}}\right) }+\cos{\left( \ensuremath{\mathrm{q1}}\right) }\right) \cos{\left( \ensuremath{\mathrm{q2}}\right) }\right) \cos{\left( \ensuremath{\mathrm{q3}}\right) }+\left( \cos{\left( \ensuremath{\mathrm{q1}}\right) }-4 \sin{\left( \ensuremath{\mathrm{q1}}\right) }\right) \sin{\left( \ensuremath{\mathrm{q2}}\right) }+\left( \sin{\left( \ensuremath{\mathrm{q1}}\right) }+4 \cos{\left( \ensuremath{\mathrm{q1}}\right) }\right) \cos{\left( \ensuremath{\mathrm{q2}}\right) }+2 \sin{\left( \ensuremath{\mathrm{q1}}\right) }+\cos{\left( \ensuremath{\mathrm{q1}}\right) }+3\\ 0 & 0 & 0 & 1\end{pmatrix}\]
1.3 Aufgabe 2
(%i45) |
p_0_s1
:
submatrix
(
4
,
H_0_1
.
addrow
(
p_1_s1
,
matrix
(
[
1
]
)
)
)
$
trigsimp ( % ) ; p_0_s2 : submatrix ( 4 , H_0_2 . addrow ( p_2_s2 , matrix ( [ 1 ] ) ) ) $ trigsimp ( % ) ; p_0_s3 : submatrix ( 4 , H_0_3 . addrow ( p_3_s3 , matrix ( [ 1 ] ) ) ) $ trigsimp ( % ) ; p_0_s4 : submatrix ( 4 , H_0_4 . addrow ( p_4_s4 , matrix ( [ 1 ] ) ) ) $ trigsimp ( % ) ; p_0_E : submatrix ( 4 , H_0_4 . addrow ( p_4_E , matrix ( [ 1 ] ) ) ) $ trigsimp ( % ) ; |
\[\operatorname{ }\begin{pmatrix}-\frac{\sin{\left( \ensuremath{\mathrm{q1}}\right) }-2 \cos{\left( \ensuremath{\mathrm{q1}}\right) }-4 \sqrt{3}-8}{4}\\ -\frac{\sqrt{3} \sin{\left( \ensuremath{\mathrm{q1}}\right) }-2 \sqrt{3} \cos{\left( \ensuremath{\mathrm{q1}}\right) }}{4}\\ \frac{2 \sin{\left( \ensuremath{\mathrm{q1}}\right) }+\cos{\left( \ensuremath{\mathrm{q1}}\right) }+6}{2}\end{pmatrix}\]
\[\operatorname{ }\begin{pmatrix}-\frac{\left( \sin{\left( \ensuremath{\mathrm{q1}}\right) }+4 \cos{\left( \ensuremath{\mathrm{q1}}\right) }\right) \sin{\left( \ensuremath{\mathrm{q2}}\right) }+\left( 4 \sin{\left( \ensuremath{\mathrm{q1}}\right) }-\cos{\left( \ensuremath{\mathrm{q1}}\right) }\right) \cos{\left( \ensuremath{\mathrm{q2}}\right) }+2 \sin{\left( \ensuremath{\mathrm{q1}}\right) }-4 \cos{\left( \ensuremath{\mathrm{q1}}\right) }-11 \sqrt{3}-8}{4}\\ -\frac{\left( \sqrt{3} \sin{\left( \ensuremath{\mathrm{q1}}\right) }+4 \sqrt{3} \cos{\left( \ensuremath{\mathrm{q1}}\right) }\right) \sin{\left( \ensuremath{\mathrm{q2}}\right) }+\left( 4 \sqrt{3} \sin{\left( \ensuremath{\mathrm{q1}}\right) }-\sqrt{3} \cos{\left( \ensuremath{\mathrm{q1}}\right) }\right) \cos{\left( \ensuremath{\mathrm{q2}}\right) }+2 \sqrt{3} \sin{\left( \ensuremath{\mathrm{q1}}\right) }-4 \sqrt{3} \cos{\left( \ensuremath{\mathrm{q1}}\right) }+7}{4}\\ -\frac{\left( 4 \sin{\left( \ensuremath{\mathrm{q1}}\right) }-\cos{\left( \ensuremath{\mathrm{q1}}\right) }\right) \sin{\left( \ensuremath{\mathrm{q2}}\right) }+\left( -\sin{\left( \ensuremath{\mathrm{q1}}\right) }-4 \cos{\left( \ensuremath{\mathrm{q1}}\right) }\right) \cos{\left( \ensuremath{\mathrm{q2}}\right) }-4 \sin{\left( \ensuremath{\mathrm{q1}}\right) }-2 \cos{\left( \ensuremath{\mathrm{q1}}\right) }-6}{2}\end{pmatrix}\]
\[\operatorname{ }\begin{pmatrix}\\ \\ -\end{pmatrix}\]
\[\operatorname{ }\begin{pmatrix}\\ -\\ -\end{pmatrix}\]
\[\operatorname{ }\begin{pmatrix}\\ -\\ \left( \left( -5 \sin{\left( \ensuremath{\mathrm{q1}}\right) }-3 \cos{\left( \ensuremath{\mathrm{q1}}\right) }\right) \sin{\left( \ensuremath{\mathrm{q2}}\right) }+\left( 5 \cos{\left( \ensuremath{\mathrm{q1}}\right) }-3 \sin{\left( \ensuremath{\mathrm{q1}}\right) }\right) \cos{\left( \ensuremath{\mathrm{q2}}\right) }\right) \sin{\left( \ensuremath{\mathrm{q3}}\right) }+\left( \left( 5 \cos{\left( \ensuremath{\mathrm{q1}}\right) }-3 \sin{\left( \ensuremath{\mathrm{q1}}\right) }\right) \sin{\left( \ensuremath{\mathrm{q2}}\right) }+\left( 5 \sin{\left( \ensuremath{\mathrm{q1}}\right) }+3 \cos{\left( \ensuremath{\mathrm{q1}}\right) }\right) \cos{\left( \ensuremath{\mathrm{q2}}\right) }\right) \cos{\left( \ensuremath{\mathrm{q3}}\right) }+\left( \cos{\left( \ensuremath{\mathrm{q1}}\right) }-4 \sin{\left( \ensuremath{\mathrm{q1}}\right) }\right) \sin{\left( \ensuremath{\mathrm{q2}}\right) }+\left( \sin{\left( \ensuremath{\mathrm{q1}}\right) }+4 \cos{\left( \ensuremath{\mathrm{q1}}\right) }\right) \cos{\left( \ensuremath{\mathrm{q2}}\right) }+2 \sin{\left( \ensuremath{\mathrm{q1}}\right) }+\cos{\left( \ensuremath{\mathrm{q1}}\right) }+3\end{pmatrix}\]
(%i46) | j_p_s : jacobian ( [ p_0_s1 , p_0_s2 , p_0_s3 , p_0_s4 , p_0_E ] , [ q1 , q2 , q3 , q4 ] ) $ |
(%i47) | j_v_0_s1 : trigsimp ( addcol ( j_p_s [ 1 , 1 ] , j_p_s [ 1 , 2 ] , j_p_s [ 1 , 3 ] , j_p_s [ 1 , 4 ] ) ) ; |
\[\operatorname{ }\begin{pmatrix}-\frac{2 \sin{\left( \ensuremath{\mathrm{q1}}\right) }+\cos{\left( \ensuremath{\mathrm{q1}}\right) }}{4} & 0 & 0 & 0\\ -\frac{2 \sqrt{3} \sin{\left( \ensuremath{\mathrm{q1}}\right) }+\sqrt{3} \cos{\left( \ensuremath{\mathrm{q1}}\right) }}{4} & 0 & 0 & 0\\ -\frac{\sin{\left( \ensuremath{\mathrm{q1}}\right) }-2 \cos{\left( \ensuremath{\mathrm{q1}}\right) }}{2} & 0 & 0 & 0\end{pmatrix}\]
(%i48) | j_v_0_s2 : trigsimp ( addcol ( j_p_s [ 2 , 1 ] , j_p_s [ 2 , 2 ] , j_p_s [ 2 , 3 ] , j_p_s [ 2 , 4 ] ) ) ; |
\[\operatorname{ }\begin{pmatrix}\frac{\left( 4 \sin{\left( \ensuremath{\mathrm{q1}}\right) }-\cos{\left( \ensuremath{\mathrm{q1}}\right) }\right) \sin{\left( \ensuremath{\mathrm{q2}}\right) }+\left( -\sin{\left( \ensuremath{\mathrm{q1}}\right) }-4 \cos{\left( \ensuremath{\mathrm{q1}}\right) }\right) \cos{\left( \ensuremath{\mathrm{q2}}\right) }-4 \sin{\left( \ensuremath{\mathrm{q1}}\right) }-2 \cos{\left( \ensuremath{\mathrm{q1}}\right) }}{4} & \frac{\left( 4 \sin{\left( \ensuremath{\mathrm{q1}}\right) }-\cos{\left( \ensuremath{\mathrm{q1}}\right) }\right) \sin{\left( \ensuremath{\mathrm{q2}}\right) }+\left( -\sin{\left( \ensuremath{\mathrm{q1}}\right) }-4 \cos{\left( \ensuremath{\mathrm{q1}}\right) }\right) \cos{\left( \ensuremath{\mathrm{q2}}\right) }}{4} & 0 & 0\\ \frac{\left( 4 \sqrt{3} \sin{\left( \ensuremath{\mathrm{q1}}\right) }-\sqrt{3} \cos{\left( \ensuremath{\mathrm{q1}}\right) }\right) \sin{\left( \ensuremath{\mathrm{q2}}\right) }+\left( -\sqrt{3} \sin{\left( \ensuremath{\mathrm{q1}}\right) }-4 \sqrt{3} \cos{\left( \ensuremath{\mathrm{q1}}\right) }\right) \cos{\left( \ensuremath{\mathrm{q2}}\right) }-4 \sqrt{3} \sin{\left( \ensuremath{\mathrm{q1}}\right) }-2 \sqrt{3} \cos{\left( \ensuremath{\mathrm{q1}}\right) }}{4} & \frac{\left( 4 \sqrt{3} \sin{\left( \ensuremath{\mathrm{q1}}\right) }-\sqrt{3} \cos{\left( \ensuremath{\mathrm{q1}}\right) }\right) \sin{\left( \ensuremath{\mathrm{q2}}\right) }+\left( -\sqrt{3} \sin{\left( \ensuremath{\mathrm{q1}}\right) }-4 \sqrt{3} \cos{\left( \ensuremath{\mathrm{q1}}\right) }\right) \cos{\left( \ensuremath{\mathrm{q2}}\right) }}{4} & 0 & 0\\ -\frac{\left( \sin{\left( \ensuremath{\mathrm{q1}}\right) }+4 \cos{\left( \ensuremath{\mathrm{q1}}\right) }\right) \sin{\left( \ensuremath{\mathrm{q2}}\right) }+\left( 4 \sin{\left( \ensuremath{\mathrm{q1}}\right) }-\cos{\left( \ensuremath{\mathrm{q1}}\right) }\right) \cos{\left( \ensuremath{\mathrm{q2}}\right) }+2 \sin{\left( \ensuremath{\mathrm{q1}}\right) }-4 \cos{\left( \ensuremath{\mathrm{q1}}\right) }}{2} & -\frac{\left( \sin{\left( \ensuremath{\mathrm{q1}}\right) }+4 \cos{\left( \ensuremath{\mathrm{q1}}\right) }\right) \sin{\left( \ensuremath{\mathrm{q2}}\right) }+\left( 4 \sin{\left( \ensuremath{\mathrm{q1}}\right) }-\cos{\left( \ensuremath{\mathrm{q1}}\right) }\right) \cos{\left( \ensuremath{\mathrm{q2}}\right) }}{2} & 0 & 0\end{pmatrix}\]
(%i49) | j_v_0_s3 : trigsimp ( addcol ( j_p_s [ 3 , 1 ] , j_p_s [ 3 , 2 ] , j_p_s [ 3 , 3 ] , j_p_s [ 3 , 4 ] ) ) ; |
\[\operatorname{ }\begin{pmatrix} & & \frac{\left( \left( 4 \sin{\left( \ensuremath{\mathrm{q1}}\right) }+\cos{\left( \ensuremath{\mathrm{q1}}\right) }\right) \sin{\left( \ensuremath{\mathrm{q2}}\right) }+\left( \sin{\left( \ensuremath{\mathrm{q1}}\right) }-4 \cos{\left( \ensuremath{\mathrm{q1}}\right) }\right) \cos{\left( \ensuremath{\mathrm{q2}}\right) }\right) \sin{\left( \ensuremath{\mathrm{q3}}\right) }+\left( \left( \sin{\left( \ensuremath{\mathrm{q1}}\right) }-4 \cos{\left( \ensuremath{\mathrm{q1}}\right) }\right) \sin{\left( \ensuremath{\mathrm{q2}}\right) }+\left( -4 \sin{\left( \ensuremath{\mathrm{q1}}\right) }-\cos{\left( \ensuremath{\mathrm{q1}}\right) }\right) \cos{\left( \ensuremath{\mathrm{q2}}\right) }\right) \cos{\left( \ensuremath{\mathrm{q3}}\right) }}{4} & 0\\ & & & 0\\ & & \frac{\left( \left( \sin{\left( \ensuremath{\mathrm{q1}}\right) }-4 \cos{\left( \ensuremath{\mathrm{q1}}\right) }\right) \sin{\left( \ensuremath{\mathrm{q2}}\right) }+\left( -4 \sin{\left( \ensuremath{\mathrm{q1}}\right) }-\cos{\left( \ensuremath{\mathrm{q1}}\right) }\right) \cos{\left( \ensuremath{\mathrm{q2}}\right) }\right) \sin{\left( \ensuremath{\mathrm{q3}}\right) }+\left( \left( -4 \sin{\left( \ensuremath{\mathrm{q1}}\right) }-\cos{\left( \ensuremath{\mathrm{q1}}\right) }\right) \sin{\left( \ensuremath{\mathrm{q2}}\right) }+\left( 4 \cos{\left( \ensuremath{\mathrm{q1}}\right) }-\sin{\left( \ensuremath{\mathrm{q1}}\right) }\right) \cos{\left( \ensuremath{\mathrm{q2}}\right) }\right) \cos{\left( \ensuremath{\mathrm{q3}}\right) }}{2} & 0\end{pmatrix}\]
(%i50) | j_v_0_s4 : trigsimp ( addcol ( j_p_s [ 4 , 1 ] , j_p_s [ 4 , 2 ] , j_p_s [ 4 , 3 ] , j_p_s [ 4 , 4 ] ) ) ; |
\[\operatorname{ }\begin{pmatrix} & & \frac{\left( \left( 9 \sin{\left( \ensuremath{\mathrm{q1}}\right) }+4 \cos{\left( \ensuremath{\mathrm{q1}}\right) }\right) \sin{\left( \ensuremath{\mathrm{q2}}\right) }+\left( 4 \sin{\left( \ensuremath{\mathrm{q1}}\right) }-9 \cos{\left( \ensuremath{\mathrm{q1}}\right) }\right) \cos{\left( \ensuremath{\mathrm{q2}}\right) }\right) \sin{\left( \ensuremath{\mathrm{q3}}\right) }+\left( \left( 4 \sin{\left( \ensuremath{\mathrm{q1}}\right) }-9 \cos{\left( \ensuremath{\mathrm{q1}}\right) }\right) \sin{\left( \ensuremath{\mathrm{q2}}\right) }+\left( -9 \sin{\left( \ensuremath{\mathrm{q1}}\right) }-4 \cos{\left( \ensuremath{\mathrm{q1}}\right) }\right) \cos{\left( \ensuremath{\mathrm{q2}}\right) }\right) \cos{\left( \ensuremath{\mathrm{q3}}\right) }}{4} & \frac{\sqrt{3}}{2}\\ & & & -\frac{1}{2}\\ & & \frac{\left( \left( 4 \sin{\left( \ensuremath{\mathrm{q1}}\right) }-9 \cos{\left( \ensuremath{\mathrm{q1}}\right) }\right) \sin{\left( \ensuremath{\mathrm{q2}}\right) }+\left( -9 \sin{\left( \ensuremath{\mathrm{q1}}\right) }-4 \cos{\left( \ensuremath{\mathrm{q1}}\right) }\right) \cos{\left( \ensuremath{\mathrm{q2}}\right) }\right) \sin{\left( \ensuremath{\mathrm{q3}}\right) }+\left( \left( -9 \sin{\left( \ensuremath{\mathrm{q1}}\right) }-4 \cos{\left( \ensuremath{\mathrm{q1}}\right) }\right) \sin{\left( \ensuremath{\mathrm{q2}}\right) }+\left( 9 \cos{\left( \ensuremath{\mathrm{q1}}\right) }-4 \sin{\left( \ensuremath{\mathrm{q1}}\right) }\right) \cos{\left( \ensuremath{\mathrm{q2}}\right) }\right) \cos{\left( \ensuremath{\mathrm{q3}}\right) }}{2} & 0\end{pmatrix}\]
(%i51) | j_v_0_E : trigsimp ( addcol ( j_p_s [ 5 , 1 ] , j_p_s [ 5 , 2 ] , j_p_s [ 5 , 3 ] , j_p_s [ 5 , 4 ] ) ) ; |
\[\operatorname{ }\begin{pmatrix} & & \frac{\left( \left( 5 \sin{\left( \ensuremath{\mathrm{q1}}\right) }+3 \cos{\left( \ensuremath{\mathrm{q1}}\right) }\right) \sin{\left( \ensuremath{\mathrm{q2}}\right) }+\left( 3 \sin{\left( \ensuremath{\mathrm{q1}}\right) }-5 \cos{\left( \ensuremath{\mathrm{q1}}\right) }\right) \cos{\left( \ensuremath{\mathrm{q2}}\right) }\right) \sin{\left( \ensuremath{\mathrm{q3}}\right) }+\left( \left( 3 \sin{\left( \ensuremath{\mathrm{q1}}\right) }-5 \cos{\left( \ensuremath{\mathrm{q1}}\right) }\right) \sin{\left( \ensuremath{\mathrm{q2}}\right) }+\left( -5 \sin{\left( \ensuremath{\mathrm{q1}}\right) }-3 \cos{\left( \ensuremath{\mathrm{q1}}\right) }\right) \cos{\left( \ensuremath{\mathrm{q2}}\right) }\right) \cos{\left( \ensuremath{\mathrm{q3}}\right) }}{2} & \frac{\sqrt{3}}{2}\\ & & & -\frac{1}{2}\\ \left( \left( 3 \sin{\left( \ensuremath{\mathrm{q1}}\right) }-5 \cos{\left( \ensuremath{\mathrm{q1}}\right) }\right) \sin{\left( \ensuremath{\mathrm{q2}}\right) }+\left( -5 \sin{\left( \ensuremath{\mathrm{q1}}\right) }-3 \cos{\left( \ensuremath{\mathrm{q1}}\right) }\right) \cos{\left( \ensuremath{\mathrm{q2}}\right) }\right) \sin{\left( \ensuremath{\mathrm{q3}}\right) }+\left( \left( -5 \sin{\left( \ensuremath{\mathrm{q1}}\right) }-3 \cos{\left( \ensuremath{\mathrm{q1}}\right) }\right) \sin{\left( \ensuremath{\mathrm{q2}}\right) }+\left( 5 \cos{\left( \ensuremath{\mathrm{q1}}\right) }-3 \sin{\left( \ensuremath{\mathrm{q1}}\right) }\right) \cos{\left( \ensuremath{\mathrm{q2}}\right) }\right) \cos{\left( \ensuremath{\mathrm{q3}}\right) }+\left( -\sin{\left( \ensuremath{\mathrm{q1}}\right) }-4 \cos{\left( \ensuremath{\mathrm{q1}}\right) }\right) \sin{\left( \ensuremath{\mathrm{q2}}\right) }+\left( \cos{\left( \ensuremath{\mathrm{q1}}\right) }-4 \sin{\left( \ensuremath{\mathrm{q1}}\right) }\right) \cos{\left( \ensuremath{\mathrm{q2}}\right) }-\sin{\left( \ensuremath{\mathrm{q1}}\right) }+2 \cos{\left( \ensuremath{\mathrm{q1}}\right) } & \left( \left( 3 \sin{\left( \ensuremath{\mathrm{q1}}\right) }-5 \cos{\left( \ensuremath{\mathrm{q1}}\right) }\right) \sin{\left( \ensuremath{\mathrm{q2}}\right) }+\left( -5 \sin{\left( \ensuremath{\mathrm{q1}}\right) }-3 \cos{\left( \ensuremath{\mathrm{q1}}\right) }\right) \cos{\left( \ensuremath{\mathrm{q2}}\right) }\right) \sin{\left( \ensuremath{\mathrm{q3}}\right) }+\left( \left( -5 \sin{\left( \ensuremath{\mathrm{q1}}\right) }-3 \cos{\left( \ensuremath{\mathrm{q1}}\right) }\right) \sin{\left( \ensuremath{\mathrm{q2}}\right) }+\left( 5 \cos{\left( \ensuremath{\mathrm{q1}}\right) }-3 \sin{\left( \ensuremath{\mathrm{q1}}\right) }\right) \cos{\left( \ensuremath{\mathrm{q2}}\right) }\right) \cos{\left( \ensuremath{\mathrm{q3}}\right) }+\left( -\sin{\left( \ensuremath{\mathrm{q1}}\right) }-4 \cos{\left( \ensuremath{\mathrm{q1}}\right) }\right) \sin{\left( \ensuremath{\mathrm{q2}}\right) }+\left( \cos{\left( \ensuremath{\mathrm{q1}}\right) }-4 \sin{\left( \ensuremath{\mathrm{q1}}\right) }\right) \cos{\left( \ensuremath{\mathrm{q2}}\right) } & \left( \left( 3 \sin{\left( \ensuremath{\mathrm{q1}}\right) }-5 \cos{\left( \ensuremath{\mathrm{q1}}\right) }\right) \sin{\left( \ensuremath{\mathrm{q2}}\right) }+\left( -5 \sin{\left( \ensuremath{\mathrm{q1}}\right) }-3 \cos{\left( \ensuremath{\mathrm{q1}}\right) }\right) \cos{\left( \ensuremath{\mathrm{q2}}\right) }\right) \sin{\left( \ensuremath{\mathrm{q3}}\right) }+\left( \left( -5 \sin{\left( \ensuremath{\mathrm{q1}}\right) }-3 \cos{\left( \ensuremath{\mathrm{q1}}\right) }\right) \sin{\left( \ensuremath{\mathrm{q2}}\right) }+\left( 5 \cos{\left( \ensuremath{\mathrm{q1}}\right) }-3 \sin{\left( \ensuremath{\mathrm{q1}}\right) }\right) \cos{\left( \ensuremath{\mathrm{q2}}\right) }\right) \cos{\left( \ensuremath{\mathrm{q3}}\right) } & 0\end{pmatrix}\]
(%i54) |
R_0_1
:
R_0_1_z
.
R_0_1_x
$
R_1_2 : R_1_2_x $ R_2_3 : R_2_3_x $ |
(%i58) |
R_0_2
:
R_0_1
.
R_1_2
$
R_0_3 : R_0_2 . R_2_3 $ R_0_4 : R_0_3 . R_3_4 $ R_0_E : R_0_4 $ |
(%i61) |
R
:
[
R_0_1
,
R_0_2
,
R_0_3
,
R_0_4
,
R_0_E
]
$
q : [ q1 , q2 , q3 , q4 ] $ q_d : [ qd1 , qd2 , qd3 , qd4 ] $ |
(%i68) |
sum
:
0
$
calc : R_0_1 $ for y : 1 while y < = 4 do ( sum : sum + ( diff ( calc , q [ y ] , 1 ) . transpose ( calc ) · q_d [ y ] ) ) $ trigsimp ( sum ) $ matrix ( [ sum [ 3 ] [ 2 ] ] , [ sum [ 1 ] [ 3 ] ] , [ sum [ 2 ] [ 1 ] ] ) $ J_sum : jacobian ( [ % ] , q_d ) $ J_w_0_1 : trigsimp ( addcol ( J_sum [ 1 ] [ 1 ] , J_sum [ 1 ] [ 2 ] , J_sum [ 1 ] [ 3 ] , J_sum [ 1 ] [ 4 ] ) ) ; |
\[\operatorname{ }\begin{pmatrix}\frac{\sqrt{3}}{2} & 0 & 0 & 0\\ -\frac{1}{2} & 0 & 0 & 0\\ 0 & 0 & 0 & 0\end{pmatrix}\]
(%i75) |
sum
:
0
$
calc : R_0_2 $ for y : 1 while y < = 4 do ( sum : sum + ( diff ( calc , q [ y ] , 1 ) . transpose ( calc ) · q_d [ y ] ) ) $ trigsimp ( sum ) $ matrix ( [ sum [ 3 ] [ 2 ] ] , [ sum [ 1 ] [ 3 ] ] , [ sum [ 2 ] [ 1 ] ] ) $ J_sum : jacobian ( [ % ] , q_d ) $ J_w_0_2 : trigsimp ( addcol ( J_sum [ 1 ] [ 1 ] , J_sum [ 1 ] [ 2 ] , J_sum [ 1 ] [ 3 ] , J_sum [ 1 ] [ 4 ] ) ) ; |
\[\operatorname{ }\begin{pmatrix}\frac{\sqrt{3}}{2} & \frac{\sqrt{3}}{2} & 0 & 0\\ -\frac{1}{2} & -\frac{1}{2} & 0 & 0\\ 0 & 0 & 0 & 0\end{pmatrix}\]
(%i82) |
sum
:
0
$
calc : R_0_3 $ for y : 1 while y < = 4 do ( sum : sum + ( diff ( calc , q [ y ] , 1 ) . transpose ( calc ) · q_d [ y ] ) ) $ trigsimp ( sum ) $ matrix ( [ sum [ 3 ] [ 2 ] ] , [ sum [ 1 ] [ 3 ] ] , [ sum [ 2 ] [ 1 ] ] ) $ J_sum : jacobian ( [ % ] , q_d ) $ J_w_0_3 : trigsimp ( addcol ( J_sum [ 1 ] [ 1 ] , J_sum [ 1 ] [ 2 ] , J_sum [ 1 ] [ 3 ] , J_sum [ 1 ] [ 4 ] ) ) ; |
\[\operatorname{ }\begin{pmatrix}\frac{\sqrt{3}}{2} & \frac{\sqrt{3}}{2} & \frac{\sqrt{3}}{2} & 0\\ -\frac{1}{2} & -\frac{1}{2} & -\frac{1}{2} & 0\\ 0 & 0 & 0 & 0\end{pmatrix}\]
(%i89) |
sum
:
0
$
calc : R_0_4 $ for y : 1 while y < = 4 do ( sum : sum + ( diff ( calc , q [ y ] , 1 ) . transpose ( calc ) · q_d [ y ] ) ) $ trigsimp ( sum ) $ matrix ( [ sum [ 3 ] [ 2 ] ] , [ sum [ 1 ] [ 3 ] ] , [ sum [ 2 ] [ 1 ] ] ) $ J_sum : jacobian ( [ % ] , q_d ) $ J_w_0_4 : trigsimp ( addcol ( J_sum [ 1 ] [ 1 ] , J_sum [ 1 ] [ 2 ] , J_sum [ 1 ] [ 3 ] , J_sum [ 1 ] [ 4 ] ) ) ; |
\[\operatorname{ }\begin{pmatrix}\frac{\sqrt{3}}{2} & \frac{\sqrt{3}}{2} & \frac{\sqrt{3}}{2} & 0\\ -\frac{1}{2} & -\frac{1}{2} & -\frac{1}{2} & 0\\ 0 & 0 & 0 & 0\end{pmatrix}\]
(%i96) |
sum
:
0
$
calc : R_0_E $ for y : 1 while y < = 4 do ( sum : sum + ( diff ( calc , q [ y ] , 1 ) . transpose ( calc ) · q_d [ y ] ) ) $ trigsimp ( sum ) $ matrix ( [ sum [ 3 ] [ 2 ] ] , [ sum [ 1 ] [ 3 ] ] , [ sum [ 2 ] [ 1 ] ] ) $ J_sum : jacobian ( [ % ] , q_d ) $ J_w_0_E : trigsimp ( addcol ( J_sum [ 1 ] [ 1 ] , J_sum [ 1 ] [ 2 ] , J_sum [ 1 ] [ 3 ] , J_sum [ 1 ] [ 4 ] ) ) ; |
\[\operatorname{ }\begin{pmatrix}\frac{\sqrt{3}}{2} & \frac{\sqrt{3}}{2} & \frac{\sqrt{3}}{2} & 0\\ -\frac{1}{2} & -\frac{1}{2} & -\frac{1}{2} & 0\\ 0 & 0 & 0 & 0\end{pmatrix}\]
1.4 Aufgabe 3
(%i99) |
m
:
[
m1
,
m2
,
m3
,
m4
]
$
J_w : [ J_w_0_1 , J_w_0_2 , J_w_0_3 , J_w_0_4 ] $ J_v : [ j_v_0_s1 , j_v_0_s2 , j_v_0_s3 , j_v_0_s4 ] $ |
1.4.1 Massematrix
1.4.1.1 Translatorischer Anteil
(%i102) |
sum
:
0
$
for i : 1 while i < = 4 do ( sum : sum + ( ( transpose ( J_v [ i ] ) · m [ i ] ) . J_v [ i ] ) ) $ MM_v : trigsimp ( sum ) ; |
\[\operatorname{ }\begin{pmatrix}- & - & - & 0\\ - & \frac{\left( 128 \ensuremath{\mathrm{m4}}+60 \ensuremath{\mathrm{m3}}\right) \sin{\left( \ensuremath{\mathrm{q3}}\right) }+\left( 100 \ensuremath{\mathrm{m4}}+32 \ensuremath{\mathrm{m3}}\right) \cos{\left( \ensuremath{\mathrm{q3}}\right) }+165 \ensuremath{\mathrm{m4}}+85 \ensuremath{\mathrm{m3}}+17 \ensuremath{\mathrm{m2}}}{4} & \frac{\left( 64 \ensuremath{\mathrm{m4}}+30 \ensuremath{\mathrm{m3}}\right) \sin{\left( \ensuremath{\mathrm{q3}}\right) }+\left( 50 \ensuremath{\mathrm{m4}}+16 \ensuremath{\mathrm{m3}}\right) \cos{\left( \ensuremath{\mathrm{q3}}\right) }+97 \ensuremath{\mathrm{m4}}+17 \ensuremath{\mathrm{m3}}}{4} & 0\\ - & \frac{\left( 64 \ensuremath{\mathrm{m4}}+30 \ensuremath{\mathrm{m3}}\right) \sin{\left( \ensuremath{\mathrm{q3}}\right) }+\left( 50 \ensuremath{\mathrm{m4}}+16 \ensuremath{\mathrm{m3}}\right) \cos{\left( \ensuremath{\mathrm{q3}}\right) }+97 \ensuremath{\mathrm{m4}}+17 \ensuremath{\mathrm{m3}}}{4} & \frac{97 \ensuremath{\mathrm{m4}}+17 \ensuremath{\mathrm{m3}}}{4} & 0\\ 0 & 0 & 0 & \ensuremath{\mathrm{m4}}\end{pmatrix}\]
1.4.1.2 Rotatorischer Anteil
(%i106) |
I1
:
matrix
(
[
I1xx
,
0
,
0
]
,
[
0
,
I1yy
,
0
]
,
[
0
,
0
,
I1zz
]
)
$
I2 : matrix ( [ I2xx , 0 , 0 ] , [ 0 , I2yy , 0 ] , [ 0 , 0 , I2zz ] ) $ I3 : matrix ( [ I3xx , 0 , 0 ] , [ 0 , I3yy , 0 ] , [ 0 , 0 , I3zz ] ) $ I4 : matrix ( [ I4xx , 0 , 0 ] , [ 0 , I4yy , 0 ] , [ 0 , 0 , I4zz ] ) $ |
(%i111) |
I_0_s1
:
trigsimp
(
R_0_1
.
I1
.
transpose
(
R_0_1
)
)
$
I_0_s2 : trigsimp ( R_0_2 . I2 . transpose ( R_0_2 ) ) $ I_0_s3 : trigsimp ( R_0_3 . I3 . transpose ( R_0_3 ) ) $ I_0_s4 : trigsimp ( R_0_4 . I4 . transpose ( R_0_4 ) ) $ I_0_s : [ I_0_s1 , I_0_s2 , I_0_s3 , I_0_s4 ] $ |
(%i114) |
sum
:
0
$
for i : 1 while i < = 4 do ( sum : sum + ( ( transpose ( J_w [ i ] ) . I_0_s [ i ] ) . J_w [ i ] ) ) $ MM_w : trigsimp ( sum ) ; |
\[\operatorname{ }\begin{pmatrix}\ensuremath{\mathrm{I4xx}}+\ensuremath{\mathrm{I3xx}}+\ensuremath{\mathrm{I2xx}}+\ensuremath{\mathrm{I1xx}} & \ensuremath{\mathrm{I4xx}}+\ensuremath{\mathrm{I3xx}}+\ensuremath{\mathrm{I2xx}} & \ensuremath{\mathrm{I4xx}}+\ensuremath{\mathrm{I3xx}} & 0\\ \ensuremath{\mathrm{I4xx}}+\ensuremath{\mathrm{I3xx}}+\ensuremath{\mathrm{I2xx}} & \ensuremath{\mathrm{I4xx}}+\ensuremath{\mathrm{I3xx}}+\ensuremath{\mathrm{I2xx}} & \ensuremath{\mathrm{I4xx}}+\ensuremath{\mathrm{I3xx}} & 0\\ \ensuremath{\mathrm{I4xx}}+\ensuremath{\mathrm{I3xx}} & \ensuremath{\mathrm{I4xx}}+\ensuremath{\mathrm{I3xx}} & \ensuremath{\mathrm{I4xx}}+\ensuremath{\mathrm{I3xx}} & 0\\ 0 & 0 & 0 & 0\end{pmatrix}\]
1.4.1.3 Gesamt
(%i115) | MM : trigsimp ( MM_v + MM_w ) ; |
\[\operatorname{ }\begin{pmatrix}- & - & - & 0\\ - & \frac{\left( 128 \ensuremath{\mathrm{m4}}+60 \ensuremath{\mathrm{m3}}\right) \sin{\left( \ensuremath{\mathrm{q3}}\right) }+\left( 100 \ensuremath{\mathrm{m4}}+32 \ensuremath{\mathrm{m3}}\right) \cos{\left( \ensuremath{\mathrm{q3}}\right) }+165 \ensuremath{\mathrm{m4}}+85 \ensuremath{\mathrm{m3}}+17 \ensuremath{\mathrm{m2}}+4 \ensuremath{\mathrm{I4xx}}+4 \ensuremath{\mathrm{I3xx}}+4 \ensuremath{\mathrm{I2xx}}}{4} & \frac{\left( 64 \ensuremath{\mathrm{m4}}+30 \ensuremath{\mathrm{m3}}\right) \sin{\left( \ensuremath{\mathrm{q3}}\right) }+\left( 50 \ensuremath{\mathrm{m4}}+16 \ensuremath{\mathrm{m3}}\right) \cos{\left( \ensuremath{\mathrm{q3}}\right) }+97 \ensuremath{\mathrm{m4}}+17 \ensuremath{\mathrm{m3}}+4 \ensuremath{\mathrm{I4xx}}+4 \ensuremath{\mathrm{I3xx}}}{4} & 0\\ - & \frac{\left( 64 \ensuremath{\mathrm{m4}}+30 \ensuremath{\mathrm{m3}}\right) \sin{\left( \ensuremath{\mathrm{q3}}\right) }+\left( 50 \ensuremath{\mathrm{m4}}+16 \ensuremath{\mathrm{m3}}\right) \cos{\left( \ensuremath{\mathrm{q3}}\right) }+97 \ensuremath{\mathrm{m4}}+17 \ensuremath{\mathrm{m3}}+4 \ensuremath{\mathrm{I4xx}}+4 \ensuremath{\mathrm{I3xx}}}{4} & \frac{97 \ensuremath{\mathrm{m4}}+17 \ensuremath{\mathrm{m3}}+4 \ensuremath{\mathrm{I4xx}}+4 \ensuremath{\mathrm{I3xx}}}{4} & 0\\ 0 & 0 & 0 & \ensuremath{\mathrm{m4}}\end{pmatrix}\]
1.4.2 Coriolismatrix
(%i118) |
CM
:
zeromatrix
(
4
,
4
)
$
for j : 1 while j < = 4 do ( for k : 1 while k < = 4 do ( for i : 1 while i < = 4 do ( cc_ijk : 1 / 2 · ( diff ( MM [ k ] [ j ] , q [ i ] ) + diff ( MM [ k ] [ i ] , q [ j ] ) − diff ( MM [ i ] [ j ] , q [ k ] ) ) , CM [ k ] [ j ] : CM [ k ] [ j ] + cc_ijk · q_d [ i ] ) ) ) $ trigsimp ( CM ) ; |
\[\operatorname{ }\begin{pmatrix}- & - & - & 0\\ - & -\frac{\left( \left( 25 \ensuremath{\mathrm{m4}}+8 \ensuremath{\mathrm{m3}}\right) \sin{\left( \ensuremath{\mathrm{q3}}\right) }+\left( -32 \ensuremath{\mathrm{m4}}-15 \ensuremath{\mathrm{m3}}\right) \cos{\left( \ensuremath{\mathrm{q3}}\right) }\right) \, \ensuremath{\mathrm{qd3}}}{2} & - & 0\\ & \frac{\left( \left( 25 \ensuremath{\mathrm{m4}}+8 \ensuremath{\mathrm{m3}}\right) \sin{\left( \ensuremath{\mathrm{q3}}\right) }+\left( -32 \ensuremath{\mathrm{m4}}-15 \ensuremath{\mathrm{m3}}\right) \cos{\left( \ensuremath{\mathrm{q3}}\right) }\right) \, \ensuremath{\mathrm{qd2}}+\left( \left( 25 \ensuremath{\mathrm{m4}}+8 \ensuremath{\mathrm{m3}}\right) \sin{\left( \ensuremath{\mathrm{q3}}\right) }+\left( -32 \ensuremath{\mathrm{m4}}-15 \ensuremath{\mathrm{m3}}\right) \cos{\left( \ensuremath{\mathrm{q3}}\right) }\right) \, \ensuremath{\mathrm{qd1}}}{2} & 0 & 0\\ 0 & 0 & 0 & 0\end{pmatrix}\]
1.4.3 Vektor der verallgemeinerten Kräfte
(%i119) | vk : trigsimp ( transpose ( j_v_0_E ) . matrix ( [ fxe ] , [ fye ] , [ fze ] ) ) ; |
\[\operatorname{ }\begin{pmatrix}\\ \\ \\ -\frac{\ensuremath{\mathrm{fye}}-\sqrt{3} \ensuremath{\mathrm{fxe}}}{2}\end{pmatrix}\]
1.4.4 Vektor der Potentialkräfte
(%i126) | p : [ p_0_s1 , p_0_s2 , p_0_s3 , p_0_s4 , p_0_E ] $ |
(%i168) |
V
:
0
$
for i : 1 while i < = 4 do ( V : V + ( ( − m [ i ] · g ) · matrix ( [ 0 ] , [ 0 ] , [ − 1 ] ) . p [ i ] ) ) $ vp : trigsimp ( transpose ( jacobian ( [ V ] , q ) ) ) ; |
\[\operatorname{ }\begin{pmatrix}\\ \\ \\ 0\end{pmatrix}\]
Created with wxMaxima.
The source of this Maxima session can be downloaded here.