08 семестр / Разное / Скамко / inform / Kinematika

1. Прямая позиционная задача.
> restart:with(linalg):
Warning, new definition for norm
Warning, new definition for trace
>
> A:=table():
> q:=array([q1,q2,q3,0]): d:=array([520,0,0,d4]): alpha:=array([0,0,0,0]): a:=([0,-250,-170,0]):
> A[i]:=array([[cos(q[i]),-cos(alpha[i])*sin(q[i]),sin(alpha[i])*sin(q[i]),a[i]*cos(q[i])],[sin(q[i]),cos(alpha[i])*cos(q[i]),-sin(alpha[i])*cos(q[i]),a[i]*sin(q[i])],[0,sin(alpha[i]),cos(alpha[i]),d[i]],[0,0,0,1]]);

A[i] :=

[cos(q[i]) , -cos(alpha[i]) sin(q[i]) , sin(alpha[i]) sin(q[i]) ,

[0, -250, -170, 0][i] cos(q[i])]

[sin(q[i]) , cos(alpha[i]) cos(q[i]) , -sin(alpha[i]) cos(q[i]) ,

[0, -250, -170, 0][i] sin(q[i])]

[0 , sin(alpha[i]) , cos(alpha[i]) , d[i]]

[0 , 0 , 0 , 1]

> for i from 1 to 4 do
> A[i]:=array([[cos(q[i]),-cos(alpha[i])*sin(q[i]),sin(alpha[i])*sin(q[i]),a[i]*cos(q[i])],[sin(q[i]),cos(alpha[i])*cos(q[i]),-sin(alpha[i])*cos(q[i]),a[i]*sin(q[i])],[0,sin(alpha[i]),cos(alpha[i]),d[i]],[0,0,0,1]]) od;

[cos(q1) -sin(q1) 0 0]
[ ]
[sin(q1) cos(q1) 0 0]
A[1] := [ ]
[ 0 0 1 520]
[ ]
[ 0 0 0 1]


[cos(q2) -sin(q2) 0 -250 cos(q2)]
[ ]
[sin(q2) cos(q2) 0 -250 sin(q2)]
A[2] := [ ]
[ 0 0 1 0 ]
[ ]
[ 0 0 0 1 ]


[cos(q3) -sin(q3) 0 -170 cos(q3)]
[ ]
[sin(q3) cos(q3) 0 -170 sin(q3)]
A[3] := [ ]
[ 0 0 1 0 ]
[ ]
[ 0 0 0 1 ]


[1 0 0 0 ]
[ ]
[0 1 0 0 ]
A[4] := [ ]
[0 0 1 d4]
[ ]
[0 0 0 1 ]

> E:=diag(1,1,1,1);

[1 0 0 0]
[ ]
[0 1 0 0]
E := [ ]
[0 0 1 0]
[ ]
[0 0 0 1]

> T:=table():
> T[0]:=E:print(T[0]);

[1 0 0 0]
[ ]
[0 1 0 0]
[ ]
[0 0 1 0]
[ ]
[0 0 0 1]

> T[1]:=multiply(E,A[1]);

[cos(q1) -sin(q1) 0 0]
[ ]
[sin(q1) cos(q1) 0 0]
T[1] := [ ]
[ 0 0 1 520]
[ ]
[ 0 0 0 1]

>
> T[2]:=multiply(A[1],A[2]);

T[2] :=

[cos(q1) cos(q2) - sin(q1) sin(q2) ,

-cos(q1) sin(q2) - sin(q1) cos(q2) , 0 ,

-250 cos(q1) cos(q2) + 250 sin(q1) sin(q2)]

[sin(q1) cos(q2) + cos(q1) sin(q2) ,

cos(q1) cos(q2) - sin(q1) sin(q2) , 0 ,

-250 sin(q1) cos(q2) - 250 cos(q1) sin(q2)]

[0 , 0 , 1 , 520]

[0 , 0 , 0 , 1]

> T[3]:=multiply(A[1],A[2],A[3]);


T[3] :=

[%1 cos(q3) + (-cos(q1) sin(q2) - sin(q1) cos(q2)) sin(q3) ,

-%1 sin(q3) + (-cos(q1) sin(q2) - sin(q1) cos(q2)) cos(q3) , 0 ,

-170 %1 cos(q3)

- 170 (-cos(q1) sin(q2) - sin(q1) cos(q2)) sin(q3)

- 250 cos(q1) cos(q2) + 250 sin(q1) sin(q2)]

[(sin(q1) cos(q2) + cos(q1) sin(q2)) cos(q3) + %1 sin(q3) ,

-(sin(q1) cos(q2) + cos(q1) sin(q2)) sin(q3) + %1 cos(q3) , 0 ,

-170 (sin(q1) cos(q2) + cos(q1) sin(q2)) cos(q3) - 170 %1 sin(q3)

- 250 sin(q1) cos(q2) - 250 cos(q1) sin(q2)]

[0 , 0 , 1 , 520]

[0 , 0 , 0 , 1]

%1 := cos(q1) cos(q2) - sin(q1) sin(q2)

