Actividad 13

Robótica

Calcular la matriz de Coriolis en base a la matriz de masas siguiente

MATRIZ DE CORIOLIS
C = (sym 3x3 matrix)

[
[ -q1p*sin(q1 + q2 + q3) + q2p*q3*cos(q1 + q2) +
[
[
[
[ q1p*(sin(q2 + q3) + 2*sin(q1 + q2 + q3)) q2p*(L*q3*sin(q2)*sin(q1 + q2) - L*q3*cos(q2)
[- ---------------------------------------- + ---------------------------------------------
[ 2
[
[ / 2
[ q1p*(sin(q2 + q3) + 2*sin(q1 + q2 + q3)) q2p*\L*L1*q1 *s
[ - ---------------------------------------- + ---------------
[ 2
q2p*sin(q2 + q3) q3p*sin(q2 + q3)
---------------- + q2p*sin(q1 + q2 + q3) + ---------------- + q3p*sin(q1 + q2 + q3)
2 2

*cos(q1 + q2) - 2*q1*sin(q1) + 2*q2 + q3*cos(q1 + q2) + 2*cos(q1)) q3p*(L*sin(q1 + q2)*co

------------------------------------------------------------------ - ----------------------
2

\ / 2
in(q2)*cos(q3) + sin(q1 + q2)/ q3p*\- L*L1*q1 *sin(q3)*cos(q2) + q2*q3*sin(q1) + sin(q2 +
------------------------------ - ----------------------------------------------------------
2 2

q1p*(-L*q3*cos(q2)*cos(q1 + q2) + q3*cos(q1 + q2) +

- ----------------------------------------------------
2

s(q2) + sin(q2 + q3) - cos(q2) - 1)

----------------------------------- q1p*(-L*q3*s
2

\ / 2
q3)/ L*q1p*\L1*q1 *sin(q2)*cos(q3)
----- - -----------------------------
2

sin(q2 + q3) + 2*sin(q1 + q2 + q3)) q3p*(sin(q1 + q2) + sin(

----------------------------------- - q2p*(q1*sin(q1) - cos(q1)) - ------------------------
2

in(q2)*sin(q1 + q2) + L*q3*cos(q2)*cos(q1 + q2) + q1*sin(q1) - cos(q1))

 \ /2 2 \
- sin(q1 + q2)*cos(q2)/ q3*q3p*\L *q3*sin(q3)*cos(q2) - 2*L *cos(q2)*cos(q3) + cos(q1)/
------------------------ + ---------------------------------------------------------------
2

q2 + q3) - cos(q2) - 1) q1p*(-2*L*L1*q1*cos(q2)*cos(q3) + sin(q2 + q3) + 2*sin(q1 + q2 +

----------------------- - ----------------------------------------------------------------
2

2 2 / 2
L *q2p*q3 *sin(q2)*cos(q3) q1p*\L*L1*q1 *sin(q2
- -------------------------- - --------------------
2

/ 2 \
q1p*\2*L*L1*q1 *sin(q3)*cos(q2) - q2*q3*sin(q1)/ q2*q3p*cos(q
- ------------------------------------------------ + ------------
2 2

q3)) q2*q3*q3p*sin(q1) q2p*(sin(q1 + q2) + sin(q2 + q3) - cos(q2))]

----- - ----------------- + -------------------------------------------]
2 2 ]
]
\ ]
)*cos(q3) - L*sin(q1 + q2)*cos(q2) + 1/ q3*q3p*cos(q1) ]
--------------------------------------- + -------------- ]
2 2 ]
]
/ 2 2 \]
1) q2p*q3*\2*L *q3*sin(q3)*cos(q2) - 4*L *cos(q2)*cos(q3) + cos(q1)/ ]
-- - ----------------------------------------------------------------- ]
2 ]

PROGRAMA REALIZADO EN OCTAVE

