Python vの例

コード例 #1

ファイル: VJM.py プロジェクト: EriKarasik/ARHW2

def JacobianPassiveLeg(tFk, tb, Tt, qa, qp, th):
    tFk[0:3, 3] = 0
    dt = dot([tb,dot([Tz(qa),Tz(th[0]),dRz(qp[0]),Tx(0.75),Tx(th[1]),Ty(th[2]),Tz(th[3]),Rx(th[4]),Ry(th[5]),Rz(th[6]),
        Rz(qp[1]),Tx(0.75),Tx(th[7]),Ty(th[8]),Tz(th[9]),Rx(th[10]),Ry(th[11]),Rz(th[12]),Rz(qp[2])]),Tt,transpose(tFk)])
    J1 = v([dt[0,3],dt[1,3],dt[2,3],dt[2,1],dt[0,2],dt[1,0]])
    dt = dot([tb,dot([Tz(qa),Tz(th[0]),Rz(qp[0]),Tx(0.75),Tx(th[1]),Ty(th[2]),Tz(th[3]),Rx(th[4]),Ry(th[5]),Rz(th[6]),
        dRz(qp[1]),Tx(0.75),Tx(th[7]),Ty(th[8]),Tz(th[9]),Rx(th[10]),Ry(th[11]),Rz(th[12]),Rz(qp[2])]),Tt,transpose(tFk)])
    J2 = v([dt[0,3],dt[1,3],dt[2,3],dt[2,1],dt[0,2],dt[1,0]])
    dt = dot([tb,dot([Tz(qa),Tz(th[0]),Rz(qp[0]),Tx(0.75),Tx(th[1]),Ty(th[2]),Tz(th[3]),Rx(th[4]),Ry(th[5]),Rz(th[6]),
        Rz(qp[1]),Tx(0.75),Tx(th[7]),Ty(th[8]),Tz(th[9]),Rx(th[10]),Ry(th[11]),Rz(th[12]),dRz(qp[2])]),Tt,transpose(tFk)])
    J3 = v([dt[0,3],dt[1,3],dt[2,3],dt[2,1],dt[0,2],dt[1,0]])
    return h([J1,J2,J3])

コード例 #2

def Jth(q, theta):
    H = fk(q, theta)
    H[0:3, 3] = 0
    inv_H = transpose(H)
    dH = dot([
        Tz(l1),
        Rz(q[0]),
        dRz(theta[0]),
        Tz(q[1]),
        Tz(theta[1]),
        Ty(l3),
        Ty(q[2]),
        Ty(theta[2]), inv_H
    ])
    J1 = v([dH[0, 3], dH[1, 3], dH[2, 3], dH[2, 1], dH[0, 2], dH[1, 0]])
    dH = dot([
        Tz(l1),
        Rz(q[0]),
        Rz(theta[0]),
        Tz(q[1]),
        dTz(),
        Ty(l3),
        Ty(q[2]),
        Ty(theta[2]), inv_H
    ])
    J2 = v([dH[0, 3], dH[1, 3], dH[2, 3], dH[2, 1], dH[0, 2], dH[1, 0]])
    dH = dot([
        Tz(l1),
        Rz(q[0]),
        Rz(theta[0]),
        Tz(q[1]),
        Tz(theta[1]),
        Ty(l3),
        Ty(q[2]),
        dTy(), inv_H
    ])
    J3 = v([dH[0, 3], dH[1, 3], dH[2, 3], dH[2, 1], dH[0, 2], dH[1, 0]])
    return h([J1, J2, J3])

コード例 #3

ファイル: vectoraddition_benchmark.py プロジェクト: Zer0-/Udon

def t1():
    a, b = gen()
    a = v(a)
    b = v(b)
    for i in range(m):
        a + b

コード例 #4

 def stepsForSegment(self, p1, p2):
     dist = np.linalg.norm(v(p1) - v(p2))
     out = int(dist / self.resolution)
     return out

コード例 #5

ファイル: vectoraddition_benchmark.py プロジェクト: kururugi/Udon

def t1():
    a, b = gen()
    a = v(a)
    b = v(b)
    for i in range(m):
        a + b

コード例 #6

ファイル: VJM.py プロジェクト: EriKarasik/ARHW2

