def cexp(a):
return asrl.axisAngle2r(a)
rval = qdot(rval, qexp2(ldqi * tildeB))
return rval
if True:
T = np.arange(bs.t_min() + 1e-6,
bs.t_max() - 1e-6, (bs.t_max() - bs.t_min()) / 200)
#qval = np.array( [ qdot(qleft,qdot(cumQuat(bs,t,qc),qright))[:,0] for t in T ] )
#qvalt = np.array( [ cumQuat(bs,t,qct) for t in T ] )
qval = np.array([cumQuat(bs, t, qct) for t in T])
qvalt = np.array([cumQuat2(bs, t, qct) for t in T])
nm = np.sum(qval * qval, 1)
pl.figure(3)
pl.clf()
pl.plot(T, qval, linewidth=4)
pl.plot(T, qvalt, linewidth=3, ls='--')
#pl.plot(T,nm,linewidth=4)
#pl.plot(k[3:-3],qc.T,'.k',markersize=4)
if True:
C = asrl.axisAngle2r(np.random.random([3, 1]) * 10)
R = asrl.axisAngle2r(np.random.random([3, 1]) * 10)
b = 10.3
C1 = cexp(b * clog(np.dot(R.T, np.dot(C, R))))
C2 = np.dot(R.T, np.dot(cexp(b * clog(C)), R))
print C1 - C2
def cexp(a):
return asrl.axisAngle2r(a)
return rval
if True:
T = np.arange(bs.t_min()+1e-6,bs.t_max()-1e-6,(bs.t_max() - bs.t_min())/200)
#qval = np.array( [ qdot(qleft,qdot(cumQuat(bs,t,qc),qright))[:,0] for t in T ] )
#qvalt = np.array( [ cumQuat(bs,t,qct) for t in T ] )
qval = np.array( [ cumQuat(bs,t,qct) for t in T ] )
qvalt = np.array( [ cumQuat2(bs,t,qct) for t in T ] )
nm = np.sum(qval * qval,1)
pl.figure(3)
pl.clf()
pl.plot(T,qval,linewidth=4)
pl.plot(T,qvalt,linewidth=3,ls='--')
#pl.plot(T,nm,linewidth=4)
#pl.plot(k[3:-3],qc.T,'.k',markersize=4)
if True:
C = asrl.axisAngle2r(np.random.random([3,1])*10)
R = asrl.axisAngle2r(np.random.random([3,1])*10)
b = 10.3
C1 = cexp(b * clog( np.dot(R.T, np.dot( C, R))))
C2 = np.dot( R.T, np.dot( cexp(b * clog(C) ), R))
print C1 - C2
