def test_rt2tr(self):
# 3D
R = rotx(0.2)
t = [3, 4, 5]
T = rt2tr(R, t)
nt.assert_array_almost_equal(t2r(T), R)
nt.assert_array_almost_equal(transl(T), np.array(t))
theta = sym.symbol('theta')
R = rotx(theta)
self.assertEqual(r2t(R).dtype, 'O')
# 2D
R = rot2(0.2)
t = [3, 4]
T = rt2tr(R, t)
nt.assert_array_almost_equal(t2r(T), R)
nt.assert_array_almost_equal(transl2(T), np.array(t))
theta = sym.symbol('theta')
R = rot2(theta)
self.assertEqual(r2t(R).dtype, 'O')
with self.assertRaises(ValueError):
rt2tr(3, 4)
with self.assertRaises(ValueError):
rt2tr(np.eye(3, 4), [1, 2, 3, 4])
def vexa(Omega):
r"""
Convert skew-symmetric matrix to vector
:param s: augmented skew-symmetric matrix
:type s: numpy.ndarray, shape=(3,3) or (4,4)
:return: vector of unique values
:rtype: numpy.ndarray, shape=(3,) or (6,)
:raises: ValueError
``vex(S)`` is the vector which has the corresponding skew-symmetric matrix ``S``.
- ``S`` is 3x3 - se(2) case - where ``S`` :math:`= \left[ \begin{array}{ccc} 0 & -v_3 & v_1 \\ v_3 & 0 & v_2 \\ 0 & 0 & 0 \end{array} \right]` then return :math:`[v_1, v_2, v_3]`.
- ``S`` is 4x4 - se(3) case - where ``S`` :math:`= \left[ \begin{array}{cccc} 0 & -v_6 & v_5 & v_1 \\ v_6 & 0 & -v_4 & v_2 \\ -v_5 & v_4 & 0 & v_3 \\ 0 & 0 & 0 & 0 \end{array} \right]` then return :math:`[v_1, v_2, v_3, v_4, v_5, v_6]`.
Notes:
- This is the inverse of the function ``skewa``.
- Only rudimentary checking (zero diagonal) is done to ensure that the matrix
is actually skew-symmetric.
- The function takes the mean of the two elements that correspond to each unique
element of the matrix.
:seealso: skewa, vex
"""
if Omega.shape == (4, 4):
return np.hstack((t3d.transl(Omega), vex(t2r(Omega))))
elif Omega.shape == (3, 3):
return np.hstack((t2d.transl2(Omega), vex(t2r(Omega))))
else:
raise AttributeError("expecting a 3x3 or 4x4 matrix")
def test_t2r(self):
# 3D
t = [1, 2, 3]
T = trotx(0.3, t=t)
R = t2r(T)
nt.assert_array_almost_equal(T[:3, :3], R)
nt.assert_array_almost_equal(transl(T), np.array(t))
# 2D
t = [1, 2]
T = trot2(0.3, t=t)
R = t2r(T)
nt.assert_array_almost_equal(T[:2, :2], R)
nt.assert_array_almost_equal(transl2(T), np.array(t))
with self.assertRaises(ValueError):
t2r(3)
with self.assertRaises(ValueError):
r2t(np.eye(3, 4))