> T[4]:=multiply(A[1],A[2],A[3],A[4]);

T[4] :=

[%1 cos(q3) + (-cos(q1) sin(q2) - sin(q1) cos(q2)) sin(q3) ,

-%1 sin(q3) + (-cos(q1) sin(q2) - sin(q1) cos(q2)) cos(q3) , 0 ,

-170 %1 cos(q3)

- 170 (-cos(q1) sin(q2) - sin(q1) cos(q2)) sin(q3)

- 250 cos(q1) cos(q2) + 250 sin(q1) sin(q2)]

[(sin(q1) cos(q2) + cos(q1) sin(q2)) cos(q3) + %1 sin(q3) ,

-(sin(q1) cos(q2) + cos(q1) sin(q2)) sin(q3) + %1 cos(q3) , 0 ,

-170 (sin(q1) cos(q2) + cos(q1) sin(q2)) cos(q3) - 170 %1 sin(q3)

- 250 sin(q1) cos(q2) - 250 cos(q1) sin(q2)]

[0 , 0 , 1 , 520 + d4]

[0 , 0 , 0 , 1]

%1 := cos(q1) cos(q2) - sin(q1) sin(q2)

Положение схвата :
> p[x]=-170*(cos(q1)*cos(q2)-sin(q1)*sin(q2))*cos(q3)-170*(-cos(q1)*sin(q2)-sin(q1)*cos(q2))*sin(q3)-250*cos(q1)*cos(q2)+250*sin(q1)*sin(q2):
> p[y]=-170*(sin(q1)*cos(q2)+cos(q1)*sin(q2))*cos(q3)-170*(cos(q1)*cos(q2)-sin(q1)*sin(q2))*sin(q3)-250*sin(q1)*cos(q2)-250*cos(q1)*sin(q2):
> p[z]=520+d4:
Ориентация схвата :
> x[4]=((cos(q1)*cos(q2)-sin(q1)*sin(q2))*cos(q3)+(-cos(q1)*sin(q2)-sin(q1)*cos(q2))*sin(q3),(sin(q1)*cos(q2)+cos(q1)*sin(q2))*cos(q3)+(cos(q1)*cos(q2)-sin(q1)*sin(q2))*sin(q3),0)^T:
> y[4]=(-(cos(q1)*cos(q2)-sin(q1)*sin(q2))*sin(q3)+(-cos(q1)*sin(q2)-sin(q1)*cos(q2))*cos(q3),-(sin(q1)*cos(q2)+cos(q1)*sin(q2))*sin(q3)+(cos(q1)*cos(q2)-sin(q1)*sin(q2))*cos(q3),0)^T:
> z[4]=(0,0,1)^T:

Матрица вращений :
> R:=table():
> R:=submatrix(T[4],1..3,1..3);
Error, non-algebraic terms in a power must be of the same type
Error, non-algebraic terms in a power must be of the same type
Error, non-algebraic terms in a power must be of the same type

R :=

[%1 cos(q3) + (-cos(q1) sin(q2) - sin(q1) cos(q2)) sin(q3) ,

-%1 sin(q3) + (-cos(q1) sin(q2) - sin(q1) cos(q2)) cos(q3) , 0]

[(sin(q1) cos(q2) + cos(q1) sin(q2)) cos(q3) + %1 sin(q3) ,

-(sin(q1) cos(q2) + cos(q1) sin(q2)) sin(q3) + %1 cos(q3) , 0]

[0 , 0 , 1]

%1 := cos(q1) cos(q2) - sin(q1) sin(q2)

Проверка (вытянутая рука):
> A_:=table():q:=array([0,0,0,0]):d:=array([0,0,0,50]):
> for i from 1 to 4 do
> A_[i]:=array([[cos(q[i]),-cos(alpha[i])*sin(q[i]),sin(alpha[i])*sin(q[i]),a[i]*cos(q[i])],[sin(q[i]),cos(alpha[i])*cos(q[i]),-sin(alpha[i])*cos(q[i]),a[i]*sin(q[i])],[0,sin(alpha[i]),cos(alpha[i]),d[i]],[0,0,0,1]]) od;

[1 0 0 0]
[ ]
[0 1 0 0]
A_[1] := [ ]
[0 0 1 0]
[ ]
[0 0 0 1]


[1 0 0 -250]
[ ]
[0 1 0 0]
A_[2] := [ ]
[0 0 1 0]
[ ]
[0 0 0 1]


[1 0 0 -170]
[ ]
[0 1 0 0]
A_[3] := [ ]
[0 0 1 0]
[ ]
[0 0 0 1]


[1 0 0 0]
[ ]
[0 1 0 0]
A_[4] := [ ]
[0 0 1 50]
[ ]
[0 0 0 1]

> T_:=table():T_[2]:=multiply(A_[1],A_[2]);T_[3]:=multiply(A_[1],A_[2],A_[3]);T_[4]:=multiply(A_[1],A_[2],A_[3],A_[4]);

[1 0 0 -250]
[ ]
[0 1 0 0]
T_[2] := [ ]
[0 0 1 0]
[ ]
[0 0 0 1]


