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 test_aa2q(self): print "\ntest_aa2q" p = np.array(range(1,4), dtype=float) print p p /= np.linalg.norm(p) q = rotations.aa2q(p) print repr(q) qtrue = np.array([ 0.87758256, 0.12813186, 0.25626373, 0.38439559]) for v1, v2 in izip(q, qtrue): self.assertAlmostEqual(v1, v2, 4)
def test_aa2q(self): aa = random_aa() q1 = aa2q(aa) q2 = rotations.aa2q(aa) self.arrays_equal(q1, q2)