def test_isometry3(self, T):
Isometry3 = mut.Isometry3_[T]
# - Default constructor
transform = Isometry3()
self.assertEqual(numpy_compare.resolve_type(transform.matrix()), T)
X = np.eye(4, 4)
numpy_compare.assert_float_equal(transform.matrix(), X)
numpy_compare.assert_float_equal(copy.copy(transform).matrix(), X)
if T == float:
self.assertEqual(str(transform), str(X))
# - Constructor with (X)
transform = Isometry3(matrix=X)
numpy_compare.assert_float_equal(transform.matrix(), X)
# - Copy constructor.
cp = Isometry3(other=transform)
numpy_compare.assert_equal(transform.matrix(), cp.matrix())
# - Identity
transform = Isometry3.Identity()
numpy_compare.assert_float_equal(transform.matrix(), X)
# - Constructor with (R, p)
R = np.array([[0., 1, 0], [-1, 0, 0], [0, 0, 1]])
p = np.array([1., 2, 3])
X = np.vstack((np.hstack((R, p.reshape((-1, 1)))), [0, 0, 0, 1]))
transform = Isometry3(rotation=R, translation=p)
numpy_compare.assert_float_equal(transform.matrix(), X)
numpy_compare.assert_float_equal(transform.translation(), p)
transform.set_translation(-p)
numpy_compare.assert_float_equal(transform.translation(), -p)
numpy_compare.assert_float_equal(transform.rotation(), R)
transform.set_rotation(R.T)
numpy_compare.assert_float_equal(transform.rotation(), R.T)
# - Check transactions for bad values.
if T != Expression:
transform = Isometry3(rotation=R, translation=p)
R_bad = np.copy(R)
R_bad[0, 0] = 10.
with self.assertRaises(RuntimeError):
transform.set_rotation(R_bad)
numpy_compare.assert_float_equal(transform.rotation(), R)
X_bad = np.copy(X)
X_bad[:3, :3] = R_bad
with self.assertRaises(RuntimeError):
transform.set_matrix(X_bad)
numpy_compare.assert_float_equal(transform.matrix(), X)
# Test `type_caster`s.
if T == float:
value = test_util.create_isometry()
self.assertTrue(isinstance(value, mut.Isometry3))
test_util.check_isometry(value)
# Operations.
transform = Isometry3(rotation=R, translation=p)
transform_I = transform.inverse().multiply(transform)
numpy_compare.assert_float_equal(transform_I.matrix(), np.eye(4))
numpy_compare.assert_float_equal(
transform.multiply(position=[10, 20, 30]), [21., -8, 33])
if six.PY3:
numpy_compare.assert_float_equal(
eval("transform.inverse() @ transform").matrix(), np.eye(4))
numpy_compare.assert_float_equal(eval("transform @ [10, 20, 30]"),
[21., -8, 33])
def test_angle_axis(self, T):
AngleAxis = mut.AngleAxis_[T]
value_identity = AngleAxis.Identity()
self.assertEqual(numpy_compare.resolve_type(value_identity.angle()), T)
numpy_compare.assert_float_equal(value_identity.angle(), 0.)
numpy_compare.assert_float_equal(value_identity.axis(), [1., 0, 0])
# Construct with rotation matrix.
R = np.array([[0., 1, 0], [-1, 0, 0], [0, 0, 1]])
value = AngleAxis(rotation=R)
numpy_compare.assert_float_allclose(value.rotation(), R)
numpy_compare.assert_float_allclose(copy.copy(value).rotation(), R)
numpy_compare.assert_float_allclose(value.inverse().rotation(), R.T)
numpy_compare.assert_float_allclose(
value.multiply(value.inverse()).rotation(), np.eye(3))
if six.PY3:
numpy_compare.assert_float_allclose(
eval("value @ value.inverse()").rotation(), np.eye(3))
value.set_rotation(np.eye(3))
numpy_compare.assert_float_equal(value.rotation(), np.eye(3))
# Construct with quaternion.
Quaternion = mut.Quaternion_[T]
q = Quaternion(R)
value = AngleAxis(quaternion=q)
numpy_compare.assert_float_allclose(value.quaternion().wxyz(),
numpy_compare.to_float(q.wxyz()))
value.set_quaternion(Quaternion.Identity())
numpy_compare.assert_float_equal(value.quaternion().wxyz(),
[1., 0, 0, 0])
# Test setters.
value = AngleAxis(value_identity)
value.set_angle(np.pi / 4)
v = normalize(np.array([0.1, 0.2, 0.3]))
if T != Expression:
with self.assertRaises(RuntimeError):
value.set_axis([0.1, 0.2, 0.3])
value.set_axis(v)
numpy_compare.assert_float_equal(value.angle(), np.pi / 4)
numpy_compare.assert_float_equal(value.axis(), v)
# Cast.
self.check_cast(mut.AngleAxis_, T)
# Test symmetry based on accessors.
# N.B. `Eigen::AngleAxis` does not disambiguate by restricting internal
# angles and axes to a half-plane.
angle = np.pi / 6
axis = normalize([0.1, 0.2, 0.3])
value = AngleAxis(angle=angle, axis=axis)
value_sym = AngleAxis(angle=-angle, axis=-axis)
numpy_compare.assert_equal(value.rotation(), value_sym.rotation())
numpy_compare.assert_equal(value.angle(), -value_sym.angle())
numpy_compare.assert_equal(value.axis(), -value_sym.axis())
def get_vector(value):
return np.hstack((value.angle(), value.axis()))
assert_pickle(self, value, get_vector, T=T)
def check_angle_axis(self, T):
AngleAxis = mut.AngleAxis_[T]
value_identity = AngleAxis.Identity()
self.assertEqual(npc.resolve_type(value_identity.angle()), T)
npc.assert_float_equal(value_identity.angle(), 0.)
npc.assert_float_equal(value_identity.axis(), [1., 0, 0])
# Construct with rotation matrix.
R = np.array([
[0., 1, 0],
[-1, 0, 0],
[0, 0, 1]])
value = AngleAxis(rotation=R)
npc.assert_float_allclose(value.rotation(), R)
npc.assert_float_allclose(copy.copy(value).rotation(), R)
npc.assert_float_allclose(value.inverse().rotation(), R.T)
npc.assert_float_allclose(
value.multiply(value.inverse()).rotation(), np.eye(3))
if six.PY3:
npc.assert_float_allclose(
eval("value @ value.inverse()").rotation(), np.eye(3))
value.set_rotation(np.eye(3))
npc.assert_float_equal(value.rotation(), np.eye(3))
# Construct with quaternion.
Quaternion = mut.Quaternion_[T]
q = Quaternion(R)
value = AngleAxis(quaternion=q)
npc.assert_float_allclose(
value.quaternion().wxyz(), npc.to_float(q.wxyz()))
value.set_quaternion(Quaternion.Identity())
npc.assert_float_equal(value.quaternion().wxyz(), [1., 0, 0, 0])
# Test setters.
value = AngleAxis(value_identity)
value.set_angle(np.pi / 4)
v = normalize(np.array([0.1, 0.2, 0.3]))
if T != Expression:
with self.assertRaises(RuntimeError):
value.set_axis([0.1, 0.2, 0.3])
value.set_axis(v)
npc.assert_float_equal(value.angle(), np.pi / 4)
npc.assert_float_equal(value.axis(), v)
# Test symmetry based on accessors.
# N.B. `Eigen::AngleAxis` does not disambiguate by restricting internal
# angles and axes to a half-plane.
angle = np.pi / 6
axis = normalize([0.1, 0.2, 0.3])
value = AngleAxis(angle=angle, axis=axis)
value_sym = AngleAxis(angle=-angle, axis=-axis)
npc.assert_equal(value.rotation(), value_sym.rotation())
npc.assert_equal(value.angle(), -value_sym.angle())
npc.assert_equal(value.axis(), -value_sym.axis())
def test_resolve_type(self):
Af = np.array([1., 2.])
self.assertEqual(numpy_compare.resolve_type(Af), float)
Ac = np.array([Custom("a"), Custom("b")])
self.assertEqual(numpy_compare.resolve_type(Ac), Custom)
with self.assertRaises(AssertionError):
numpy_compare.resolve_type([])
with self.assertRaises(AssertionError):
numpy_compare.resolve_type(["a", 1., None])
def test_resolve_type(self):
Af = np.array([1., 2.])
self.assertEqual(npc.resolve_type(Af), float)
Ac = np.array([Custom("a"), Custom("b")])
self.assertEqual(npc.resolve_type(Ac), Custom)
with self.assertRaises(AssertionError):
npc.resolve_type([])
with self.assertRaises(AssertionError):
npc.resolve_type(["a", 1., None])
def test_quaternion(self, T):
# Simple API.
Quaternion = mut.Quaternion_[T]
cast = np.vectorize(T)
q_identity = Quaternion()
self.assertEqual(numpy_compare.resolve_type(q_identity.wxyz()), T)
numpy_compare.assert_float_equal(q_identity.wxyz(), [1., 0, 0, 0])
numpy_compare.assert_float_equal(
copy.copy(q_identity).wxyz(), [1., 0, 0, 0])
numpy_compare.assert_equal(q_identity.wxyz(),
Quaternion.Identity().wxyz())
if T == float:
self.assertEqual(str(q_identity),
"Quaternion_[float](w=1.0, x=0.0, y=0.0, z=0.0)")
self.check_cast(mut.Quaternion_, T)
# Test ordering.
q_wxyz = normalize([0.1, 0.3, 0.7, 0.9])
q = Quaternion(w=q_wxyz[0], x=q_wxyz[1], y=q_wxyz[2], z=q_wxyz[3])
# - Accessors.
numpy_compare.assert_float_equal(q.w(), q_wxyz[0])
numpy_compare.assert_float_equal(q.x(), q_wxyz[1])
numpy_compare.assert_float_equal(q.y(), q_wxyz[2])
numpy_compare.assert_float_equal(q.z(), q_wxyz[3])
numpy_compare.assert_float_equal(q.xyz(), q_wxyz[1:])
numpy_compare.assert_float_equal(q.wxyz(), q_wxyz)
# - Mutators.
q_wxyz_new = q_wxyz[::-1]
numpy_compare.assert_not_equal(q_wxyz, q_wxyz_new)
q.set_wxyz(wxyz=q_wxyz_new)
numpy_compare.assert_float_equal(q.wxyz(), q_wxyz_new)
q.set_wxyz(w=q_wxyz_new[0],
x=q_wxyz_new[1],
y=q_wxyz_new[2],
z=q_wxyz_new[3])
numpy_compare.assert_float_equal(q.wxyz(), q_wxyz_new)
# Alternative constructors.
q_other = Quaternion(wxyz=q_wxyz)
numpy_compare.assert_float_equal(q_other.wxyz(), q_wxyz)
R = np.array([[0., 0, 1], [1, 0, 0], [0, 1, 0]])
q_wxyz_expected = np.array([0.5, 0.5, 0.5, 0.5])
q_other = Quaternion(q_wxyz_expected)
numpy_compare.assert_float_equal(q_other.rotation(), R)
R_I = np.eye(3, 3)
q_other.set_rotation(R_I)
numpy_compare.assert_equal(q_other.wxyz(), q_identity.wxyz())
# - Copy constructor.
cp = Quaternion(other=q)
numpy_compare.assert_equal(q.wxyz(), cp.wxyz())
# Bad values.
if T != Expression:
q = Quaternion.Identity()
# - wxyz
q_wxyz_bad = [1., 2, 3, 4]
with self.assertRaises(RuntimeError):
q.set_wxyz(q_wxyz_bad)
numpy_compare.assert_float_equal(q.wxyz(), [1., 0, 0, 0])
# - Rotation.
R_bad = np.copy(R)
R_bad[0, 0] = 10
with self.assertRaises(RuntimeError):
q_other.set_rotation(R_bad)
numpy_compare.assert_float_equal(q_other.rotation(), R_I)
# Operations.
q_AB = Quaternion(wxyz=[0.5, 0.5, 0.5, 0.5])
q_I = q_AB.inverse().multiply(q_AB)
numpy_compare.assert_float_equal(q_I.wxyz(), [1., 0, 0, 0])
if six.PY3:
numpy_compare.assert_float_equal(
eval("q_AB.inverse() @ q_AB").wxyz(), [1., 0, 0, 0])
v_B = np.array([1., 2, 3])
v_A = np.array([3., 1, 2])
numpy_compare.assert_float_allclose(q_AB.multiply(vector=v_B), v_A)
vlist_B = np.array([v_B, v_B]).T
vlist_A = np.array([v_A, v_A]).T
numpy_compare.assert_float_equal(q_AB.multiply(vector=vlist_B),
vlist_A)
# Test deprecation.
with catch_drake_warnings(expected_count=2):
self.assertEqual(q_AB.multiply(position=v_B).shape, v_B.shape)
self.assertEqual(
q_AB.multiply(position=vlist_B).shape, vlist_B.shape)
with catch_drake_warnings(expected_count=0):
# No deprecation should happen with position arguments.
self.assertEqual(q_AB.multiply(v_B).shape, v_B.shape)
self.assertEqual(q_AB.multiply(vlist_B).shape, vlist_B.shape)
q_AB_conj = q_AB.conjugate()
numpy_compare.assert_float_equal(q_AB_conj.wxyz(),
[0.5, -0.5, -0.5, -0.5])
# Test `type_caster`s.
if T == float:
value = test_util.create_quaternion()
self.assertTrue(isinstance(value, mut.Quaternion))
test_util.check_quaternion(value)
def test_isometry3(self, T):
Isometry3 = mut.Isometry3_[T]
# - Default constructor
transform = Isometry3()
self.assertEqual(numpy_compare.resolve_type(transform.matrix()), T)
X_I_np = np.eye(4, 4)
numpy_compare.assert_float_equal(transform.matrix(), X_I_np)
numpy_compare.assert_float_equal(copy.copy(transform).matrix(), X_I_np)
if T == float:
self.assertEqual(str(transform), str(X_I_np))
# - Constructor with (X_I_np)
transform = Isometry3(matrix=X_I_np)
numpy_compare.assert_float_equal(transform.matrix(), X_I_np)
# - Copy constructor.
cp = Isometry3(other=transform)
numpy_compare.assert_equal(transform.matrix(), cp.matrix())
# - Identity
transform = Isometry3.Identity()
numpy_compare.assert_float_equal(transform.matrix(), X_I_np)
# - Constructor with (R, p)
R_AB = np.array([[0., 1, 0], [-1, 0, 0], [0, 0, 1]])
p_AB = np.array([1., 2, 3])
X_AB_np = np.eye(4)
X_AB_np[:3, :3] = R_AB
X_AB_np[:3, 3] = p_AB
X_AB = Isometry3(rotation=R_AB, translation=p_AB)
numpy_compare.assert_float_equal(X_AB.matrix(), X_AB_np)
numpy_compare.assert_float_equal(X_AB.translation(), p_AB)
numpy_compare.assert_float_equal(X_AB.rotation(), R_AB)
# - Setters.
X_AB = Isometry3()
X_AB.set_translation(p_AB)
numpy_compare.assert_float_equal(X_AB.translation(), p_AB)
X_AB.set_rotation(R_AB)
numpy_compare.assert_float_equal(X_AB.rotation(), R_AB)
# - Cast
self.check_cast(mut.Isometry3_, T)
# - Check transactions for bad values.
if T != Expression:
X_temp = Isometry3(rotation=R_AB, translation=p_AB)
R_bad = np.copy(R_AB)
R_bad[0, 0] = 10.
with self.assertRaises(RuntimeError):
X_temp.set_rotation(R_bad)
numpy_compare.assert_float_equal(X_temp.rotation(), R_AB)
X_bad_np = np.copy(X_I_np)
X_bad_np[:3, :3] = R_bad
with self.assertRaises(RuntimeError):
X_temp.set_matrix(X_bad_np)
numpy_compare.assert_float_equal(X_temp.matrix(), X_AB_np)
# Test `type_caster`s.
if T == float:
value = test_util.create_isometry()
self.assertTrue(isinstance(value, mut.Isometry3))
test_util.check_isometry(value)
# Operations.
X_AB = Isometry3(rotation=R_AB, translation=p_AB)
X_I = X_AB.inverse().multiply(X_AB)
numpy_compare.assert_float_equal(X_I.matrix(), X_I_np)
p_BQ = [10, 20, 30]
p_AQ = [21., -8, 33]
numpy_compare.assert_float_equal(X_AB.multiply(position=p_BQ), p_AQ)
p_BQlist = np.array([p_BQ, p_BQ]).T
p_AQlist = np.array([p_AQ, p_AQ]).T
numpy_compare.assert_float_equal(X_AB.multiply(position=p_BQlist),
p_AQlist)
if six.PY3:
numpy_compare.assert_float_equal(
eval("X_AB.inverse() @ X_AB").matrix(), X_I_np)
numpy_compare.assert_float_equal(eval("X_AB @ p_BQ"), p_AQ)
def test_quaternion(self, T):
# Simple API.
Quaternion = mut.Quaternion_[T]
cast = np.vectorize(T)
q_identity = Quaternion()
self.assertEqual(numpy_compare.resolve_type(q_identity.wxyz()), T)
numpy_compare.assert_float_equal(q_identity.wxyz(), [1., 0, 0, 0])
numpy_compare.assert_float_equal(
copy.copy(q_identity).wxyz(), [1., 0, 0, 0])
numpy_compare.assert_equal(q_identity.wxyz(),
Quaternion.Identity().wxyz())
if T == float:
self.assertEqual(str(q_identity),
"Quaternion_[float](w=1.0, x=0.0, y=0.0, z=0.0)")
# Test ordering.
q_wxyz = normalize([0.1, 0.3, 0.7, 0.9])
q = Quaternion(w=q_wxyz[0], x=q_wxyz[1], y=q_wxyz[2], z=q_wxyz[3])
# - Accessors.
numpy_compare.assert_float_equal(q.w(), q_wxyz[0])
numpy_compare.assert_float_equal(q.x(), q_wxyz[1])
numpy_compare.assert_float_equal(q.y(), q_wxyz[2])
numpy_compare.assert_float_equal(q.z(), q_wxyz[3])
numpy_compare.assert_float_equal(q.xyz(), q_wxyz[1:])
numpy_compare.assert_float_equal(q.wxyz(), q_wxyz)
# - Mutators.
q_wxyz_new = q_wxyz[::-1]
numpy_compare.assert_not_equal(q_wxyz, q_wxyz_new)
q.set_wxyz(wxyz=q_wxyz_new)
numpy_compare.assert_float_equal(q.wxyz(), q_wxyz_new)
q.set_wxyz(w=q_wxyz_new[0],
x=q_wxyz_new[1],
y=q_wxyz_new[2],
z=q_wxyz_new[3])
numpy_compare.assert_float_equal(q.wxyz(), q_wxyz_new)
# Alternative constructors.
q_other = Quaternion(wxyz=q_wxyz)
numpy_compare.assert_float_equal(q_other.wxyz(), q_wxyz)
R = np.array([[0., 0, 1], [1, 0, 0], [0, 1, 0]])
q_wxyz_expected = np.array([0.5, 0.5, 0.5, 0.5])
q_other = Quaternion(q_wxyz_expected)
numpy_compare.assert_float_equal(q_other.rotation(), R)
R_I = np.eye(3, 3)
q_other.set_rotation(R_I)
numpy_compare.assert_equal(q_other.wxyz(), q_identity.wxyz())
# - Copy constructor.
cp = Quaternion(other=q)
numpy_compare.assert_equal(q.wxyz(), cp.wxyz())
# Bad values.
if T != Expression:
q = Quaternion.Identity()
# - wxyz
q_wxyz_bad = [1., 2, 3, 4]
with self.assertRaises(RuntimeError):
q.set_wxyz(q_wxyz_bad)
numpy_compare.assert_float_equal(q.wxyz(), [1., 0, 0, 0])
# - Rotation.
R_bad = np.copy(R)
R_bad[0, 0] = 10
with self.assertRaises(RuntimeError):
q_other.set_rotation(R_bad)
numpy_compare.assert_float_equal(q_other.rotation(), R_I)
# Operations.
q = Quaternion(wxyz=[0.5, 0.5, 0.5, 0.5])
numpy_compare.assert_float_equal(q.multiply(position=[1, 2, 3]),
[3., 1, 2])
q_I = q.inverse().multiply(q)
numpy_compare.assert_float_equal(q_I.wxyz(), [1., 0, 0, 0])
if six.PY3:
numpy_compare.assert_float_equal(
eval("q.inverse() @ q").wxyz(), [1., 0, 0, 0])
q_conj = q.conjugate()
numpy_compare.assert_float_equal(q_conj.wxyz(),
[0.5, -0.5, -0.5, -0.5])
# Test `type_caster`s.
if T == float:
value = test_util.create_quaternion()
self.assertTrue(isinstance(value, mut.Quaternion))
test_util.check_quaternion(value)
def check_quaternion(self, T):
# Simple API.
Quaternion = mut.Quaternion_[T]
cast = np.vectorize(T)
q_identity = Quaternion()
self.assertEqual(npc.resolve_type(q_identity.wxyz()), T)
npc.assert_float_equal(q_identity.wxyz(), [1., 0, 0, 0])
npc.assert_float_equal(copy.copy(q_identity).wxyz(), [1., 0, 0, 0])
npc.assert_equal(q_identity.wxyz(), Quaternion.Identity().wxyz())
if T == float:
self.assertEqual(
str(q_identity),
"Quaternion_[float](w=1.0, x=0.0, y=0.0, z=0.0)")
# Test ordering.
q_wxyz = normalize([0.1, 0.3, 0.7, 0.9])
q = Quaternion(w=q_wxyz[0], x=q_wxyz[1], y=q_wxyz[2], z=q_wxyz[3])
# - Accessors.
npc.assert_float_equal(q.w(), q_wxyz[0])
npc.assert_float_equal(q.x(), q_wxyz[1])
npc.assert_float_equal(q.y(), q_wxyz[2])
npc.assert_float_equal(q.z(), q_wxyz[3])
npc.assert_float_equal(q.xyz(), q_wxyz[1:])
npc.assert_float_equal(q.wxyz(), q_wxyz)
# - Mutators.
q_wxyz_new = q_wxyz[::-1]
npc.assert_not_equal(q_wxyz, q_wxyz_new)
q.set_wxyz(wxyz=q_wxyz_new)
npc.assert_float_equal(q.wxyz(), q_wxyz_new)
q.set_wxyz(
w=q_wxyz_new[0], x=q_wxyz_new[1], y=q_wxyz_new[2], z=q_wxyz_new[3])
npc.assert_float_equal(q.wxyz(), q_wxyz_new)
# Alternative constructors.
q_other = Quaternion(wxyz=q_wxyz)
npc.assert_float_equal(q_other.wxyz(), q_wxyz)
R = np.array([
[0., 0, 1],
[1, 0, 0],
[0, 1, 0]])
q_wxyz_expected = np.array([0.5, 0.5, 0.5, 0.5])
q_other = Quaternion(q_wxyz_expected)
npc.assert_float_equal(q_other.rotation(), R)
R_I = np.eye(3, 3)
q_other.set_rotation(R_I)
npc.assert_equal(q_other.wxyz(), q_identity.wxyz())
# - Copy constructor.
cp = Quaternion(other=q)
npc.assert_equal(q.wxyz(), cp.wxyz())
# Bad values.
if T != Expression:
q = Quaternion.Identity()
# - wxyz
q_wxyz_bad = [1., 2, 3, 4]
with self.assertRaises(RuntimeError):
q.set_wxyz(q_wxyz_bad)
npc.assert_float_equal(q.wxyz(), [1., 0, 0, 0])
# - Rotation.
R_bad = np.copy(R)
R_bad[0, 0] = 10
with self.assertRaises(RuntimeError):
q_other.set_rotation(R_bad)
npc.assert_float_equal(q_other.rotation(), R_I)
# Operations.
q = Quaternion(wxyz=[0.5, 0.5, 0.5, 0.5])
npc.assert_float_equal(q.multiply(position=[1, 2, 3]), [3., 1, 2])
q_I = q.inverse().multiply(q)
npc.assert_float_equal(q_I.wxyz(), [1., 0, 0, 0])
if six.PY3:
npc.assert_float_equal(
eval("q.inverse() @ q").wxyz(), [1., 0, 0, 0])
q_conj = q.conjugate()
npc.assert_float_equal(q_conj.wxyz(), [0.5, -0.5, -0.5, -0.5])
# Test `type_caster`s.
if T == float:
value = test_util.create_quaternion()
self.assertTrue(isinstance(value, mut.Quaternion))
test_util.check_quaternion(value)
def check_isometry3(self, T):
Isometry3 = mut.Isometry3_[T]
# - Default constructor
transform = Isometry3()
self.assertEqual(npc.resolve_type(transform.matrix()), T)
X = np.eye(4, 4)
npc.assert_float_equal(transform.matrix(), X)
npc.assert_float_equal(copy.copy(transform).matrix(), X)
if T == float:
self.assertEqual(str(transform), str(X))
# - Constructor with (X)
transform = Isometry3(matrix=X)
npc.assert_float_equal(transform.matrix(), X)
# - Copy constructor.
cp = Isometry3(other=transform)
npc.assert_equal(transform.matrix(), cp.matrix())
# - Identity
transform = Isometry3.Identity()
npc.assert_float_equal(transform.matrix(), X)
# - Constructor with (R, p)
R = np.array([
[0., 1, 0],
[-1, 0, 0],
[0, 0, 1]])
p = np.array([1., 2, 3])
X = np.vstack((np.hstack((R, p.reshape((-1, 1)))), [0, 0, 0, 1]))
transform = Isometry3(rotation=R, translation=p)
npc.assert_float_equal(transform.matrix(), X)
npc.assert_float_equal(transform.translation(), p)
transform.set_translation(-p)
npc.assert_float_equal(transform.translation(), -p)
npc.assert_float_equal(transform.rotation(), R)
transform.set_rotation(R.T)
npc.assert_float_equal(transform.rotation(), R.T)
# - Check transactions for bad values.
if T != Expression:
transform = Isometry3(rotation=R, translation=p)
R_bad = np.copy(R)
R_bad[0, 0] = 10.
with self.assertRaises(RuntimeError):
transform.set_rotation(R_bad)
npc.assert_float_equal(transform.rotation(), R)
X_bad = np.copy(X)
X_bad[:3, :3] = R_bad
with self.assertRaises(RuntimeError):
transform.set_matrix(X_bad)
npc.assert_float_equal(transform.matrix(), X)
# Test `type_caster`s.
if T == float:
value = test_util.create_isometry()
self.assertTrue(isinstance(value, mut.Isometry3))
test_util.check_isometry(value)
# Operations.
transform = Isometry3(rotation=R, translation=p)
transform_I = transform.inverse().multiply(transform)
npc.assert_float_equal(transform_I.matrix(), np.eye(4))
npc.assert_float_equal(
transform.multiply(position=[10, 20, 30]), [21., -8, 33])
if six.PY3:
npc.assert_float_equal(
eval("transform.inverse() @ transform").matrix(), np.eye(4))
npc.assert_float_equal(
eval("transform @ [10, 20, 30]"), [21., -8, 33])
def test_quaternion(self, T):
# Simple API.
Quaternion = mut.Quaternion_[T]
cast = np.vectorize(T)
q_identity = Quaternion()
self.assertEqual(numpy_compare.resolve_type(q_identity.wxyz()), T)
numpy_compare.assert_float_equal(q_identity.wxyz(), [1., 0, 0, 0])
numpy_compare.assert_float_equal(
copy.copy(q_identity).wxyz(), [1., 0, 0, 0])
numpy_compare.assert_equal(q_identity.wxyz(),
Quaternion.Identity().wxyz())
if T == float:
self.assertEqual(str(q_identity),
"Quaternion_[float](w=1.0, x=0.0, y=0.0, z=0.0)")
self.check_cast(mut.Quaternion_, T)
# Test ordering.
q_wxyz = normalize([0.1, 0.3, 0.7, 0.9])
q = Quaternion(w=q_wxyz[0], x=q_wxyz[1], y=q_wxyz[2], z=q_wxyz[3])
# - Accessors.
numpy_compare.assert_float_equal(q.w(), q_wxyz[0])
numpy_compare.assert_float_equal(q.x(), q_wxyz[1])
numpy_compare.assert_float_equal(q.y(), q_wxyz[2])
numpy_compare.assert_float_equal(q.z(), q_wxyz[3])
numpy_compare.assert_float_equal(q.xyz(), q_wxyz[1:])
numpy_compare.assert_float_equal(q.wxyz(), q_wxyz)
# - Mutators.
q_wxyz_new = q_wxyz[::-1]
numpy_compare.assert_not_equal(q_wxyz, q_wxyz_new)
q.set_wxyz(wxyz=q_wxyz_new)
numpy_compare.assert_float_equal(q.wxyz(), q_wxyz_new)
q.set_wxyz(w=q_wxyz_new[0],
x=q_wxyz_new[1],
y=q_wxyz_new[2],
z=q_wxyz_new[3])
numpy_compare.assert_float_equal(q.wxyz(), q_wxyz_new)
# Alternative constructors.
q_other = Quaternion(wxyz=q_wxyz)
numpy_compare.assert_float_equal(q_other.wxyz(), q_wxyz)
R = np.array([[0., 0, 1], [1, 0, 0], [0, 1, 0]])
q_wxyz_expected = np.array([0.5, 0.5, 0.5, 0.5])
q_other = Quaternion(q_wxyz_expected)
numpy_compare.assert_float_equal(q_other.rotation(), R)
R_I = np.eye(3, 3)
q_other.set_rotation(R_I)
numpy_compare.assert_equal(q_other.wxyz(), q_identity.wxyz())
# - Copy constructor.
cp = Quaternion(other=q)
numpy_compare.assert_equal(q.wxyz(), cp.wxyz())
# Bad values.
if T != Expression:
q = Quaternion.Identity()
# - wxyz
q_wxyz_bad = [1., 2, 3, 4]
with self.assertRaises(RuntimeError):
q.set_wxyz(q_wxyz_bad)
numpy_compare.assert_float_equal(q.wxyz(), [1., 0, 0, 0])
# - Rotation.
R_bad = np.copy(R)
R_bad[0, 0] = 10
with self.assertRaises(RuntimeError):
q_other.set_rotation(R_bad)
numpy_compare.assert_float_equal(q_other.rotation(), R_I)
# Operations.
q_AB = Quaternion(wxyz=[0.5, 0.5, 0.5, 0.5])
q_I = q_AB.inverse().multiply(q_AB)
numpy_compare.assert_float_equal(q_I.wxyz(), [1., 0, 0, 0])
numpy_compare.assert_float_equal((q_AB.inverse() @ q_AB).wxyz(),
[1., 0, 0, 0])
v_B = np.array([1., 2, 3])
v_A = np.array([3., 1, 2])
numpy_compare.assert_float_allclose(q_AB.multiply(vector=v_B), v_A)
vlist_B = np.array([v_B, v_B]).T
vlist_A = np.array([v_A, v_A]).T
numpy_compare.assert_float_equal(q_AB.multiply(vector=vlist_B),
vlist_A)
q_AB_conj = q_AB.conjugate()
numpy_compare.assert_float_equal(q_AB_conj.wxyz(),
[0.5, -0.5, -0.5, -0.5])
numpy_compare.assert_float_equal(
q_I.slerp(t=0, other=q_I).wxyz(), [1., 0, 0, 0])
# - Test shaping (#13885).
v = np.array([0., 0., 0.])
vs = np.array([[1., 2., 3.], [4., 5., 6.]]).T
self.assertEqual((q_AB @ v).shape, (3, ))
self.assertEqual((q_AB @ v.reshape((3, 1))).shape, (3, 1))
self.assertEqual((q_AB @ vs).shape, (3, 2))
# Test `type_caster`s.
if T == float:
value = test_util.create_quaternion()
self.assertTrue(isinstance(value, mut.Quaternion))
test_util.check_quaternion(value)
assert_pickle(self, q_AB, Quaternion.wxyz, T=T)