[1 0 0 -420]
[ ]
[0 1 0 0]
T_[3] := [ ]
[0 0 1 0]
[ ]
[0 0 0 1]


[1 0 0 -420]
[ ]
[0 1 0 0]
T_[4] := [ ]
[0 0 1 50]
[ ]
[0 0 0 1]

2. Обратная позиционная задача.
Пусть известна матрица Txyz :
>
> T[xyz]:=array([[x[x],y[x],z[x],p[x]],[x[y],y[y],z[y],p[y]],[x[z],y[z],z[z],p[z]],[0,0,0,1]]);

[x[x] y[x] z[x] p[x]]
[ ]
[x[y] y[y] z[y] p[y]]
T[xyz] := [ ]
[x[z] y[z] z[z] p[z]]
[ ]
[ 0 0 0 1 ]

> print(T[xyz]=A[1]*A[2]*A[3]*A[4]);

T[xyz] = A[1] A[2] A[3] A[4]

> multiply(A[1],A[2],A[3],A[4]);

[%1 cos(q3) + (-cos(q1) sin(q2) - sin(q1) cos(q2)) sin(q3) ,

-%1 sin(q3) + (-cos(q1) sin(q2) - sin(q1) cos(q2)) cos(q3) , 0 ,

-170 %1 cos(q3)

- 170 (-cos(q1) sin(q2) - sin(q1) cos(q2)) sin(q3)

- 250 cos(q1) cos(q2) + 250 sin(q1) sin(q2)]

[(sin(q1) cos(q2) + cos(q1) sin(q2)) cos(q3) + %1 sin(q3) ,

-(sin(q1) cos(q2) + cos(q1) sin(q2)) sin(q3) + %1 cos(q3) , 0 ,

-170 (sin(q1) cos(q2) + cos(q1) sin(q2)) cos(q3) - 170 %1 sin(q3)

- 250 sin(q1) cos(q2) - 250 cos(q1) sin(q2)]

[0 , 0 , 1 , 520 + d4]

[0 , 0 , 0 , 1]

%1 := cos(q1) cos(q2) - sin(q1) sin(q2)

> p[z]=(520+d4):
Откуда:
> d4=p[z]-520:
> print(A_inv[2]*A_inv[1]*T[xyz]=A[3]*A[4]);

A_inv[2] A_inv[1] T[xyz] = A[3] A[4]

> A_inv[1]:=inverse(A[1]);

[ cos(q1) sin(q1) ]
[ ------- ------- 0 0]
[ %1 %1 ]
[ ]
[ sin(q1) cos(q1) ]
A_inv[1] := [- ------- ------- 0 0]
[ %1 %1 ]
[ ]
[ 0 0 1 -520]
[ ]
[ 0 0 0 1]

2 2
%1 := cos(q1) + sin(q1)

> A_inv[2]:=inverse(A[2]);

[ cos(q2) sin(q2) ]
[ ------- ------- 0 250]
[ %1 %1 ]
[ ]
[ sin(q2) cos(q2) ]
A_inv[2] := [- ------- ------- 0 0]
[ %1 %1 ]
[ ]
[ 0 0 1 0]
[ ]
[ 0 0 0 1]

2 2
%1 := cos(q2) + sin(q2)

> multiply(A_inv[2],A_inv[1],T[xyz]);

[%4 x[x] + %3 x[y] , %4 y[x] + %3 y[y] , %4 z[x] + %3 z[y] ,

%4 p[x] + %3 p[y] + 250]

[%5 x[x] + %4 x[y] , %5 y[x] + %4 y[y] , %5 z[x] + %4 z[y] ,

%5 p[x] + %4 p[y]]

[x[z] , y[z] , z[z] , p[z] - 520]

[0 , 0 , 0 , 1]

2 2
%1 := cos(q1) + sin(q1)

2 2
%2 := cos(q2) + sin(q2)

cos(q2) sin(q1) sin(q2) cos(q1)
%3 := --------------- + ---------------
%2 %1 %2 %1

cos(q2) cos(q1) sin(q2) sin(q1)
%4 := --------------- - ---------------
%2 %1 %2 %1

sin(q2) cos(q1) cos(q2) sin(q1)
%5 := - --------------- - ---------------
%2 %1 %2 %1

