Código:
y este es mi codigodef fkine(robot, q): q = mat(q) n = numrows(robot) if numrows(q)==1 and numcols(q)==n: t = robot.base for i in range(0,n): t = t * robot.links[i].tr(q[0,i]) t = t * robot.tool return t else: if numcols(q) != n: raise 'bad data' t = [] for qv in q: # for each trajectory point tt = robot.base for i in range(0,n): tt = tt * robot.links[i].tr(qv[0,i]) t.append(tt*robot.tool) return t
Código:
import numpy as np from math import * from robot import * from robot import Robot dh=np.matrix("0 0 pi/2 0 0;0 10 pi/2 0 0;0 10 pi/2 0 0") q=np.matrix("pi/2 pi/2 pi/2") z=fkine(dh,q) print z