def JacobianthLeg(TFk,Tb,Tt,qa,qp,th):
    TFk[0:3, 3] = 0
    dt = dot([Tb,dot([Tz(qa),dTz(),Rz(qp[0]),Tx(0.75),Tx(th[1]),Ty(th[2]),Tz(th[3]),Rx(th[4]),Ry(th[5]),Rz(th[6]),
        Rz(qp[1]),Tx(0.75),Tx(th[7]),Ty(th[8]), Tz(th[9]), Rx(th[10]), Ry(th[11]),Rz(th[12]),Rz(qp[2])]), Tt, transpose(TFk)])
    J1 = v([dt[0, 3], dt[1, 3], dt[2, 3], dt[2, 1], dt[0, 2], dt[1, 0]])
    dt = dot([Tb,dot([Tz(qa),Tz(th[0]),Rz(qp[0]),Tx(0.75),dTx(),Ty(th[2]),Tz(th[3]),Rx(th[4]),Ry(th[5]),Rz(th[6]),
        Rz(qp[1]),Tx(0.75),Tx(th[7]),Ty(th[8]), Tz(th[9]), Rx(th[10]), Ry(th[11]),Rz(th[12]),Rz(qp[2])]), Tt, transpose(TFk)])
    J2 = v([dt[0, 3], dt[1, 3], dt[2, 3], dt[2, 1], dt[0, 2], dt[1, 0]])
    dt = dot([Tb,dot([Tz(qa),Tz(th[0]),Rz(qp[0]),Tx(0.75),Tx(th[1]),dTy(),Tz(th[3]),Rx(th[4]),Ry(th[5]),Rz(th[6]),
        Rz(qp[1]),Tx(0.75),Tx(th[7]),Ty(th[8]),Tz(th[9]),Rx(th[10]),Ry(th[11]),Rz(th[12]),Rz(qp[2])]), Tt, transpose(TFk)])
    J3 = v([dt[0, 3], dt[1, 3], dt[2, 3], dt[2, 1], dt[0, 2], dt[1, 0]])
    dt = dot([Tb,dot([Tz(qa),Tz(th[0]),Rz(qp[0]),Tx(0.75),Tx(th[1]),Ty(th[2]),dTz(),Rx(th[4]),Ry(th[5]),Rz(th[6]),
        Rz(qp[1]),Tx(0.75),Tx(th[7]),Ty(th[8]),Tz(th[9]),Rx(th[10]),Ry(th[11]),Rz(th[12]),Rz(qp[2])]), Tt, transpose(TFk)])
    J4 = v([dt[0, 3], dt[1, 3], dt[2, 3], dt[2, 1], dt[0, 2], dt[1, 0]])
    dt = dot([Tb,dot([Tz(qa),Tz(th[0]),Rz(qp[0]),Tx(0.75),Tx(th[1]),Ty(th[2]),Tz(th[3]),dRx(th[4]),Ry(th[5]),Rz(th[6]),
        Rz(qp[1]),Tx(0.75),Tx(th[7]),Ty(th[8]), Tz(th[9]), Rx(th[10]), Ry(th[11]),Rz(th[12]),Rz(qp[2])]), Tt, transpose(TFk)])
    J5 = v([dt[0, 3], dt[1, 3], dt[2, 3], dt[2, 1], dt[0, 2], dt[1, 0]])
    dt = dot([Tb,dot([Tz(qa),Tz(th[0]),Rz(qp[0]),Tx(0.75),Tx(th[1]),Ty(th[2]),Tz(th[3]),Rx(th[4]),dRy(th[5]),Rz(th[6]),
        Rz(qp[1]),Tx(0.75),Tx(th[7]),Ty(th[8]),Tz(th[9]), Rx(th[10]), Ry(th[11]),Rz(th[12]),Rz(qp[2])]), Tt, transpose(TFk)])
    J6 = v([dt[0, 3], dt[1, 3], dt[2, 3], dt[2, 1], dt[0, 2], dt[1, 0]])
    dt = dot([Tb,dot([Tz(qa),Tz(th[0]),Rz(qp[0]),Tx(0.75),Tx(th[1]),Ty(th[2]),Tz(th[3]),Rx(th[4]),Ry(th[5]),dRz(th[6]),
        Rz(qp[1]),Tx(0.75),Tx(th[7]),Ty(th[8]),Tz(th[9]), Rx(th[10]), Ry(th[11]),Rz(th[12]),Rz(qp[2])]), Tt, transpose(TFk)])
    J7 = v([dt[0, 3], dt[1, 3], dt[2, 3], dt[2, 1], dt[0, 2], dt[1, 0]])
    dt = dot([Tb,dot([Tz(qa),Tz(th[0]),Rz(qp[0]),Tx(0.75),Tx(th[1]),Ty(th[2]),Tz(th[3]),Rx(th[4]),Ry(th[5]),Rz(th[6]),
        Rz(qp[1]),Tx(0.75),dTx(),Ty(th[8]),Tz(th[9]),Rx(th[10]),Ry(th[11]),Rz(th[12]),Rz(qp[2])]), Tt, transpose(TFk)])
    J8 = v([dt[0, 3], dt[1, 3], dt[2, 3], dt[2, 1], dt[0, 2], dt[1, 0]])
    dt = dot([Tb,dot([Tz(qa),Tz(th[0]),Rz(qp[0]),Tx(0.75),Tx(th[1]),Ty(th[2]),Tz(th[3]),Rx(th[4]),Ry(th[5]),Rz(th[6]),
        Rz(qp[1]),Tx(0.75),Tx(th[7]),dTy(),Tz(th[9]),Rx(th[10]),Ry(th[11]),Rz(th[12]),Rz(qp[2])]), Tt, transpose(TFk)])
    J9 = v([dt[0, 3], dt[1, 3], dt[2, 3], dt[2, 1], dt[0, 2], dt[1, 0]])
    dt = dot([Tb,dot([Tz(qa),Tz(th[0]),Rz(qp[0]),Tx(0.75),Tx(th[1]),Ty(th[2]),Tz(th[3]),Rx(th[4]),Ry(th[5]),Rz(th[6]),
        Rz(qp[1]),Tx(0.75),Tx(th[7]),Ty(th[8]),dTz(),Rx(th[10]),Ry(th[11]),Rz(th[12]),Rz(qp[2])]), Tt, transpose(TFk)])
    J10 = v([dt[0, 3], dt[1, 3], dt[2, 3], dt[2, 1], dt[0, 2], dt[1, 0]])
    dt = dot([Tb,dot([Tz(qa),Tz(th[0]),Rz(qp[0]),Tx(0.75),Tx(th[1]),Ty(th[2]),Tz(th[3]),Rx(th[4]),Ry(th[5]),Rz(th[6]),
        Rz(qp[1]),Tx(0.75),Tx(th[7]),Ty(th[8]),Tz(th[9]),dRx(th[10]),Ry(th[11]),Rz(th[12]),Rz(qp[2])]), Tt, transpose(TFk)])
    J11 = v([dt[0, 3], dt[1, 3], dt[2, 3], dt[2, 1], dt[0, 2], dt[1, 0]])
    dt = dot([Tb,dot([Tz(qa),Tz(th[0]),Rz(qp[0]),Tx(0.75),Tx(th[1]),Ty(th[2]),Tz(th[3]),Rx(th[4]),Ry(th[5]),Rz(th[6]),
        Rz(qp[1]),Tx(0.75),Tx(th[7]),Ty(th[8]),Tz(th[9]),Rx(th[10]),dRy(th[11]),Rz(th[12]),Rz(qp[2])]), Tt, transpose(TFk)])
    J12 = v([dt[0, 3], dt[1, 3], dt[2, 3], dt[2, 1], dt[0, 2], dt[1, 0]])
    dt = dot([Tb,dot([Tz(qa),Tz(th[0]),Rz(qp[0]),Tx(0.75),Tx(th[1]),Ty(th[2]),Tz(th[3]),Rx(th[4]),Ry(th[5]),Rz(th[6]),
        Rz(qp[1]),Tx(0.75),Tx(th[7]),Ty(th[8]),Tz(th[9]),Rx(th[10]),Ry(th[11]),dRz(th[12]),Rz(qp[2])]), Tt, transpose(TFk)])
    J13 = v([dt[0, 3], dt[1, 3], dt[2, 3], dt[2, 1], dt[0, 2], dt[1, 0]])
    return h([J1, J2, J3, J4, J5, J6, J7, J8, J9, J10, J11, J12, J13])