> simplify(MATRIX([[(cos(q2)/(cos(q2)^2+sin(q2)^2)*cos(q1)/(cos(q1)^2+sin(q1)^2)-sin(q2)/(cos(q2)^2+sin(q2)^2)*sin(q1)/(cos(q1)^2+sin(q1)^2))*x[x]+(cos(q2)/(cos(q2)^2+sin(q2)^2)*sin(q1)/(cos(q1)^2+sin(q1)^2)+sin(q2)/(cos(q2)^2+sin(q2)^2)*cos(q1)/(cos(q1)^2+sin(q1)^2))*x[y], (cos(q2)/(cos(q2)^2+sin(q2)^2)*cos(q1)/(cos(q1)^2+sin(q1)^2)-sin(q2)/(cos(q2)^2+sin(q2)^2)*sin(q1)/(cos(q1)^2+sin(q1)^2))*y[x]+(cos(q2)/(cos(q2)^2+sin(q2)^2)*sin(q1)/(cos(q1)^2+sin(q1)^2)+sin(q2)/(cos(q2)^2+sin(q2)^2)*cos(q1)/(cos(q1)^2+sin(q1)^2))*y[y], (cos(q2)/(cos(q2)^2+sin(q2)^2)*cos(q1)/(cos(q1)^2+sin(q1)^2)-sin(q2)/(cos(q2)^2+sin(q2)^2)*sin(q1)/(cos(q1)^2+sin(q1)^2))*z[x]+(cos(q2)/(cos(q2)^2+sin(q2)^2)*sin(q1)/(cos(q1)^2+sin(q1)^2)+sin(q2)/(cos(q2)^2+sin(q2)^2)*cos(q1)/(cos(q1)^2+sin(q1)^2))*z[y], (cos(q2)/(cos(q2)^2+sin(q2)^2)*cos(q1)/(cos(q1)^2+sin(q1)^2)-sin(q2)/(cos(q2)^2+sin(q2)^2)*sin(q1)/(cos(q1)^2+sin(q1)^2))*p[x]+(cos(q2)/(cos(q2)^2+sin(q2)^2)*sin(q1)/(cos(q1)^2+sin(q1)^2)+sin(q2)/(cos(q2)^2+sin(q2)^2)*cos(q1)/(cos(q1)^2+sin(q1)^2))*p[y]+250], [(-sin(q2)/(cos(q2)^2+sin(q2)^2)*cos(q1)/(cos(q1)^2+sin(q1)^2)-cos(q2)/(cos(q2)^2+sin(q2)^2)*sin(q1)/(cos(q1)^2+sin(q1)^2))*x[x]+(cos(q2)/(cos(q2)^2+sin(q2)^2)*cos(q1)/(cos(q1)^2+sin(q1)^2)-sin(q2)/(cos(q2)^2+sin(q2)^2)*sin(q1)/(cos(q1)^2+sin(q1)^2))*x[y], (-sin(q2)/(cos(q2)^2+sin(q2)^2)*cos(q1)/(cos(q1)^2+sin(q1)^2)-cos(q2)/(cos(q2)^2+sin(q2)^2)*sin(q1)/(cos(q1)^2+sin(q1)^2))*y[x]+(cos(q2)/(cos(q2)^2+sin(q2)^2)*cos(q1)/(cos(q1)^2+sin(q1)^2)-sin(q2)/(cos(q2)^2+sin(q2)^2)*sin(q1)/(cos(q1)^2+sin(q1)^2))*y[y], (-sin(q2)/(cos(q2)^2+sin(q2)^2)*cos(q1)/(cos(q1)^2+sin(q1)^2)-cos(q2)/(cos(q2)^2+sin(q2)^2)*sin(q1)/(cos(q1)^2+sin(q1)^2))*z[x]+(cos(q2)/(cos(q2)^2+sin(q2)^2)*cos(q1)/(cos(q1)^2+sin(q1)^2)-sin(q2)/(cos(q2)^2+sin(q2)^2)*sin(q1)/(cos(q1)^2+sin(q1)^2))*z[y], (-sin(q2)/(cos(q2)^2+sin(q2)^2)*cos(q1)/(cos(q1)^2+sin(q1)^2)-cos(q2)/(cos(q2)^2+sin(q2)^2)*sin(q1)/(cos(q1)^2+sin(q1)^2))*p[x]+(cos(q2)/(cos(q2)^2+sin(q2)^2)*cos(q1)/(cos(q1)^2+sin(q1)^2)-sin(q2)/(cos(q2)^2+sin(q2)^2)*sin(q1)/(cos(q1)^2+sin(q1)^2))*p[y]], [x[z], y[z], z[z], -520+p[z]], [0, 0, 0, 1]]));

[x[x] cos(q2) cos(q1) - x[x] sin(q2) sin(q1)

+ x[y] cos(q2) sin(q1) + x[y] sin(q2) cos(q1) ,

y[x] cos(q2) cos(q1) - y[x] sin(q2) sin(q1)

+ y[y] cos(q2) sin(q1) + y[y] sin(q2) cos(q1) ,

z[x] cos(q2) cos(q1) - z[x] sin(q2) sin(q1)

+ z[y] cos(q2) sin(q1) + z[y] sin(q2) cos(q1) ,

p[x] cos(q2) cos(q1) - p[x] sin(q2) sin(q1)

+ p[y] cos(q2) sin(q1) + p[y] sin(q2) cos(q1) + 250]

[-x[x] cos(q2) sin(q1) - x[x] sin(q2) cos(q1)

+ x[y] cos(q2) cos(q1) - x[y] sin(q2) sin(q1) ,

-y[x] cos(q2) sin(q1) - y[x] sin(q2) cos(q1)

+ y[y] cos(q2) cos(q1) - y[y] sin(q2) sin(q1) ,

-z[x] cos(q2) sin(q1) - z[x] sin(q2) cos(q1)

+ z[y] cos(q2) cos(q1) - z[y] sin(q2) sin(q1) ,

-p[x] cos(q2) sin(q1) - p[x] sin(q2) cos(q1)

+ p[y] cos(q2) cos(q1) - p[y] sin(q2) sin(q1)]

