def test_functions_with_float(self):
# TODO(eric.cousineau): Use concrete values once vectorized methods are
# supported.
v_x = 1.0
v_y = 1.0
npc.assert_equal(sym.abs(v_x), np.abs(v_x))
npc.assert_not_equal(sym.abs(v_x), 0.5*np.abs(v_x))
npc.assert_equal(sym.abs(v_x), np.abs(v_x))
npc.assert_equal(sym.abs(v_x), np.abs(v_x))
npc.assert_equal(sym.exp(v_x), np.exp(v_x))
npc.assert_equal(sym.sqrt(v_x), np.sqrt(v_x))
npc.assert_equal(sym.pow(v_x, v_y), v_x ** v_y)
npc.assert_equal(sym.sin(v_x), np.sin(v_x))
npc.assert_equal(sym.cos(v_x), np.cos(v_x))
npc.assert_equal(sym.tan(v_x), np.tan(v_x))
npc.assert_equal(sym.asin(v_x), np.arcsin(v_x))
npc.assert_equal(sym.acos(v_x), np.arccos(v_x))
npc.assert_equal(sym.atan(v_x), np.arctan(v_x))
npc.assert_equal(sym.atan2(v_x, v_y), np.arctan2(v_x, v_y))
npc.assert_equal(sym.sinh(v_x), np.sinh(v_x))
npc.assert_equal(sym.cosh(v_x), np.cosh(v_x))
npc.assert_equal(sym.tanh(v_x), np.tanh(v_x))
npc.assert_equal(sym.min(v_x, v_y), min(v_x, v_y))
npc.assert_equal(sym.max(v_x, v_y), max(v_x, v_y))
npc.assert_equal(sym.ceil(v_x), np.ceil(v_x))
npc.assert_equal(sym.floor(v_x), np.floor(v_x))
npc.assert_equal(
sym.if_then_else(
sym.Expression(v_x) > sym.Expression(v_y),
v_x, v_y),
v_x if v_x > v_y else v_y)
def test_functions_with_float(self):
# TODO(eric.cousineau): Use concrete values once vectorized methods are
# supported.
v_x = 1.0
v_y = 1.0
numpy_compare.assert_equal(sym.abs(v_x), np.abs(v_x))
numpy_compare.assert_not_equal(sym.abs(v_x), 0.5 * np.abs(v_x))
numpy_compare.assert_equal(sym.abs(v_x), np.abs(v_x))
numpy_compare.assert_equal(sym.abs(v_x), np.abs(v_x))
numpy_compare.assert_equal(sym.exp(v_x), np.exp(v_x))
numpy_compare.assert_equal(sym.sqrt(v_x), np.sqrt(v_x))
numpy_compare.assert_equal(sym.pow(v_x, v_y), v_x**v_y)
numpy_compare.assert_equal(sym.sin(v_x), np.sin(v_x))
numpy_compare.assert_equal(sym.cos(v_x), np.cos(v_x))
numpy_compare.assert_equal(sym.tan(v_x), np.tan(v_x))
numpy_compare.assert_equal(sym.asin(v_x), np.arcsin(v_x))
numpy_compare.assert_equal(sym.acos(v_x), np.arccos(v_x))
numpy_compare.assert_equal(sym.atan(v_x), np.arctan(v_x))
numpy_compare.assert_equal(sym.atan2(v_x, v_y), np.arctan2(v_x, v_y))
numpy_compare.assert_equal(sym.sinh(v_x), np.sinh(v_x))
numpy_compare.assert_equal(sym.cosh(v_x), np.cosh(v_x))
numpy_compare.assert_equal(sym.tanh(v_x), np.tanh(v_x))
numpy_compare.assert_equal(sym.min(v_x, v_y), min(v_x, v_y))
numpy_compare.assert_equal(sym.max(v_x, v_y), max(v_x, v_y))
numpy_compare.assert_equal(sym.ceil(v_x), np.ceil(v_x))
numpy_compare.assert_equal(sym.floor(v_x), np.floor(v_x))
numpy_compare.assert_equal(
sym.if_then_else(
sym.Expression(v_x) > sym.Expression(v_y), v_x, v_y),
v_x if v_x > v_y else v_y)
def test_hash(self):
p1 = sym.Polynomial(x * x, [x])
p2 = sym.Polynomial(x * x, [x])
npc.assert_equal(p1, p2)
self.assertEqual(hash(p1), hash(p2))
p1 += 1
npc.assert_not_equal(p1, p2)
self.assertNotEqual(hash(p1), hash(p2))
def test_asserts_autodiff(self):
# Test only scalar; other cases are handled by above test case.
a = AutoDiffXd(1., [1., 0.])
b = AutoDiffXd(1., [0., 1.])
c = AutoDiffXd(2., [3., 4.])
numpy_compare.assert_equal(a, a)
numpy_compare.assert_not_equal(a, b)
numpy_compare.assert_not_equal(a, c)
def test_hash(self):
p1 = sym.Polynomial(x * x, [x])
p2 = sym.Polynomial(x * x, [x])
numpy_compare.assert_equal(p1, p2)
self.assertEqual(hash(p1), hash(p2))
p1 += 1
numpy_compare.assert_not_equal(p1, p2)
self.assertNotEqual(hash(p1), hash(p2))
def test_asserts_autodiff(self):
# Test only scalar; other cases are handled by above test case.
a = AutoDiffXd(1., [1., 0.])
b = AutoDiffXd(1., [0., 1.])
c = AutoDiffXd(2., [3., 4.])
npc.assert_equal(a, a)
npc.assert_not_equal(a, b)
npc.assert_not_equal(a, c)
def test_comparison(self):
p = sym.Polynomial()
npc.assert_equal(p, p)
self.assertIsInstance(p == p, sym.Formula)
self.assertEqual(p == p, sym.Formula.True_())
self.assertTrue(p.EqualTo(p))
q = sym.Polynomial(sym.Expression(10))
npc.assert_not_equal(p, q)
self.assertIsInstance(p != q, sym.Formula)
self.assertEqual(p != q, sym.Formula.True_())
self.assertFalse(p.EqualTo(q))
def test_comparison(self):
p = sym.Polynomial()
numpy_compare.assert_equal(p, p)
self.assertIsInstance(p == p, sym.Formula)
self.assertEqual(p == p, sym.Formula.True_())
self.assertTrue(p.EqualTo(p))
q = sym.Polynomial(sym.Expression(10))
numpy_compare.assert_not_equal(p, q)
self.assertIsInstance(p != q, sym.Formula)
self.assertEqual(p != q, sym.Formula.True_())
self.assertFalse(p.EqualTo(q))
def test_comparison(self):
p = sym.Polynomial()
numpy_compare.assert_equal(p, p)
self.assertIsInstance(p == p, sym.Formula)
self.assertEqual(p == p, sym.Formula.True_())
self.assertTrue(p.EqualTo(p))
q = sym.Polynomial(sym.Expression(10))
numpy_compare.assert_not_equal(p, q)
self.assertIsInstance(p != q, sym.Formula)
self.assertEqual(p != q, sym.Formula.True_())
self.assertFalse(p.EqualTo(q))
self.assertTrue(
p.CoefficientsAlmostEqual(p + sym.Polynomial(1e-7), 1e-6))
self.assertTrue(
p.CoefficientsAlmostEqual(p + sym.Polynomial(1e-7 * x), 1e-6))
self.assertFalse(
p.CoefficientsAlmostEqual(p + sym.Polynomial(2e-6 * x), 1e-6))
def test_asserts_builtin(self):
a = 1.
b = 0.
# Scalar.
npc.assert_equal(a, a)
with self.assertRaises(AssertionError):
npc.assert_equal(a, b)
npc.assert_not_equal(a, b)
with self.assertRaises(AssertionError):
npc.assert_not_equal(a, a)
# Array.
A = np.array([a, a])
C = np.array([1., 2.])
npc.assert_equal(A, a)
npc.assert_equal(C, C)
with self.assertRaises(AssertionError):
npc.assert_equal(A, b)
npc.assert_not_equal(A, A + [0, 0.1])
npc.assert_not_equal(A, b)
with self.assertRaises(AssertionError):
npc.assert_not_equal(C, C)
def test_asserts_builtin(self):
a = 1.
b = 0.
# Scalar.
numpy_compare.assert_equal(a, a)
with self.assertRaises(AssertionError):
numpy_compare.assert_equal(a, b)
numpy_compare.assert_not_equal(a, b)
with self.assertRaises(AssertionError):
numpy_compare.assert_not_equal(a, a)
# Array.
A = np.array([a, a])
C = np.array([1., 2.])
numpy_compare.assert_equal(A, a)
numpy_compare.assert_equal(C, C)
with self.assertRaises(AssertionError):
numpy_compare.assert_equal(A, b)
numpy_compare.assert_not_equal(A, A + [0, 0.1])
numpy_compare.assert_not_equal(A, b)
with self.assertRaises(AssertionError):
numpy_compare.assert_not_equal(C, C)
def test_asserts_symbolic(self):
x = Variable("x")
y = Variable("y")
e = x + y
numpy_compare.assert_equal(x, x)
numpy_compare.assert_equal(x, "x")
numpy_compare.assert_not_equal(x, y)
numpy_compare.assert_equal(e, x + y)
numpy_compare.assert_equal(e, "(x + y)")
numpy_compare.assert_not_equal(e, x - y)
numpy_compare.assert_not_equal(e, "(x - y)")
numpy_compare.assert_equal(Formula.True_(), True)
numpy_compare.assert_equal(Formula.False_(), False)
numpy_compare.assert_not_equal(Formula.True_(), False)
def test_asserts_symbolic(self):
x = Variable("x")
y = Variable("y")
e = x + y
numpy_compare.assert_equal(x, x)
numpy_compare.assert_equal(x, "x")
numpy_compare.assert_not_equal(x, y)
numpy_compare.assert_equal(e, x + y)
numpy_compare.assert_equal(e, "(x + y)")
numpy_compare.assert_not_equal(e, x - y)
numpy_compare.assert_not_equal(e, "(x - y)")
numpy_compare.assert_equal(Formula.True_(), True)
numpy_compare.assert_equal(Formula.False_(), False)
numpy_compare.assert_not_equal(Formula.True_(), False)
with self.assertRaises(AssertionError):
numpy_compare.assert_allclose(x, x)
def test_asserts_symbolic(self):
x = Variable("x")
y = Variable("y")
e = x + y
npc.assert_equal(x, x)
npc.assert_equal(x, "x")
npc.assert_not_equal(x, y)
npc.assert_equal(e, x + y)
npc.assert_equal(e, "(x + y)")
npc.assert_not_equal(e, x - y)
npc.assert_not_equal(e, "(x - y)")
def test_asserts_symbolic(self):
x = Variable("x")
y = Variable("y")
e = x + y
npc.assert_equal(x, x)
npc.assert_equal(x, "x")
npc.assert_not_equal(x, y)
npc.assert_equal(e, x + y)
npc.assert_equal(e, "(x + y)")
npc.assert_not_equal(e, x - y)
npc.assert_not_equal(e, "(x - y)")
def test_asserts_custom(self):
a = Custom("a")
b = Custom("b")
# Scalar.
npc.assert_equal(a, a)
npc.assert_equal(a, "a")
with self.assertRaises(AssertionError):
npc.assert_equal(a, b)
npc.assert_not_equal(a, b)
with self.assertRaises(AssertionError):
npc.assert_not_equal(a, a)
with self.assertRaises(AssertionError):
npc.assert_not_equal(a, "a")
# Array.
A = np.array([a, a])
C = np.array([Custom("c0"), Custom("c1")])
npc.assert_equal(A, a)
npc.assert_equal(A, "a")
npc.assert_equal(C, C)
npc.assert_equal(C, ["c0", "c1"])
with self.assertRaises(AssertionError):
npc.assert_equal(A, b)
npc.assert_not_equal(A, [a, b])
npc.assert_not_equal(A, ["a", "b"])
with self.assertRaises(AssertionError):
npc.assert_not_equal(C, C)
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 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 test_asserts_custom(self):
a = Custom("a")
b = Custom("b")
# Scalar.
numpy_compare.assert_equal(a, a)
numpy_compare.assert_equal(a, "a")
with self.assertRaises(AssertionError):
numpy_compare.assert_equal(a, b)
numpy_compare.assert_not_equal(a, b)
with self.assertRaises(AssertionError):
numpy_compare.assert_not_equal(a, a)
with self.assertRaises(AssertionError):
numpy_compare.assert_not_equal(a, "a")
# Array.
A = np.array([a, a])
C = np.array([Custom("c0"), Custom("c1")])
numpy_compare.assert_equal(A, a)
numpy_compare.assert_equal(A, "a")
numpy_compare.assert_equal(C, C)
numpy_compare.assert_equal(C, ["c0", "c1"])
with self.assertRaises(AssertionError):
numpy_compare.assert_equal(A, b)
numpy_compare.assert_not_equal(A, [a, b])
numpy_compare.assert_not_equal(A, ["a", "b"])
with self.assertRaises(AssertionError):
numpy_compare.assert_not_equal(C, C)
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_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)
