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)))
def mx2aa(m):
return q2aa(mx2q(m))