Robótica unidad 4

>> syms q

>> syms l4

>> syms d3

>>

>> A=[cos(q)*(l4+d3) 0 sin(q) 0;0 1 0 0;-sin(q)*(l4+d3) 0 cos(q) 0;0 0 0 cos(q)]

A=

[ cos(q)*(d3 + l4), 0, sin(q), 0]

[ 0, 1, 0, 0]

[ -sin(q)*(d3 + l4), 0, cos(q), 0]

[ 0, 0, 0, cos(q)]

>> D=det(A)

D=

cos(q)*(d3*sin(q)^2 + l4*sin(q)^2 + d3*cos(q)^2 + l4*cos(q)^2)

>> q=(90*pi/180)

q=

1.5708

>> D=cos(q)*(d3*sin(q)^2 + l4*sin(q)^2 + d3*cos(q)^2 + l4*cos(q)^2)

D=

(907548233286809381061144841050568787646261303381729908994461087*d3)/14821387422
376473014217086081112052205218558037201992197050570753012880593911808 +
(907548233286809381061144841050568787646261303381729908994461087*l4)/14821387422
376473014217086081112052205218558037201992197050570753012880593911808

>> q=(-90*pi/180)

q=

-1.5708

>> D=cos(q)*(d3*sin(q)^2 + l4*sin(q)^2 + d3*cos(q)^2 + l4*cos(q)^2)

D=

(907548233286809381061144841050568787646261303381729908994461087*d3)/14821387422
376473014217086081112052205218558037201992197050570753012880593911808 +
(907548233286809381061144841050568787646261303381729908994461087*l4)/14821387422
376473014217086081112052205218558037201992197050570753012880593911808

>> j=[0 0 -1 0;-325 0 0 0;0 1 0 0;0 0 0 -1;0 0 0 0;1 0 0 0]

j=

0 0 -1 0

-325 0 0 0

0 1 0 0

0 0 0 -1

0 0 0 0
 1 0 0 0

>> ji=j'

ji =

0 -325 0 0 0 1

0 0 1 0 0 0

-1 0 0 0 0 0

0 0 0 -1 0 0

>> ji*j

ans =

105626 0 0 0

0 1 0 0

0 0 1 0

0 0 0 1

>> j=[105626 0 0 0 1 0 0 0;0 1 0 0 0 1 0 0;0 0 1 0 0 0 1 0;0 0 0 1 0 0 0 1]

j=

105626 0 0 0 1 0 0 0

0 1 0 0 0 1 0 0

0 0 1 0 0 0 1 0

0 0 0 1 0 0 0 1
>> R=rref(j)

R=

1.0000 0 0 0 0.0000 0 0 0

0 1.0000 0 0 0 1.0000 0 0

0 0 1.0000 0 0 0 1.0000 0

0 0 0 1.0000 0 0 0 1.0000

>> jt=[1/105626 0 0 0;0 1 0 0;0 0 1 0;0 0 0 1]

jt =

0.0000 0 0 0

0 1.0000 0 0

0 0 1.0000 0

0 0 0 1.0000

>> jt2=jt*ji

jt2 =

0 -0.0031 0 0 0 0.0000

0 0 1.0000 0 0 0

-1.0000 0 0 0 0 0

0 0 0 -1.0000 0 0

>> v=[-1;-510.51;1;1.25;-1.5;0.85]
v=

-1.0000

-510.5100

1.0000

1.2500

-1.5000

0.8500

>> q=jt2*v

q=

1.5708

1.0000

1.0000

-1.2500

>>