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
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