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)
def test_aa2q(self):
aa = random_aa()
q1 = aa2q(aa)
q2 = rotations.aa2q(aa)
self.arrays_equal(q1, q2)
