def test_q2aa(self): print "\ntest_q2aa" v = np.array(range(1,4), dtype=float) v /= np.linalg.norm(v) q = np.zeros(4) q[1:4] = v q[0] = 4 print q aa = rotations.q2aa(q) print repr(aa) aatrue = np.array([ 0.1309466 , 0.26189321, 0.39283981]) for v1, v2 in izip(aa, aatrue): self.assertAlmostEqual(v1, v2, 4)
def interpolate_angleaxis(initial, final, t): """interpolate between 2 arrays of angle axis coordinates Parameters ---------- initial: np.array configuration 1 final: np.array configuration 2 t: float interpolation parameter with t in [0,1] """ conf = initial.copy() for i in xrange(conf.shape[0]): conf[i] = rotations.q2aa(rotations.q_slerp(rotations.aa2q(initial[i]), rotations.aa2q(final[i]), t)) return conf
def interpolate_angleaxis(initial, final, t): """interpolate between 2 arrays of angle axis coordinates Parameters ---------- initial: np.array configuration 1 final: np.array configuration 2 t: float interpolation parameter with t in [0,1] """ conf = initial.copy() for i in range(conf.shape[0]): conf[i] = rotations.q2aa(rotations.q_slerp(rotations.aa2q(initial[i]), rotations.aa2q(final[i]), t)) return conf
def rotate_aa(p1, p2): """ change a given angle axis rotation p1 by the rotation p2 """ return q2aa(q_multiply( aa2q(p2), aa2q(p1) ))
def mx2aa(m): return q2aa(mx2q(m))
def rotate_aa(p1, p2): """ change a given angle axis rotation p2 by the rotation p1 """ return q2aa(q_multiply(aa2q(p2), aa2q(p1)))