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)")