re = pd_tm1_adams.l2()
rf = pd_tm1_adams.l1()
# Constantes Trigonometricas
sqrt3 = pd_tm1_adams.sqrt3()
pi = pd_tm1_adams.pi()
sin120 = sqrt3 / 2.0
cos120 = pd_tm1_adams.cos120()
tan60 = sqrt3
sin30 = pd_tm1_adams.sin30()
tan30 = pd_tm1_adams.tan30()
dtr = pd_tm1_adams.dtr()
mtmm = pd_tm1_adams.mtmm()
mmtm = pd_tm1_adams.mmtm()
rtd = pd_tm1_adams.rtd()
###############################################
########### [Jacobian Total] ##################
###############################################
def jacobian_total(px, py, pz, theta11, theta12, theta13):
# Cambio unidades Internacional
pxx = px * mmtm
pyy = py * mmtm
pzz = pz * mmtm
# Ordenar segun sistema LOCAL
punto = [-pyy, -pxx, -pzz]
phi_orden = [0, 120, 240]
thetai_orden = [theta12, theta11, theta13]
mp = pd_tm1_adams.mp()
# Trigonometric constants
sqrt3 = pd_tm1_adams.sqrt3()
pi = pd_tm1_adams.pi()
sin120 = sqrt3 / 2.0
cos120 = pd_tm1_adams.cos120()
tan60 = sqrt3
sin30 = pd_tm1_adams.sin30()
tan30 = pd_tm1_adams.tan30()
dtr = pd_tm1_adams.dtr() # degrees to radians
mtmm = pd_tm1_adams.mtmm() # Metros a Milimetros
mmtm = pd_tm1_adams.mmtm() # Milimetros a Metros
kgm2tgrmm2 = pd_tm1_adams.kgm2tgrmm2()
rtd = pd_tm1_adams.rtd() # degrees to radians
###############################################
########## [Torque Menu Input] ################
###############################################
# |------------------------------------------|
# |------- Torque TOTAL (puntual) -----------|
# |------------------------------------------|
def ti(theta11_pp_deg, theta12_pp_deg, theta13_pp_deg, theta11_deg,
theta12_deg, theta13_deg, xp_mm, yp_mm, zp_mm, xp_pp_mm, yp_pp_mm,
zp_pp_mm, fpx, fpy, fpz, m_playload):
# Cambiar unidades a SI
# Cambiar orden angulos y coordenadas cartesianas a sistema referencia LOCAL
phi = [0 * dtr, 120 * dtr, 240 * dtr]
theta1i_pp = [