コード例 #7

ファイル: VJM.py プロジェクト: EriKarasik/ARHW2

def Tz(q): return [[1,0,0,0],[0,1,0,0],[0,0,1,q],[0,0,0,1]]
def dTz(): return [[0,0,0,0],[0,0,0,0],[0,0,0,1],[0,0,0,0]]

th = [zeros(13), zeros(13), zeros(13)]
Tb = [dot([Ty(1),Ry(pi/2),Rz(pi)]), dot([Tz(1),Rx(-pi/2)]),I(4)]
Tt = [transpose(dot([Ry(pi/2),Rz(pi)])), transpose(Rx(-pi/2)), I(4)]

k11 = [[E*A/L,0,0,0,0,0],[0,12*E*Iz/L**3,0,0,0,6*E*Iz/L**2],[0,0,12*E*Iy/L**3,0,-6*E*Iy/L**2,0],[0,0,0,G*Ip/L,0,0],
       [0,0,-6*E*Iy/L**2,0,4*E*Iy/L,0],[0,6*E*Iz/L**2,0,0,0,4*E*Iz/L]]
k12 = [[-E*A/L,0,0,0,0,0],[0,-12*E*Iz/L**3,0,0,0,-6*E*Iz/L**2],[0,0,-12*E*Iy/L**3,0,6*E*Iy/L**2,0],[0,0,0,-G*Ip/L,0,0],
       [0,0,-6*E*Iy/L**2,0,2*E*Iy/L,0],[0,6*E*Iz/L**2,0,0,0,2*E*Iz/L]]
k22 = [[E*A/L,0,0,0,0,0],[0,12*E*Iz/L**3,0,0,0,-6*E*Iz/L**2],[0,0,12*E*Iy/L**3,0,6*E*Iy/L**2,0],[0,0,0,G*Ip/L,0,0],
     [0,0,6*E*Iy/L**2,0,4*E*Iy/L,0],[0,-6*E*Iz/L**2,0,0,0,4*E*Iz/L]]
k21 = transpose(k12)
#K = v([h([k11,k12]),h([transpose(k12), k22])])
Kth = v([h([Ka,zeros(12)]),h([zeros((6,1)),k22,zeros((6,6))]),h([zeros((6,1)),zeros((6,6)),k22])])

def JacobianPassiveLeg(tFk, tb, Tt, qa, qp, th):
    tFk[0:3, 3] = 0
    dt = dot([tb,dot([Tz(qa),Tz(th[0]),dRz(qp[0]),Tx(0.75),Tx(th[1]),Ty(th[2]),Tz(th[3]),Rx(th[4]),Ry(th[5]),Rz(th[6]),
        Rz(qp[1]),Tx(0.75),Tx(th[7]),Ty(th[8]),Tz(th[9]),Rx(th[10]),Ry(th[11]),Rz(th[12]),Rz(qp[2])]),Tt,transpose(tFk)])
    J1 = v([dt[0,3],dt[1,3],dt[2,3],dt[2,1],dt[0,2],dt[1,0]])
    dt = dot([tb,dot([Tz(qa),Tz(th[0]),Rz(qp[0]),Tx(0.75),Tx(th[1]),Ty(th[2]),Tz(th[3]),Rx(th[4]),Ry(th[5]),Rz(th[6]),
        dRz(qp[1]),Tx(0.75),Tx(th[7]),Ty(th[8]),Tz(th[9]),Rx(th[10]),Ry(th[11]),Rz(th[12]),Rz(qp[2])]),Tt,transpose(tFk)])
    J2 = v([dt[0,3],dt[1,3],dt[2,3],dt[2,1],dt[0,2],dt[1,0]])
    dt = dot([tb,dot([Tz(qa),Tz(th[0]),Rz(qp[0]),Tx(0.75),Tx(th[1]),Ty(th[2]),Tz(th[3]),Rx(th[4]),Ry(th[5]),Rz(th[6]),
        Rz(qp[1]),Tx(0.75),Tx(th[7]),Ty(th[8]),Tz(th[9]),Rx(th[10]),Ry(th[11]),Rz(th[12]),dRz(qp[2])]),Tt,transpose(tFk)])
    J3 = v([dt[0,3],dt[1,3],dt[2,3],dt[2,1],dt[0,2],dt[1,0]])
    return h([J1,J2,J3])

def JacobianthLeg(TFk,Tb,Tt,qa,qp,th):