1.

Calcular la cinemática directa e inversa del robot de la siguiente figura:

CINEMATICA DIRECTA
𝑐𝑜𝑠(𝑞1) ∗ 𝑐𝑜𝑠(𝑞4) −𝑐𝑜𝑠(𝑞1) ∗ 𝑠𝑖𝑛(𝑞4) 𝑠𝑖𝑛(𝑞1) 𝑎2 ∗ 𝑐𝑜𝑠(𝑞1) + 𝑙4 ∗ 𝑠𝑖𝑛(𝑞1) + 𝑞3 ∗ 𝑠𝑖𝑛(𝑞1)
𝑐𝑜𝑠(𝑞4) ∗ 𝑠𝑖𝑛(𝑞1) −𝑠𝑖𝑛(𝑞1) ∗ 𝑠𝑖𝑛(𝑞4) −𝑐𝑜𝑠(𝑞1) 𝑎2 ∗ 𝑠𝑖𝑛(𝑞1) − 𝑙4 ∗ 𝑐𝑜𝑠(𝑞1) − 𝑞3 ∗ 𝑐𝑜𝑠(𝑞1)
𝑠𝑖𝑛(𝑞4) 𝑐𝑜𝑠(𝑞4) 0 𝑙1 + 𝑞2

CINEMATICA INVERSA
2. Calcular la cinemática directa e inversa del robot de la siguiente figura:
 CINEMATICA DIRECTA

Nx = sin(q6)*(sin(q1 + 90)*cos(q4) - sin(q4)*(cos(q1 + 90)*cos(q2 - 90)*cos(q3) -

cos(q1 + 90)*sin(q2 - 90)*sin(q3))) + cos(q6)*(cos(q5)*(sin(q1 + 90)*sin(q4) +
cos(q4)*(cos(q1 + 90)*cos(q2 - 90)*cos(q3) - cos(q1 + 90)*sin(q2 - 90)*sin(q3))) -
sin(q5)*(cos(q1 + 90)*cos(q2 - 90)*sin(q3) + cos(q1 + 90)*sin(q2 - 90)*cos(q3)))

Ny = cos(q6)*(cos(q5)*(cos(q4)*(cos(q2 - 90)*sin(q1 + 90)*cos(q3) - sin(q1 +

90)*sin(q2 - 90)*sin(q3)) - cos(q1 + 90)*sin(q4)) - sin(q5)*(cos(q2 - 90)*sin(q1 +
90)*sin(q3) + sin(q1 + 90)*sin(q2 - 90)*cos(q3))) - sin(q6)*(sin(q4)*(cos(q2 -
90)*sin(q1 + 90)*cos(q3) - sin(q1 + 90)*sin(q2 - 90)*sin(q3)) + cos(q1 +
90)*cos(q4))
Nz= sin(q2 + q3 - 90)*sin(q4)*sin(q6) - cos(q6)*(cos(q2 + q3 - 90)*sin(q5) +
sin(q2 + q3 - 90)*cos(q4)*cos(q5))

Ox= cos(q6)*(sin(q1 + 90)*cos(q4) - sin(q4)*(cos(q1 + 90)*cos(q2 - 90)*cos(q3) -

cos(q1 + 90)*sin(q2 - 90)*sin(q3))) - sin(q6)*(cos(q5)*(sin(q1 + 90)*sin(q4) +
cos(q4)*(cos(q1 + 90)*cos(q2 - 90)*cos(q3) - cos(q1 + 90)*sin(q2 - 90)*sin(q3))) -
sin(q5)*(cos(q1 + 90)*cos(q2 - 90)*sin(q3) + cos(q1 + 90)*sin(q2 - 90)*cos(q3)))

Oy= - cos(q6)*(sin(q4)*(cos(q2 - 90)*sin(q1 + 90)*cos(q3) - sin(q1 + 90)*sin(q2 -

90)*sin(q3)) + cos(q1 + 90)*cos(q4)) - sin(q6)*(cos(q5)*(cos(q4)*(cos(q2 -
90)*sin(q1 + 90)*cos(q3) - sin(q1 + 90)*sin(q2 - 90)*sin(q3)) - cos(q1 +
90)*sin(q4)) - sin(q5)*(cos(q2 - 90)*sin(q1 + 90)*sin(q3) + sin(q1 + 90)*sin(q2 -
90)*cos(q3)))

Oz= sin(q6)*(cos(q2 + q3 - 90)*sin(q5) + sin(q2 + q3 - 90)*cos(q4)*cos(q5)) +

sin(q2 + q3 - 90)*cos(q6)*sin(q4)

Ax= - sin(q5)*(sin(q1 + 90)*sin(q4) + cos(q4)*(cos(q1 + 90)*cos(q2 - 90)*cos(q3)

- cos(q1 + 90)*sin(q2 - 90)*sin(q3))) - cos(q5)*(cos(q1 + 90)*cos(q2 - 90)*sin(q3)
+ cos(q1 + 90)*sin(q2 - 90)*cos(q3))

Ay= - cos(q5)*(cos(q2 - 90)*sin(q1 + 90)*sin(q3) + sin(q1 + 90)*sin(q2 -

90)*cos(q3)) - sin(q5)*(cos(q4)*(cos(q2 - 90)*sin(q1 + 90)*cos(q3) - sin(q1 +
90)*sin(q2 - 90)*sin(q3)) - cos(q1 + 90)*sin(q4))

Az= sin(q2 + q3 - 90)*cos(q4)*sin(q5) - cos(q2 + q3 - 90)*cos(q5)

Px= l5*cos(q1 + 90)*sin(q2 - 90)*cos(q4)*sin(q3)*sin(q5) - l4*cos(q1 + 90)*sin(q2

+ q3 - 90) - l5*cos(q1 + 90)*sin(q2 + q3 - 90)*cos(q5) - l5*sin(q1 +
90)*sin(q4)*sin(q5) - l5*cos(q1 + 90)*cos(q2 - 90)*cos(q3)*cos(q4)*sin(q5) -
a2*cos(q1 + 90)*cos(q2 - 90)

Py= l5*cos(q1 + 90)*sin(q4)*sin(q5) - l4*sin(q1 + 90)*sin(q2 + q3 - 90) - l5*sin(q1

+ 90)*sin(q2 + q3 - 90)*cos(q5) - a2*cos(q2 - 90)*sin(q1 + 90) - l5*cos(q2 -
90)*sin(q1 + 90)*cos(q3)*cos(q4)*sin(q5) + l5*sin(q1 + 90)*sin(q2 -
90)*cos(q4)*sin(q3)*sin(q5)

Pz= l1 + a2*sin(q2 - 90) - l5*(cos(q5)*(cos(q2 - 90)*cos(q3) - sin(q2 - 90)*sin(q3))

- cos(q4)*sin(q5)*(cos(q2 - 90)*sin(q3) + sin(q2 - 90)*cos(q3))) - l4*(cos(q2 -
90)*cos(q3) - sin(q2 - 90)*sin(q3))