[x[z] , y[z] , z[z] , p[z] - 520]

[0 , 0 , 0 , 1]

> multiply(A[3],A[4]);

[cos(q3) -sin(q3) 0 -170 cos(q3)]
[ ]
[sin(q3) cos(q3) 0 -170 sin(q3)]
[ ]
[ 0 0 1 d4 ]
[ ]
[ 0 0 0 1 ]

> solve({z[x]*cos(q2)*cos(q1)-z[x]*sin(q2)*sin(q1)+z[y]*cos(q2)*sin(q1)+z[y]*sin(q2)*cos(q1)=0} , {q1,q2});

z[x] + z[y] tan(q2)
{q1 = arctan(-------------------), q2 = q2}
z[x] tan(q2) - z[y]

> solve({-z[x]*sin(q2)*cos(q1)-z[x]*cos(q2)*sin(q1)+z[y]*cos(q2)*cos(q1)-z[y]*sin(q2)*sin(q1)} , {q1,q2});

z[x] tan(q2) - z[y]
{q2 = q2, q1 = -arctan(-------------------)}
z[x] + z[y] tan(q2)

> solve(cos(q3)=cos(arctan((z[x]+z[y])/(z[x]-z[y])))*x[x]+sin(arctan((z[x]+z[y])/(z[x]-z[y])))*x[y],q3);


1/2
2 (x[x] z[x] - x[x] z[y] + x[y] z[x] + x[y] z[y])
arccos(1/2 ----------------------------------------------------),
2 2 1/2
(z[x] + z[y] )

Pi -

1/2
2 (x[x] z[x] - x[x] z[y] + x[y] z[x] + x[y] z[y])
arccos(1/2 ----------------------------------------------------)
2 2 1/2
(z[x] + z[y] )

3. Прямая задача по скорости.
> z:=table(): q:=array([q1,q2,q3,0]):
> z[0]:=submatrix(T[0],1..3,3..3);
> z[1]:=submatrix(T[1],1..3,3..3);
> z[2]:=submatrix(T[2],1..3,3..3);
> z[3]:=submatrix(T[3],1..3,3..3);

[0]
[ ]
z[0] := [0]
[ ]
[1]


[0]
[ ]
z[1] := [0]
[ ]
[1]


[0]
[ ]
z[2] := [0]
[ ]
[1]


[0]
[ ]
z[3] := [0]
[ ]
[1]

> p:=table(): J:=table():
> p[0,4]:=submatrix(T[4],1..3,4..4);

p[0, 4] :=

[-170 (cos(q1) cos(q2) - sin(q1) sin(q2)) cos(q3)

- 170 (-cos(q1) sin(q2) - sin(q1) cos(q2)) sin(q3)

- 250 cos(q1) cos(q2) + 250 sin(q1) sin(q2)]

[-170 (sin(q1) cos(q2) + cos(q1) sin(q2)) cos(q3)

- 170 (cos(q1) cos(q2) - sin(q1) sin(q2)) sin(q3)

- 250 sin(q1) cos(q2) - 250 cos(q1) sin(q2)]

[520 + d4]

> p4:=p[0,4]: p3:= submatrix (T[3],1..3,4..4); p2:=submatrix(T[2],1..3,4..4); p1:=submatrix(T[1],1..3,4..4);

p3 :=

[-170 (cos(q1) cos(q2) - sin(q1) sin(q2)) cos(q3)

- 170 (-cos(q1) sin(q2) - sin(q1) cos(q2)) sin(q3)

- 250 cos(q1) cos(q2) + 250 sin(q1) sin(q2)]

[-170 (sin(q1) cos(q2) + cos(q1) sin(q2)) cos(q3)

- 170 (cos(q1) cos(q2) - sin(q1) sin(q2)) sin(q3)

- 250 sin(q1) cos(q2) - 250 cos(q1) sin(q2)]

[520]


[-250 cos(q1) cos(q2) + 250 sin(q1) sin(q2)]
[ ]
p2 := [-250 sin(q1) cos(q2) - 250 cos(q1) sin(q2)]
[ ]
[ 520 ]


[ 0]
[ ]
p1 := [ 0]
[ ]
[520]

> p[1,4]:=evalm(p4-p1);
> p[3,4]:=evalm(p4-p3);
> p[2,4]:=evalm(p4-p2);

p[1, 4] :=

[-170 (cos(q1) cos(q2) - sin(q1) sin(q2)) cos(q3)

- 170 (-cos(q1) sin(q2) - sin(q1) cos(q2)) sin(q3)

- 250 cos(q1) cos(q2) + 250 sin(q1) sin(q2)]

[-170 (sin(q1) cos(q2) + cos(q1) sin(q2)) cos(q3)

- 170 (cos(q1) cos(q2) - sin(q1) sin(q2)) sin(q3)

- 250 sin(q1) cos(q2) - 250 cos(q1) sin(q2)]

[d4]


[0 ]
[ ]
p[3, 4] := [0 ]
[ ]
[d4]


p[2, 4] :=

[-170 (cos(q1) cos(q2) - sin(q1) sin(q2)) cos(q3)

- 170 (-cos(q1) sin(q2) - sin(q1) cos(q2)) sin(q3)]

[-170 (sin(q1) cos(q2) + cos(q1) sin(q2)) cos(q3)

- 170 (cos(q1) cos(q2) - sin(q1) sin(q2)) sin(q3)]

[d4]

> J[0]:=array(1..6,1..1,[[0],[0],[1],[170*(sin(q1)*cos(q2)+cos(q1)*sin(q2))*cos(q3)+170*(cos(q1)*cos(q2)-sin(q1)*sin(q2))*sin(q3)+250*sin(q1)*cos(q2)+250*cos(q1)*sin(q2)],[-170*(cos(q1)*cos(q2)-sin(q1)*sin(q2))*cos(q3)-170*(-cos(q1)*sin(q2)-sin(q1)*cos(q2))*sin(q3)-250*cos(q1)*cos(q2)+250*sin(q1)*sin(q2)],[0]]);
> J[1]:=array(1..6,1..1,[[0],[0],[1],[170*(sin(q1)*cos(q2)+cos(q1)*sin(q2))*cos(q3)+170*(cos(q1)*cos(q2)-sin(q1)*sin(q2))*sin(q3)+250*sin(q1)*cos(q2)+250*cos(q1)*sin(q2)],[-170*(cos(q1)*cos(q2)-sin(q1)*sin(q2))*cos(q3)-170*(-cos(q1)*sin(q2)-sin(q1)*cos(q2))*sin(q3)-250*cos(q1)*cos(q2)+250*sin(q1)*sin(q2)],[0]]);
> J[2]:=array(1..6,1..1,[[0],[0],[1],[170*(sin(q1)*cos(q2)+cos(q1)*sin(q2))*cos(q3)+170*(cos(q1)*cos(q2)-sin(q1)*sin(q2))*sin(q3)],[-170*(cos(q1)*cos(q2)-sin(q1)*sin(q2))*cos(q3)-170*(-cos(q1)*sin(q2)-sin(q1)*cos(q2))*sin(q3)],[0]]);
> J[3]:=array(1..6,1..1,[[0],[0],[0],[0],[0],[1]]);

J[0] :=

[0]

[0]

[1]

[170 (sin(q1) cos(q2) + cos(q1) sin(q2)) cos(q3)

+ 170 (cos(q1) cos(q2) - sin(q1) sin(q2)) sin(q3)

+ 250 sin(q1) cos(q2) + 250 cos(q1) sin(q2)]

[-170 (cos(q1) cos(q2) - sin(q1) sin(q2)) cos(q3)

- 170 (-cos(q1) sin(q2) - sin(q1) cos(q2)) sin(q3)

- 250 cos(q1) cos(q2) + 250 sin(q1) sin(q2)]

[0]


J[1] :=

[0]

[0]

[1]

[170 (sin(q1) cos(q2) + cos(q1) sin(q2)) cos(q3)

+ 170 (cos(q1) cos(q2) - sin(q1) sin(q2)) sin(q3)

+ 250 sin(q1) cos(q2) + 250 cos(q1) sin(q2)]

[-170 (cos(q1) cos(q2) - sin(q1) sin(q2)) cos(q3)

- 170 (-cos(q1) sin(q2) - sin(q1) cos(q2)) sin(q3)

- 250 cos(q1) cos(q2) + 250 sin(q1) sin(q2)]

[0]


J[2] :=

[0]

[0]

[1]

[170 (sin(q1) cos(q2) + cos(q1) sin(q2)) cos(q3)

+ 170 (cos(q1) cos(q2) - sin(q1) sin(q2)) sin(q3)]

[-170 (cos(q1) cos(q2) - sin(q1) sin(q2)) cos(q3)

- 170 (-cos(q1) sin(q2) - sin(q1) cos(q2)) sin(q3)]

[0]


[0]
[ ]
[0]
[ ]
[0]
J[3] := [ ]
[0]
[ ]
[0]
[ ]
[1]

> Jac:=array(1..6,1..4,[]):
> copyinto(J[0],Jac,1,1):copyinto(J[1],Jac,1,2):copyinto(J[2],Jac,1,3):copyinto(J[3],Jac,1,4);

[0 , 0 , 0 , 0]

[0 , 0 , 0 , 0]

[1 , 1 , 1 , 0]

[170 %2 + 170 %1 sin(q3) + 250 sin(q1) cos(q2)

+ 250 cos(q1) sin(q2) , 170 %2 + 170 %1 sin(q3)

+ 250 sin(q1) cos(q2) + 250 cos(q1) sin(q2) ,

170 %2 + 170 %1 sin(q3) , 0]