clc all
syms q1 q2 q3 L1 L2 L C111 C112 C113 C121 C122 C123 C131 C132 C133 C211 C212 C213 C221
C222 C223 C231 C232 C233 C311 C312 C313 C321 C322 C323 C331 C332 C333 q1p q2p q3p
M=[2*cos(q1+q2+q3)+cos(q2+q3),
q3*L*cos(q2)*sin(q1+q2),q1*q1*L1*L*cos(q2)*cos(q3);q2*q2+q3*sin(q1+q2),2*q1*cos(q1),q3*q
3*L*cos(q2)*L*cos(q3);sin(q2)+cos(q2+q3),q1+L1*L2,q3*q2*cos(q1)]
C111=(1/2)*(diff(M(1,1),q1)+diff(M(1,1),q1)-diff(M(1,1),q1))
C112=(1/2)*(diff(M(1,1),q2)+diff(M(1,2),q1)-diff(M(1,2),q1))
C113=(1/2)*(diff(M(1,1),q3)+diff(M(1,3),q1)-diff(M(1,3),q1))
C121=(1/2)*(diff(M(1,2),q1)+diff(M(1,1),q2)-diff(M(2,1),q1))
C122=(1/2)*(diff(M(1,2),q2)+diff(M(1,2),q2)-diff(M(2,2),q1))
C123=(1/2)*(diff(M(1,2),q3)+diff(M(1,3),q2)-diff(M(2,3),q1))
C131=(1/2)*(diff(M(1,3),q1)+diff(M(1,1),q3)-diff(M(3,1),q1))
C132=(1/2)*(diff(M(1,3),q2)+diff(M(1,2),q3)-diff(M(3,2),q1))
C133=(1/2)*(diff(M(1,3),q3)+diff(M(1,3),q3)-diff(M(3,3),q1))
C211=(1/2)*(diff(M(2,1),q1)+diff(M(2,1),q1)-diff(M(1,1),q2))
C212=(1/2)*(diff(M(2,1),q2)+diff(M(2,2),q1)-diff(M(1,2),q2))
C213=(1/2)*(diff(M(2,1),q3)+diff(M(2,3),q1)-diff(M(1,3),q2))
C221=(1/2)*(diff(M(2,2),q1)+diff(M(2,1),q2)-diff(M(2,1),q2))
C222=(1/2)*(diff(M(2,2),q2)+diff(M(2,2),q2)-diff(M(2,2),q2))
C223=(1/2)*(diff(M(2,2),q3)+diff(M(2,3),q2)-diff(M(2,3),q2))
C231=(1/2)*(diff(M(2,3),q1)+diff(M(2,1),q3)-diff(M(3,1),q2))
C232=(1/2)*(diff(M(2,3),q2)+diff(M(2,2),q3)-diff(M(3,2),q2))
C233=(1/2)*(diff(M(2,3),q3)+diff(M(2,3),q3)-diff(M(3,3),q2))
C311=(1/2)*(diff(M(3,1),q1)+diff(M(3,1),q1)-diff(M(1,1),q3))
C312=(1/2)*(diff(M(3,1),q2)+diff(M(3,2),q1)-diff(M(1,2),q3))
C313=(1/2)*(diff(M(3,1),q3)+diff(M(3,3),q1)-diff(M(1,3),q3))
C321=(1/2)*(diff(M(3,2),q1)+diff(M(3,1),q2)-diff(M(2,1),q3))
C322=(1/2)*(diff(M(3,2),q2)+diff(M(3,2),q2)-diff(M(2,2),q3))
C323=(1/2)*(diff(M(3,2),q3)+diff(M(3,3),q2)-diff(M(2,3),q3))
C331=(1/2)*(diff(M(3,3),q1)+diff(M(3,1),q3)-diff(M(3,1),q3))
C332=(1/2)*(diff(M(3,3),q2)+diff(M(3,2),q3)-diff(M(3,2),q3))
C333=(1/2)*(diff(M(3,3),q3)+diff(M(3,3),q3)-diff(M(3,3),q3))
qp=[q1p;q2p;q3p]
B11=transpuesta([C111;C211;C311])*qp
B12=transpuesta([C121;C221;C321])*qp
B13=transpuesta([C131;C231;C331])*qp
B21=transpuesta([C112;C212;C312])*qp
B22=transpuesta([C122;C222;C322])*qp
B23=transpuesta([C132;C232;C332])*qp
B31=transpuesta([C113;C213;C313])*qp
B32=transpuesta([C123;C223;C323])*qp
B33=transpuesta([C133;C233;C333])*qp
%C=MATRIZ DE CORIOLIS
C=simplify([B11,B12,B13;B21,B22,B23;B31,B32,B33])

NOTA: ESTE PROGRAMA ESTA REALIZADO MEDIANTE LA FORMULA VISTA EN CLASE DE LOS
SIMBOLOS DE CHISTOFFEL QUE ES LA SIGUIENTE:

C ijk= (
1 ∂ mij ∂ mik ∂ m jk
+
2 ∂ qk ∂ q j
−
∂q i )
ES IMPORTANTE DECIR QUE EN LA TESIS SIGUIENTE
https://tesis.ipn.mx/bitstream/handle/123456789/9067/9.pdf?sequence=1&isAllowed=y
PAGINA 23 LA FORMULA TRAE UNA DIFERENCIA PER APUNTE: