Example #1
0
def cumQuat2(bs,t,qc):
    si = bs.segmentIndex(t)
    cidx = bs.localCoefficientVectorIndices(t)
    Mi = bs.Mi(si)
    c  = bs.coefficients()
    dc = c[0,1:] - c[0,0:-1]    
    u = bs.u(t,0)
    bi = np.dot(u.T,Mi)

    ci = bs.localCoefficientVectorIndices(t)
    qci = qc[:,ci]
    rval = qci[:,0]

    Q = np.zeros([4,len(ci)])
    
    Q[:,0] = qci[:,0]
    #print bi
    #print sum(bi)
    #print ci
    for i in range(0,len(ci) - 1):
        dqi = qdot(qinv(qci[:,i]),qci[:,i+1])
        ldqi = qlog(dqi)
        tildeB = np.sum(bi[i+1:])
        qq = qexp(ldqi * tildeB)
        rval= qdot(rval, qq)
        Q[:,i+1] = qq

    rval = np.eye(4)
    for i in range(len(ci) - 1,0,-1):
        rval = np.dot(rval, asrl.quatPlus(qinv(Q[:,i])))
    rval = qinv(np.dot(rval,qinv(Q[:,0])))

    return rval
def cumQuat2(bs, t, qc):
    si = bs.segmentIndex(t)
    cidx = bs.localCoefficientVectorIndices(t)
    Mi = bs.Mi(si)
    c = bs.coefficients()
    dc = c[0, 1:] - c[0, 0:-1]
    u = bs.u(t, 0)
    bi = np.dot(u.T, Mi)

    ci = bs.localCoefficientVectorIndices(t)
    qci = qc[:, ci]
    rval = qci[:, 0]

    Q = np.zeros([4, len(ci)])

    Q[:, 0] = qci[:, 0]
    #print bi
    #print sum(bi)
    #print ci
    for i in range(0, len(ci) - 1):
        dqi = qdot(qinv(qci[:, i]), qci[:, i + 1])
        ldqi = qlog(dqi)
        tildeB = np.sum(bi[i + 1:])
        qq = qexp(ldqi * tildeB)
        rval = qdot(rval, qq)
        Q[:, i + 1] = qq

    rval = np.eye(4)
    for i in range(len(ci) - 1, 0, -1):
        rval = np.dot(rval, asrl.quatPlus(qinv(Q[:, i])))
    rval = qinv(np.dot(rval, qinv(Q[:, 0])))

    return rval
Example #3
0
def qdot(q1,q2):
    return numpy.dot(sm.quatPlus(q1),q2)
Example #4
0
def qdot(q1,q2):
    return dot(asrl.quatPlus(q1),q2)
Example #5
0
def qdot(q1, q2):
    return numpy.dot(sm.quatPlus(q1), q2)
def qdot(q1, q2):
    return dot(asrl.quatPlus(q1), q2)