[-170 %1 cos(q3) - 170 %3 - 250 cos(q1) cos(q2)

+ 250 sin(q1) sin(q2) , -170 %1 cos(q3) - 170 %3

- 250 cos(q1) cos(q2) + 250 sin(q1) sin(q2) ,

-170 %1 cos(q3) - 170 %3 , 0]

[0 , 0 , 0 , 1]

%1 := cos(q1) cos(q2) - sin(q1) sin(q2)

%2 := (sin(q1) cos(q2) + cos(q1) sin(q2)) cos(q3)

%3 := (-cos(q1) sin(q2) - sin(q1) cos(q2)) sin(q3)

> V:=array(1..6,1..1,[[omega(x)],[omega(y)],[omega(z)],[v(x)],[v(y)],[v(z)]]);

[omega(x)]
[ ]
[omega(y)]
[ ]
[omega(z)]
V := [ ]
[ v(x) ]
[ ]
[ v(y) ]
[ ]
[ v(z) ]

> dq:=array(1..4,1..1,[]): -Mатрица производных обобщенных координат.
> print(dq);

[dq[1, 1]]
[ ]
[dq[2, 1]]
[ ]
[dq[3, 1]]
[ ]
[dq[4, 1]]

> V:=multiply(Jac,dq);

V :=

[0]

[0]

[dq[1, 1] + dq[2, 1] + dq[3, 1]]

[(170 %2 + 170 %1 sin(q3) + 250 sin(q1) cos(q2)

+ 250 cos(q1) sin(q2)) dq[1, 1] + (170 %2 + 170 %1 sin(q3)

+ 250 sin(q1) cos(q2) + 250 cos(q1) sin(q2)) dq[2, 1]

+ (170 %2 + 170 %1 sin(q3)) dq[3, 1]]

[(-170 %1 cos(q3) - 170 %3 - 250 cos(q1) cos(q2)

+ 250 sin(q1) sin(q2)) dq[1, 1] + (-170 %1 cos(q3) - 170 %3

- 250 cos(q1) cos(q2) + 250 sin(q1) sin(q2)) dq[2, 1]

+ (-170 %1 cos(q3) - 170 %3) dq[3, 1]]

[dq[4, 1]]

%1 := cos(q1) cos(q2) - sin(q1) sin(q2)

%2 := (sin(q1) cos(q2) + cos(q1) sin(q2)) cos(q3)

%3 := (-cos(q1) sin(q2) - sin(q1) cos(q2)) sin(q3)

>
4.Обратная скоростная задача.
Пусть известна матрица V:
> V:=array(1..3,1..1,[[omega(z)],[v(x)],[v(y)]]);

[omega(z)]
[ ]
V := [ v(x) ]
[ ]
[ v(y) ]

> Jac:=submatrix(Jac,[3,4,5],[1,2,3]);

Jac :=

[1 , 1 , 1]

[170 %2 + 170 %1 sin(q3) + 250 sin(q1) cos(q2)

+ 250 cos(q1) sin(q2) , 170 %2 + 170 %1 sin(q3)

+ 250 sin(q1) cos(q2) + 250 cos(q1) sin(q2) ,

170 %2 + 170 %1 sin(q3)]

[-170 %1 cos(q3) - 170 %3 - 250 cos(q1) cos(q2)

+ 250 sin(q1) sin(q2) , -170 %1 cos(q3) - 170 %3

- 250 cos(q1) cos(q2) + 250 sin(q1) sin(q2) ,

-170 %1 cos(q3) - 170 %3]

%1 := cos(q1) cos(q2) - sin(q1) sin(q2)

%2 := (sin(q1) cos(q2) + cos(q1) sin(q2)) cos(q3)

%3 := (-cos(q1) sin(q2) - sin(q1) cos(q2)) sin(q3)

> det(Jac);

0

> Jac_trans:=transpose(Jac);

Jac_trans :=

[1 , 170 %2 + 170 %1 sin(q3) + 250 sin(q1) cos(q2)

+ 250 cos(q1) sin(q2) , -170 %1 cos(q3) - 170 %3

- 250 cos(q1) cos(q2) + 250 sin(q1) sin(q2)]

[1 , 170 %2 + 170 %1 sin(q3) + 250 sin(q1) cos(q2)

+ 250 cos(q1) sin(q2) , -170 %1 cos(q3) - 170 %3

- 250 cos(q1) cos(q2) + 250 sin(q1) sin(q2)]

[1 , 170 %2 + 170 %1 sin(q3) , -170 %1 cos(q3) - 170 %3]

%1 := cos(q1) cos(q2) - sin(q1) sin(q2)

%2 := (sin(q1) cos(q2) + cos(q1) sin(q2)) cos(q3)

%3 := (-cos(q1) sin(q2) - sin(q1) cos(q2)) sin(q3)

> dq_:=multiply(Jac_trans,V);

dq_ :=

[omega(z) + (170 %3 + 170 %2 sin(q3) + 250 sin(q1) cos(q2)

+ 250 cos(q1) sin(q2)) v(x) + (-170 %2 cos(q3) - 170 %1

- 250 cos(q1) cos(q2) + 250 sin(q1) sin(q2)) v(y)]

