Integrantes

Granada Jhonny
Alex Jami
Salazar Pablo
Villacres Roberto

Definición de matriz homogénea

import sympy as sp #biblioteca para el calculo simbolico

def sTdh (d, th, a, alpha):
cth = sp.cos(th);
sth = sp.sin(th);
ca = sp.cos(alpha);
sa = sp.sin(alpha);
Tdh = sp.Matrix([[cth, -ca*sth, sa*sth, a*cth],
[sth, ca*cth, -sa*cth, a*sth],
[0, sa, ca, d],
[0, 0, 0, 1]])
return Tdh

#Definicion de variables simbolicas

q1, q2, q3 = sp.symbols("q1 q2 q3")
l1, l2, l3 = sp.symbols("l1 l2 l3")
#Trasnformacion Homogenea
T01 = sTdh(0, q1, l1, 0)
T12 = sTdh(0, sp.pi/2+q2, -l2, 0)
T23 = sTdh(l3, 145+q3, 0, 0)
print("T01:"); display(T01)
print("T12:"); display(T12)
print("T23:"); display(T23)

T01:
⎡cos (q1) — sin (q1) 0 l1cos (q 1) ⎤
⎢ sin (q1) cos (q1) 0 l1 sin (q1)⎥
0 0 1 0
⎣ 0 0 0 1 ⎦
T12:
⎡— sin (q2) — cos (q2) 0 l2 sin (q2) ⎤
⎢ cos (q2) — sin (q2) 0 −l2 cos (q2) ⎥
⎢ 0 0 1 0 ⎥
⎣ 0 0 0 1 ⎦
T23:
cos (q3 + 145) — sin (q3 + 145) 0 0⎤
sin (q3 + 145) cos (q3 + 145) 0 0⎥
⎢ 0 0 1 l3 ⎥
⎣ 0 0 0 1⎦

1 de 3 10/07/2021 15:49
 Tf = sp.simplify(T01*T12*T23)
print("Transformacion final T03"); display(Tf)

Transformacion final T03

⎡— sin (q1 + q2 + q3 + 145) − cos (q1 + q2 + q3 + 145) 0 l1 cos (q1) + l2 sin (q1 + q2) ⎤
✓ 0s se ejecutó 15:48

T03 cuando q=(0,0,0):

⎡− sin (145) − cos (145) 0 0.2

⎢
0 0
⎣ cos (145) − sin (145) 0 −0.14 ⎦
⎢ 0 0 1 0.1 ⎥
0 1

Calculo Numerico de la cinematica directa

import numpy as np
def Tdh(d,th,a,alpha):
cth= np.cos(th)
sth=np.sin(th)
ca=np.cos(alpha)
sa=np.sin(alpha)

Tdh= np.array([[cth, -ca*sth, sa*sth, a*cth],

[sth, ca*cth, -sa*cth, a*sth],
[0, sa, ca, d],
[0, 0, 0, 1]])
return Tdh

# funcionque obtiene la transgeometria dados parametrosDh

import numpy as np
def cdirecta_ejer(q,l1,l2,l3):

#transformaciones homogeneas de Dh
T01=Tdh(0,q[0],l1,0)
T12=Tdh(0,np.pi/2+q[1],-l2,0)
T23=Tdh(l3,145+q[2],0,0)

#Efecto final con respecto a la base

Tf=T01.dot(T12).dot(T23)
return Tf

#ejemplo de calculo de la cinematica directa

l1=0.2
l2=0.14
l3=0.10
q=[0,np.deg2rad(0), np.deg2rad(0),np.deg2rad(0)]
2 de 3 10/07/2021 15:49
 import serialrobot
L=[[0,0,l1,0,'r'],
[0,np.pi/2,-l2,0,'r'],
[l3,145,0,0,'r']]
ejer=serialrobot.SerialRobot(L, name="ejer")
T=ejer.fkine([0,0,0], verbose=False)
print(np.round(T,2))

[[-0.47 -0.88 0. 0.2 ]

[ 0.88 -0.47 0. -0.14]
[ 0. 0. 1. 0.1 ]
[ 0. 0. 0. 1. ]]

3 de 3 10/07/2021 15:49