def test_addition(self):
p = sym.Polynomial()
npc.assert_equal(p + p, p)
m = sym.Monomial(x)
npc.assert_equal(m + p, sym.Polynomial(1 * x))
npc.assert_equal(p + m, sym.Polynomial(1 * x))
npc.assert_equal(p + 0, p)
npc.assert_equal(0 + p, p)
def test_subtraction(self):
p = sym.Polynomial()
npc.assert_equal(p - p, p)
m = sym.Monomial(x)
npc.assert_equal(m - p, sym.Polynomial(1 * x))
npc.assert_equal(p - m, sym.Polynomial(-1 * x))
npc.assert_equal(p - 0, p)
npc.assert_equal(0 - p, -p)
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_multiplication(self):
p = sym.Polynomial()
npc.assert_equal(p * p, p)
m = sym.Monomial(x)
npc.assert_equal(m * p, p)
npc.assert_equal(p * m, p)
npc.assert_equal(p * 0, p)
npc.assert_equal(0 * p, p)
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_jacobian(self):
e = 5 * x ** 2 + 4 * y ** 2 + 8 * x * y
p = sym.Polynomial(e, [x, y]) # p = 5x² + 4y² + 8xy
p_dx = sym.Polynomial(10 * x + 8 * y, [x, y]) # ∂p/∂x = 10x + 8y
p_dy = sym.Polynomial(8 * y + 8 * x, [x, y]) # ∂p/∂y = 8y + 8x
J = p.Jacobian([x, y])
npc.assert_equal(J[0], p_dx)
npc.assert_equal(J[1], p_dy)
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_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_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_array_api(self):
a = AD(1, [1., 0])
b = AD(2, [0, 1.])
x = np.array([a, b])
self.assertEqual(x.dtype, object)
# Idempotent check.
npc.assert_equal(x, x)
# Conversion.
with self.assertRaises(TypeError):
# Avoid implicit coercion, as this will imply information loss.
xf = np.zeros(2, dtype=np.float)
xf[:] = x
with self.assertRaises(TypeError):
# We could define `__float__` to allow this, but then that will
# enable implicit coercion, which we should avoid.
xf = x.astype(dtype=np.float)
# Presently, does not convert.
x = np.zeros((3, 3), dtype=AD)
self.assertFalse(isinstance(x[0, 0], AD))
x = np.eye(3).astype(AD)
self.assertFalse(isinstance(x[0, 0], AD))
# Test implicit conversion.
npc.assert_equal(
autodiff_vector_pass_through([1, 2]), # int
[AD(1., []), AD(2., [])])
npc.assert_equal(
autodiff_vector_pass_through([1., 2.]), # float
[AD(1., []), AD(2., [])])
def test_linear_algebra(self):
a_scalar = AD(1, [1., 0])
b_scalar = AD(2, [0, 1.])
A = np.array([[a_scalar, a_scalar]])
B = np.array([[b_scalar, b_scalar]]).T
C = np.dot(A, B)
npc.assert_equal(C, [[AD(4, [4., 2])]])
# `matmul` not supported for `dtype=object` (#11332). `np.dot` should
# be used instead.
with self.assertRaises(TypeError):
C2 = np.matmul(A, B)
# Type mixing
Bf = np.array([[2., 2]]).T
C2 = np.dot(A, Bf) # Leverages implicit casting.
npc.assert_equal(C2, [[AD(4, [4., 0])]])
# Other methods.
X = np.array([[a_scalar, b_scalar], [b_scalar, a_scalar]])
npc.assert_equal(np.trace(X), AD(2, [2., 0]))
# `inv` is a ufunc that we must implement, if possible. However, given
# that this is currently `dtype=object`, it would be extremely unwise
# to do so. See #8116 for alternative.
with self.assertRaises(TypeError):
Y = np.linalg.inv(X)
# Use workaround for inverse. For now, just check values.
X_float = npc.to_float(X)
Xinv_float = np.linalg.inv(X_float)
Xinv = drake_math.inv(X)
np.testing.assert_equal(npc.to_float(Xinv), Xinv_float)
def test_addition_assignment(self):
p = sym.Polynomial()
p += p
npc.assert_equal(p, sym.Polynomial())
p += sym.Monomial(x)
npc.assert_equal(p, sym.Polynomial(1 * x))
p += 3
npc.assert_equal(p, sym.Polynomial(3 + 1 * x))
def test_subtraction_assignment(self):
p = sym.Polynomial()
p -= p
npc.assert_equal(p, sym.Polynomial())
p -= sym.Monomial(x)
npc.assert_equal(p, sym.Polynomial(-1 * x))
p -= 3
npc.assert_equal(p, sym.Polynomial(-1 * x - 3))
def test_multiplication_assignment(self):
p = sym.Polynomial()
p *= p
npc.assert_equal(p, sym.Polynomial())
p *= sym.Monomial(x)
npc.assert_equal(p, sym.Polynomial())
p *= 3
npc.assert_equal(p, sym.Polynomial())
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 check_logical(func, a, b, expected):
# Checks logical operations, with broadcasting, checking that `a` and `b`
# (of type `T`) have compatible logical operators when the left or right
# operands are `float`s. Specifically, tests:
# - f(T, T)
# - f(T, float)
# - f(float, T)
npc.assert_equal(func(a, b), expected)
af = npc.to_float(a)
bf = npc.to_float(b)
npc.assert_equal(func(a, bf), expected)
npc.assert_equal(func(af, b), expected)
def test_expand(self):
ex = 2 * (x + y)
numpy_compare.assert_equal(ex, "(2 * (x + y))")
numpy_compare.assert_equal(ex.Expand(), "(2 * x + 2 * y)")
def test_remove_terms_with_small_coefficients(self):
e = 3 * x + 1e-12 * y
p = sym.Polynomial(e, [x, y])
q = p.RemoveTermsWithSmallCoefficients(1e-6)
numpy_compare.assert_equal(q.ToExpression(), 3 * x)
def test_subtraction(self):
p = sym.Polynomial(0.0, [x])
numpy_compare.assert_equal(p - p, p)
m = sym.Monomial(x)
numpy_compare.assert_equal(m - p, sym.Polynomial(1 * x))
numpy_compare.assert_equal(p - m, sym.Polynomial(-1 * x))
numpy_compare.assert_equal(p - 0, p)
numpy_compare.assert_equal(0 - p, -p)
numpy_compare.assert_equal(x - p, sym.Polynomial(x))
numpy_compare.assert_equal(p - x, sym.Polynomial(-x))
def test_to_expression(self):
m = sym.Monomial(x, 3) * sym.Monomial(y) # m = x³y
e = m.ToExpression()
npc.assert_equal(e, "(pow(x, 3) * y)")
def test_simplify(self):
numpy_compare.assert_equal(0 * (x + y), "0")
numpy_compare.assert_equal(x + y - x - y, "0")
numpy_compare.assert_equal(x / x - 1, "0")
numpy_compare.assert_equal(x / x, "1")
def test_non_method_jacobian(self):
# Jacobian([x * cos(y), x * sin(y), x ** 2], [x, y]) returns
# the following 3x2 matrix:
#
# = |cos(y) -x * sin(y)|
# |sin(y) x * cos(y)|
# | 2 * x 0|
J = sym.Jacobian([x * sym.cos(y), x * sym.sin(y), x**2], [x, y])
numpy_compare.assert_equal(J[0, 0], sym.cos(y))
numpy_compare.assert_equal(J[1, 0], sym.sin(y))
numpy_compare.assert_equal(J[2, 0], 2 * x)
numpy_compare.assert_equal(J[0, 1], -x * sym.sin(y))
numpy_compare.assert_equal(J[1, 1], x * sym.cos(y))
numpy_compare.assert_equal(J[2, 1], sym.Expression(0))
def test_differentiate(self):
e = x * x
numpy_compare.assert_equal(e.Differentiate(x), 2 * x)
def test_constructor_expression(self):
e = 2 * x + 3 * y
p = sym.Polynomial(e)
npc.assert_equal(p.ToExpression(), e)
def test_substitute_with_dict(self):
e = x + y
env = {x: x + 2, y: y + 3}
numpy_compare.assert_equal(e.Substitute(env), x + y + 5)
def test_str(self):
m1 = sym.Monomial(x, 2)
npc.assert_equal(m1, "x^2")
m2 = m1 * sym.Monomial(y)
npc.assert_equal(m2, "x^2 * y")
def test_monomial_to_coefficient_map(self):
m = sym.Monomial(x, 2)
e = a * (x ** 2)
p = sym.Polynomial(e, [x])
the_map = p.monomial_to_coefficient_map()
npc.assert_equal(the_map[m], a)
def test_differentiate(self):
e = a * (x ** 2)
p = sym.Polynomial(e, [x]) # p = ax²
result = p.Differentiate(x) # = 2ax
npc.assert_equal(result.ToExpression(), 2 * a * x)
def test_add_product(self):
p = sym.Polynomial()
m = sym.Monomial(x)
p.AddProduct(sym.Expression(3), m) # p += 3 * x
npc.assert_equal(p.ToExpression(), 3 * x)
def test_simplify(self):
npc.assert_equal(0 * (x + y), "0")
npc.assert_equal(x + y - x - y, "0")
npc.assert_equal(x / x - 1, "0")
npc.assert_equal(x / x, "1")
def test_constructor_maptype(self):
m = {sym.Monomial(x): sym.Expression(3),
sym.Monomial(y): sym.Expression(2)} # 3x + 2y
p = sym.Polynomial(m)
expected = 3 * x + 2 * y
npc.assert_equal(p.ToExpression(), expected)
def test_str(self):
m1 = sym.Monomial(x, 2)
numpy_compare.assert_equal(m1, "x^2")
m2 = m1 * sym.Monomial(y)
numpy_compare.assert_equal(m2, "x^2 * y")
def test_relational_operators(self):
# Variable rop float
numpy_compare.assert_equal(x >= 1, "(x >= 1)")
numpy_compare.assert_equal(x > 1, "(x > 1)")
numpy_compare.assert_equal(x <= 1, "(x <= 1)")
numpy_compare.assert_equal(x < 1, "(x < 1)")
numpy_compare.assert_equal(x == 1, "(x == 1)")
numpy_compare.assert_equal(x != 1, "(x != 1)")
# float rop Variable
numpy_compare.assert_equal(1 < y, "(y > 1)")
numpy_compare.assert_equal(1 <= y, "(y >= 1)")
numpy_compare.assert_equal(1 > y, "(y < 1)")
numpy_compare.assert_equal(1 >= y, "(y <= 1)")
numpy_compare.assert_equal(1 == y, "(y == 1)")
numpy_compare.assert_equal(1 != y, "(y != 1)")
# Variable rop Variable
numpy_compare.assert_equal(x < y, "(x < y)")
numpy_compare.assert_equal(x <= y, "(x <= y)")
numpy_compare.assert_equal(x > y, "(x > y)")
numpy_compare.assert_equal(x >= y, "(x >= y)")
numpy_compare.assert_equal(x == y, "(x == y)")
numpy_compare.assert_equal(x != y, "(x != y)")
def test_default_constructor(self):
p = sym.Polynomial()
numpy_compare.assert_equal(p.ToExpression(), sym.Expression())
def test_method_jacobian(self):
# (x * cos(y)).Jacobian([x, y]) returns [cos(y), -x * sin(y)].
J = (x * sym.cos(y)).Jacobian([x, y])
numpy_compare.assert_equal(J[0], sym.cos(y))
numpy_compare.assert_equal(J[1], -x * sym.sin(y))
def test_monomial_to_coefficient_map(self):
m = sym.Monomial(x, 2)
e = a * (x**2)
p = sym.Polynomial(e, [x])
the_map = p.monomial_to_coefficient_map()
numpy_compare.assert_equal(the_map[m], a)
def test_substitute_with_pair(self):
e = x + y
numpy_compare.assert_equal(e.Substitute(x, x + 5), x + y + 5)
numpy_compare.assert_equal(e.Substitute(y, z), x + z)
numpy_compare.assert_equal(e.Substitute(y, 3), x + 3)
def test_remove_terms_with_small_coefficients(self):
e = 3 * x + 1e-12 * y
p = sym.Polynomial(e, [x, y])
q = p.RemoveTermsWithSmallCoefficients(1e-6)
npc.assert_equal(q.ToExpression(), 3 * x)
def test_copy(self):
numpy_compare.assert_equal(copy.copy(e_x), e_x)
numpy_compare.assert_equal(copy.deepcopy(e_x), e_x)
def test_expand(self):
ex = 2 * (x + y)
npc.assert_equal(ex, "(2 * (x + y))")
npc.assert_equal(ex.Expand(), "(2 * x + 2 * y)")
def test_to_expression(self):
m = sym.Monomial(x, 3) * sym.Monomial(y) # m = x³y
e = m.ToExpression()
numpy_compare.assert_equal(e, "(pow(x, 3) * y)")
def test_pow(self):
e = a * (x ** 2)
p = sym.Polynomial(e, [x]) # p = ax²
p = pow(p, 2) # p = a²x⁴
npc.assert_equal(p.ToExpression(), (a ** 2) * (x ** 4))
def test_constructor_expression(self):
e = 2 * x + 3 * y
p = sym.Polynomial(e)
numpy_compare.assert_equal(p.ToExpression(), e)
def test_pow(self):
e = a * (x**2)
p = sym.Polynomial(e, [x]) # p = ax²
p = pow(p, 2) # p = a²x⁴
numpy_compare.assert_equal(p.ToExpression(), (a**2) * (x**4))
def test_differentiate(self):
e = a * (x**2)
p = sym.Polynomial(e, [x]) # p = ax²
result = p.Differentiate(x) # = 2ax
numpy_compare.assert_equal(result.ToExpression(), 2 * a * x)
def _assert_equal(test, a, b):
if isinstance(a, np.ndarray):
numpy_compare.assert_equal(a, b)
else:
test.assertEqual(a, b)
def test_add_product(self):
p = sym.Polynomial()
m = sym.Monomial(x)
p.AddProduct(sym.Expression(3), m) # p += 3 * x
numpy_compare.assert_equal(p.ToExpression(), 3 * x)
def _check_algebra(self, algebra):
a_scalar = AD(1, [1., 0])
b_scalar = AD(2, [0, 1.])
c_scalar = AD(0, [1., 0])
d_scalar = AD(1, [0, 1.])
a, b, c, d = map(
algebra.to_algebra, (a_scalar, b_scalar, c_scalar, d_scalar))
# Arithmetic
numpy_compare.assert_equal(-a, AD(-1, [-1., 0]))
numpy_compare.assert_equal(a + b, AD(3, [1, 1]))
numpy_compare.assert_equal(a + 1, AD(2, [1, 0]))
numpy_compare.assert_equal(1 + a, AD(2, [1, 0]))
numpy_compare.assert_equal(a - b, AD(-1, [1, -1]))
numpy_compare.assert_equal(a - 1, AD(0, [1, 0]))
numpy_compare.assert_equal(1 - a, AD(0, [-1, 0]))
numpy_compare.assert_equal(a * b, AD(2, [2, 1]))
numpy_compare.assert_equal(a * 2, AD(2, [2, 0]))
numpy_compare.assert_equal(2 * a, AD(2, [2, 0]))
numpy_compare.assert_equal(a / b, AD(1./2, [1./2, -1./4]))
numpy_compare.assert_equal(a / 2, AD(0.5, [0.5, 0]))
numpy_compare.assert_equal(2 / a, AD(2, [-2, 0]))
# Logical
check_logical(lambda x, y: x == y, a, a, True)
check_logical(algebra.eq, a, a, True)
check_logical(lambda x, y: x != y, a, a, False)
check_logical(algebra.ne, a, a, False)
check_logical(lambda x, y: x < y, a, b, True)
check_logical(algebra.lt, a, b, True)
check_logical(lambda x, y: x <= y, a, b, True)
check_logical(algebra.le, a, b, True)
check_logical(lambda x, y: x > y, a, b, False)
check_logical(algebra.gt, a, b, False)
check_logical(lambda x, y: x >= y, a, b, False)
check_logical(algebra.ge, a, b, False)
# Additional math
# - See `math_overloads_test` for scalar overloads.
numpy_compare.assert_equal(a**2, AD(1, [2., 0]))
numpy_compare.assert_equal(algebra.log(a), AD(0, [1., 0]))
numpy_compare.assert_equal(algebra.abs(-a), AD(1, [1., 0]))
numpy_compare.assert_equal(algebra.exp(a), AD(np.e, [np.e, 0]))
numpy_compare.assert_equal(algebra.sqrt(a), AD(1, [0.5, 0]))
numpy_compare.assert_equal(algebra.pow(a, 2), AD(1, [2., 0]))
numpy_compare.assert_equal(algebra.pow(a, 0.5), AD(1, [0.5, 0]))
numpy_compare.assert_equal(algebra.sin(c), AD(0, [1, 0]))
numpy_compare.assert_equal(algebra.cos(c), AD(1, [0, 0]))
numpy_compare.assert_equal(algebra.tan(c), AD(0, [1, 0]))
numpy_compare.assert_equal(algebra.arcsin(c), AD(0, [1, 0]))
numpy_compare.assert_equal(algebra.arccos(c), AD(np.pi / 2, [-1, 0]))
numpy_compare.assert_equal(algebra.arctan2(c, d), AD(0, [1, 0]))
numpy_compare.assert_equal(algebra.sinh(c), AD(0, [1, 0]))
numpy_compare.assert_equal(algebra.cosh(c), AD(1, [0, 0]))
numpy_compare.assert_equal(algebra.tanh(c), AD(0, [1, 0]))
numpy_compare.assert_equal(algebra.min(a, b), a_scalar)
numpy_compare.assert_equal(algebra.max(a, b), b_scalar)
# Because `ceil` and `floor` return `double`, we have to special case
# this comparison since the matrix is `dtype=object`, even though the
# elements are all doubles. We must cast it to float.
# N.B. This would be fixed if we registered a UFunc for these
# methods, so NumPy would have already returned a `float` array.
ceil_a = algebra.ceil(a)
floor_a = algebra.floor(a)
if isinstance(algebra, VectorizedAlgebra):
self.assertEqual(ceil_a.dtype, object)
self.assertIsInstance(ceil_a[0], float)
ceil_a = ceil_a.astype(float)
floor_a = floor_a.astype(float)
numpy_compare.assert_equal(ceil_a, a_scalar.value())
numpy_compare.assert_equal(floor_a, a_scalar.value())
# Return value so it can be inspected.
return a
def test_addition(self):
p = sym.Polynomial(0.0, [x])
numpy_compare.assert_equal(p + p, p)
m = sym.Monomial(x)
numpy_compare.assert_equal(m + p, sym.Polynomial(1 * x))
numpy_compare.assert_equal(p + m, sym.Polynomial(1 * x))
numpy_compare.assert_equal(p + 0, p)
numpy_compare.assert_equal(0 + p, p)
numpy_compare.assert_equal(x + p, sym.Polynomial(x) + p)
numpy_compare.assert_equal(p + x, p + sym.Polynomial(x))
def test_unary_operators(self):
numpy_compare.assert_equal(+x, "x")
numpy_compare.assert_equal(-x, "(-1 * x)")
def test_multiplication(self):
p = sym.Polynomial(0.0, [x])
numpy_compare.assert_equal(p * p, p)
m = sym.Monomial(x)
numpy_compare.assert_equal(m * p, p)
numpy_compare.assert_equal(p * m, p)
numpy_compare.assert_equal(p * 0, p)
numpy_compare.assert_equal(0 * p, p)
numpy_compare.assert_equal(
sym.Polynomial(x) * x, sym.Polynomial(x * x))
numpy_compare.assert_equal(x * sym.Polynomial(x),
sym.Polynomial(x * x))
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_division(self):
p = sym.Polynomial(x * x + x)
numpy_compare.assert_equal(p / 2,
sym.Polynomial(1 / 2 * x * x + 1 / 2 * x))
def test_default_constructor(self):
p = sym.Polynomial()
npc.assert_equal(p.ToExpression(), sym.Expression())
def test_pow(self):
numpy_compare.assert_equal(x**2, "pow(x, 2)")
numpy_compare.assert_equal(x**y, "pow(x, y)")
numpy_compare.assert_equal((x + 1)**(y - 1), "pow((1 + x), (-1 + y))")
def test_matrix_substitute_with_substitution(self):
m = np.array([[x + y, x * y]])
env = {x: x + 2, y: y + 3}
substituted = sym.Substitute(m, env)
npc.assert_equal(substituted[0, 0], m[0, 0].Substitute(env))
npc.assert_equal(substituted[0, 1], m[0, 1].Substitute(env))
def test_rotation_matrix(self, T):
# - Constructors.
RotationMatrix = mut.RotationMatrix_[T]
AngleAxis = AngleAxis_[T]
Quaternion = Quaternion_[T]
RollPitchYaw = mut.RollPitchYaw_[T]
R = RotationMatrix()
numpy_compare.assert_float_equal(
RotationMatrix(other=R).matrix(), np.eye(3))
numpy_compare.assert_float_equal(R.matrix(), np.eye(3))
numpy_compare.assert_float_equal(copy.copy(R).matrix(), np.eye(3))
numpy_compare.assert_float_equal(
RotationMatrix.Identity().matrix(), np.eye(3))
R = RotationMatrix(R=np.eye(3))
numpy_compare.assert_float_equal(R.matrix(), np.eye(3))
R = RotationMatrix(quaternion=Quaternion.Identity())
numpy_compare.assert_float_equal(R.matrix(), np.eye(3))
R = RotationMatrix(theta_lambda=AngleAxis(angle=0, axis=[0, 0, 1]))
numpy_compare.assert_float_equal(R.matrix(), np.eye(3))
R = RotationMatrix(rpy=RollPitchYaw(rpy=[0, 0, 0]))
numpy_compare.assert_float_equal(R.matrix(), np.eye(3))
# One axis RotationMatrices
R = RotationMatrix.MakeXRotation(theta=0)
numpy_compare.assert_float_equal(R.matrix(), np.eye(3))
R = RotationMatrix.MakeYRotation(theta=0)
numpy_compare.assert_float_equal(R.matrix(), np.eye(3))
R = RotationMatrix.MakeZRotation(theta=0)
numpy_compare.assert_float_equal(R.matrix(), np.eye(3))
# TODO(eric.cousineau): #11575, remove the conditional.
if T == float:
numpy_compare.assert_float_equal(R.row(index=0), [1., 0., 0.])
numpy_compare.assert_float_equal(R.col(index=0), [1., 0., 0.])
R.set(R=np.eye(3))
numpy_compare.assert_float_equal(R.matrix(), np.eye(3))
# - Cast.
self.check_cast(mut.RotationMatrix_, T)
# - Nontrivial quaternion.
q = Quaternion(wxyz=[0.5, 0.5, 0.5, 0.5])
R = RotationMatrix(quaternion=q)
q_R = R.ToQuaternion()
numpy_compare.assert_float_equal(
q.wxyz(), numpy_compare.to_float(q_R.wxyz()))
# - Conversion to AngleAxis
angle_axis = R.ToAngleAxis()
self.assertIsInstance(angle_axis, AngleAxis)
R_AngleAxis = RotationMatrix(angle_axis)
R_I = R.inverse().multiply(R_AngleAxis)
numpy_compare.assert_equal(R_I.IsIdentityToInternalTolerance(), True)
# - Inverse, transpose, projection
R_I = R.inverse().multiply(R)
numpy_compare.assert_float_equal(R_I.matrix(), np.eye(3))
numpy_compare.assert_float_equal((R.inverse() @ R).matrix(), np.eye(3))
R_T = R.transpose().multiply(R)
numpy_compare.assert_float_equal(R_T.matrix(), np.eye(3))
R_P = RotationMatrix.ProjectToRotationMatrix(M=2*np.eye(3))
numpy_compare.assert_float_equal(R_P.matrix(), np.eye(3))
# - Multiplication.
R_AB = RotationMatrix([
[0., 1, 0],
[-1, 0, 0],
[0, 0, 1]])
v_B = [10, 20, 30]
v_A = [20., -10., 30]
numpy_compare.assert_float_equal(R_AB.multiply(v_B=v_B), v_A)
# N.B. Remember that this takes ndarray[3, n], NOT ndarray[n, 3]!
vlist_B = np.array([v_B, v_B]).T
vlist_A = np.array([v_A, v_A]).T
numpy_compare.assert_float_equal(R_AB.multiply(v_B=vlist_B), vlist_A)
# Matrix checks
numpy_compare.assert_equal(R.IsValid(), True)
R = RotationMatrix()
numpy_compare.assert_equal(R.IsExactlyIdentity(), True)
numpy_compare.assert_equal(R.IsIdentityToInternalTolerance(), True)
# Test pickling.
assert_pickle(self, R_AB, RotationMatrix.matrix, T=T)
def test_matrix_substitute_with_variable_and_expression(self):
m = np.array([[x + y, x * y]])
substituted = sym.Substitute(m, x, 3.0)
npc.assert_equal(substituted[0, 0], m[0, 0].Substitute(x, 3.0))
npc.assert_equal(substituted[0, 1], m[0, 1].Substitute(x, 3.0))
def test_scalar_api(self):
a = AD(1, [1., 0])
self.assertEqual(a.value(), 1.)
numpy_compare.assert_equal(a.derivatives(), [1., 0])
self.assertEqual(str(a), "AD{1.0, nderiv=2}")
self.assertEqual(repr(a), "<AutoDiffXd 1.0 nderiv=2>")
numpy_compare.assert_equal(a, a)
# Test construction from `float` and `int`.
numpy_compare.assert_equal(AD(1), AD(1., []))
numpy_compare.assert_equal(AD(1.), AD(1., []))
# Test implicit conversion.
numpy_compare.assert_equal(
autodiff_scalar_pass_through(1), # int
AD(1., []))
numpy_compare.assert_equal(
autodiff_scalar_pass_through(1.), # float
AD(1., []))
# Test multi-element pass-through.
x = np.array([AD(1.), AD(2.), AD(3.)])
numpy_compare.assert_equal(autodiff_vector_pass_through(x), x)
# Ensure fixed-size vectors are correctly converted (#9886).
numpy_compare.assert_equal(autodiff_vector3_pass_through(x), x)
# Ensure we can copy.
numpy_compare.assert_equal(copy.copy(a), a)
numpy_compare.assert_equal(copy.deepcopy(a), a)
# Ensure that we can pickle.
assert_pickle(self, a, lambda x: x)
def test_division(self):
numpy_compare.assert_equal(x / y, "(x / y)")
numpy_compare.assert_equal(x / 1, "x")
numpy_compare.assert_equal(1 / x, "(1 / x)")