[omega(z) + (170 %3 + 170 %2 sin(q3) + 250 sin(q1) cos(q2)

+ 250 cos(q1) sin(q2)) v(x) + (-170 %2 cos(q3) - 170 %1

- 250 cos(q1) cos(q2) + 250 sin(q1) sin(q2)) v(y)]

[omega(z) + (170 %3 + 170 %2 sin(q3)) v(x)

+ (-170 %2 cos(q3) - 170 %1) v(y)]

%1 := (-cos(q1) sin(q2) - sin(q1) cos(q2)) sin(q3)

%2 := cos(q1) cos(q2) - sin(q1) sin(q2)

%3 := (sin(q1) cos(q2) + cos(q1) sin(q2)) cos(q3)

> dq1:=(omega(z)+(170*(sin(q1)*cos(q2)+cos(q1)*sin(q2))*cos(q3)+170*(cos(q1)*cos(q2)-sin(q1)*sin(q2))*sin(q3)+250*sin(q1)*cos(q2)+250*cos(q1)*sin(q2))*v(x)+(-170*(cos(q1)*cos(q2)-sin(q1)*sin(q2))*cos(q3)-170*(-cos(q1)*sin(q2)-sin(q1)*cos(q2))*sin(q3)-250*cos(q1)*cos(q2)+250*sin(q1)*sin(q2))*v(y));

dq1 := omega(z) + (170 (sin(q1) cos(q2) + cos(q1) sin(q2)) cos(q3)

+ 170 (cos(q1) cos(q2) - sin(q1) sin(q2)) sin(q3)

+ 250 sin(q1) cos(q2) + 250 cos(q1) sin(q2)) v(x) + (

-170 (cos(q1) cos(q2) - sin(q1) sin(q2)) cos(q3)

- 170 (-cos(q1) sin(q2) - sin(q1) cos(q2)) sin(q3)

- 250 cos(q1) cos(q2) + 250 sin(q1) sin(q2)) v(y)

> dq2:=(omega(z)+(170*(sin(q1)*cos(q2)+cos(q1)*sin(q2))*cos(q3)+170*(cos(q1)*cos(q2)-sin(q1)*sin(q2))*sin(q3)+250*sin(q1)*cos(q2)+250*cos(q1)*sin(q2))*v(x)+(-170*(cos(q1)*cos(q2)-sin(q1)*sin(q2))*cos(q3)-170*(-cos(q1)*sin(q2)-sin(q1)*cos(q2))*sin(q3)-250*cos(q1)*cos(q2)+250*sin(q1)*sin(q2))*v(y));

dq2 := omega(z) + (170 (sin(q1) cos(q2) + cos(q1) sin(q2)) cos(q3)

+ 170 (cos(q1) cos(q2) - sin(q1) sin(q2)) sin(q3)

+ 250 sin(q1) cos(q2) + 250 cos(q1) sin(q2)) v(x) + (

-170 (cos(q1) cos(q2) - sin(q1) sin(q2)) cos(q3)

- 170 (-cos(q1) sin(q2) - sin(q1) cos(q2)) sin(q3)

- 250 cos(q1) cos(q2) + 250 sin(q1) sin(q2)) v(y)

> dq3:=(omega(z)+(170*(sin(q1)*cos(q2)+cos(q1)*sin(q2))*cos(q3)+170*(cos(q1)*cos(q2)-sin(q1)*sin(q2))*sin(q3))*v(x)+(-170*(cos(q1)*cos(q2)-sin(q1)*sin(q2))*cos(q3)-170*(-cos(q1)*sin(q2)-sin(q1)*cos(q2))*sin(q3))*v(y));

dq3 := omega(z) + (170 (sin(q1) cos(q2) + cos(q1) sin(q2)) cos(q3)

+ 170 (cos(q1) cos(q2) - sin(q1) sin(q2)) sin(q3)) v(x) + (

-170 (cos(q1) cos(q2) - sin(q1) sin(q2)) cos(q3)

- 170 (-cos(q1) sin(q2) - sin(q1) cos(q2)) sin(q3)) v(y)

> dd4:=v(z);

dd4 := v(z)

>
Рассмотрим вырожденные состояния :
> q1:=0: q2:=0: q3:=0: -вытянутая рука.
> dq:=array(1..4,1..1,[[dq1],[dq2],[dq3],[dd4]]);

[omega(z) - 420 v(y)]
[ ]
[omega(z) - 420 v(y)]
dq := [ ]
[omega(z) - 170 v(y)]
[ ]
[ v(z) ]

> solve({q_1+q_2+q_3=Pi/2,q_1+q_2=Pi/2} , {q_1,q_2,q_3});

{q_2 = q_2, q_1 = -q_2 + 1/2 Pi, q_3 = 0}

> q1:=-q2+Pi/2: q2:=q2: q3:=0:
> dq:=array(1..4,1..1,[[dq1],[dq2],[dq3],[dd4]]);

[omega(z) + 420 v(x)]
[ ]
[omega(z) + 420 v(x)]
dq := [ ]
[omega(z) + 170 v(x)]
[ ]
[ v(z) ]