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
self.assertEqualStructure(sym.abs(v_x), np.abs(v_x))
self.assertNotEqualStructure(sym.abs(v_x), 0.5 * np.abs(v_x))
self._check_scalar(sym.abs(v_x), np.abs(v_x))
self._check_scalar(sym.abs(v_x), np.abs(v_x))
self._check_scalar(sym.exp(v_x), np.exp(v_x))
self._check_scalar(sym.sqrt(v_x), np.sqrt(v_x))
self._check_scalar(sym.pow(v_x, v_y), v_x**v_y)
self._check_scalar(sym.sin(v_x), np.sin(v_x))
self._check_scalar(sym.cos(v_x), np.cos(v_x))
self._check_scalar(sym.tan(v_x), np.tan(v_x))
self._check_scalar(sym.asin(v_x), np.arcsin(v_x))
self._check_scalar(sym.acos(v_x), np.arccos(v_x))
self._check_scalar(sym.atan(v_x), np.arctan(v_x))
self._check_scalar(sym.atan2(v_x, v_y), np.arctan2(v_x, v_y))
self._check_scalar(sym.sinh(v_x), np.sinh(v_x))
self._check_scalar(sym.cosh(v_x), np.cosh(v_x))
self._check_scalar(sym.tanh(v_x), np.tanh(v_x))
self._check_scalar(sym.min(v_x, v_y), min(v_x, v_y))
self._check_scalar(sym.max(v_x, v_y), max(v_x, v_y))
self._check_scalar(sym.ceil(v_x), np.ceil(v_x))
self._check_scalar(sym.floor(v_x), np.floor(v_x))
self._check_scalar(
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):
v_x = 1.0
v_y = 1.0
self.assertEqual(sym.abs(v_x), np.abs(v_x))
self.assertEqual(sym.exp(v_x), np.exp(v_x))
self.assertEqual(sym.sqrt(v_x), np.sqrt(v_x))
self.assertEqual(sym.pow(v_x, v_y), v_x**v_y)
self.assertEqual(sym.sin(v_x), np.sin(v_x))
self.assertEqual(sym.cos(v_x), np.cos(v_x))
self.assertEqual(sym.tan(v_x), np.tan(v_x))
self.assertEqual(sym.asin(v_x), np.arcsin(v_x))
self.assertEqual(sym.acos(v_x), np.arccos(v_x))
self.assertEqual(sym.atan(v_x), np.arctan(v_x))
self.assertEqual(sym.atan2(v_x, v_y), np.arctan2(v_x, v_y))
self.assertEqual(sym.sinh(v_x), np.sinh(v_x))
self.assertEqual(sym.cosh(v_x), np.cosh(v_x))
self.assertEqual(sym.tanh(v_x), np.tanh(v_x))
self.assertEqual(sym.min(v_x, v_y), min(v_x, v_y))
self.assertEqual(sym.max(v_x, v_y), max(v_x, v_y))
self.assertEqual(sym.ceil(v_x), np.ceil(v_x))
self.assertEqual(sym.floor(v_x), np.floor(v_x))
self.assertEqual(
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
self.assertEqualStructure(sym.abs(v_x), np.abs(v_x))
self.assertNotEqualStructure(sym.abs(v_x), 0.5*np.abs(v_x))
self._check_scalar(sym.abs(v_x), np.abs(v_x))
self._check_scalar(sym.abs(v_x), np.abs(v_x))
self._check_scalar(sym.exp(v_x), np.exp(v_x))
self._check_scalar(sym.sqrt(v_x), np.sqrt(v_x))
self._check_scalar(sym.pow(v_x, v_y), v_x ** v_y)
self._check_scalar(sym.sin(v_x), np.sin(v_x))
self._check_scalar(sym.cos(v_x), np.cos(v_x))
self._check_scalar(sym.tan(v_x), np.tan(v_x))
self._check_scalar(sym.asin(v_x), np.arcsin(v_x))
self._check_scalar(sym.acos(v_x), np.arccos(v_x))
self._check_scalar(sym.atan(v_x), np.arctan(v_x))
self._check_scalar(sym.atan2(v_x, v_y), np.arctan2(v_x, v_y))
self._check_scalar(sym.sinh(v_x), np.sinh(v_x))
self._check_scalar(sym.cosh(v_x), np.cosh(v_x))
self._check_scalar(sym.tanh(v_x), np.tanh(v_x))
self._check_scalar(sym.min(v_x, v_y), min(v_x, v_y))
self._check_scalar(sym.max(v_x, v_y), max(v_x, v_y))
self._check_scalar(sym.ceil(v_x), np.ceil(v_x))
self._check_scalar(sym.floor(v_x), np.floor(v_x))
self._check_scalar(
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_variable(self):
self.assertEqual(str(sym.abs(x)), "abs(x)")
self.assertEqual(str(sym.exp(x)), "exp(x)")
self.assertEqual(str(sym.sqrt(x)), "sqrt(x)")
self.assertEqual(str(sym.pow(x, y)), "pow(x, y)")
self.assertEqual(str(sym.sin(x)), "sin(x)")
self.assertEqual(str(sym.cos(x)), "cos(x)")
self.assertEqual(str(sym.tan(x)), "tan(x)")
self.assertEqual(str(sym.asin(x)), "asin(x)")
self.assertEqual(str(sym.acos(x)), "acos(x)")
self.assertEqual(str(sym.atan(x)), "atan(x)")
self.assertEqual(str(sym.atan2(x, y)), "atan2(x, y)")
self.assertEqual(str(sym.sinh(x)), "sinh(x)")
self.assertEqual(str(sym.cosh(x)), "cosh(x)")
self.assertEqual(str(sym.tanh(x)), "tanh(x)")
self.assertEqual(str(sym.min(x, y)), "min(x, y)")
self.assertEqual(str(sym.max(x, y)), "max(x, y)")
self.assertEqual(str(sym.ceil(x)), "ceil(x)")
self.assertEqual(str(sym.floor(x)), "floor(x)")
self.assertEqual(str(sym.if_then_else(x > y, x, y)),
"(if (x > y) then x else y)")
def test_functions_with_variable(self):
self.assertEqual(str(sym.abs(x)), "abs(x)")
self.assertEqual(str(sym.exp(x)), "exp(x)")
self.assertEqual(str(sym.sqrt(x)), "sqrt(x)")
self.assertEqual(str(sym.pow(x, y)), "pow(x, y)")
self.assertEqual(str(sym.sin(x)), "sin(x)")
self.assertEqual(str(sym.cos(x)), "cos(x)")
self.assertEqual(str(sym.tan(x)), "tan(x)")
self.assertEqual(str(sym.asin(x)), "asin(x)")
self.assertEqual(str(sym.acos(x)), "acos(x)")
self.assertEqual(str(sym.atan(x)), "atan(x)")
self.assertEqual(str(sym.atan2(x, y)), "atan2(x, y)")
self.assertEqual(str(sym.sinh(x)), "sinh(x)")
self.assertEqual(str(sym.cosh(x)), "cosh(x)")
self.assertEqual(str(sym.tanh(x)), "tanh(x)")
self.assertEqual(str(sym.min(x, y)), "min(x, y)")
self.assertEqual(str(sym.max(x, y)), "max(x, y)")
self.assertEqual(str(sym.ceil(x)), "ceil(x)")
self.assertEqual(str(sym.floor(x)), "floor(x)")
self.assertEqual(str(sym.if_then_else(x > y, x, y)),
"(if (x > y) then x else y)")
def test_functions_with_expression(self):
self.assertEqual(str(sym.abs(e_x)), "abs(x)")
self.assertEqual(str(sym.exp(e_x)), "exp(x)")
self.assertEqual(str(sym.sqrt(e_x)), "sqrt(x)")
self.assertEqual(str(sym.pow(e_x, e_y)), "pow(x, y)")
self.assertEqual(str(sym.sin(e_x)), "sin(x)")
self.assertEqual(str(sym.cos(e_x)), "cos(x)")
self.assertEqual(str(sym.tan(e_x)), "tan(x)")
self.assertEqual(str(sym.asin(e_x)), "asin(x)")
self.assertEqual(str(sym.acos(e_x)), "acos(x)")
self.assertEqual(str(sym.atan(e_x)), "atan(x)")
self.assertEqual(str(sym.atan2(e_x, e_y)), "atan2(x, y)")
self.assertEqual(str(sym.sinh(e_x)), "sinh(x)")
self.assertEqual(str(sym.cosh(e_x)), "cosh(x)")
self.assertEqual(str(sym.tanh(e_x)), "tanh(x)")
self.assertEqual(str(sym.min(e_x, e_y)), "min(x, y)")
self.assertEqual(str(sym.max(e_x, e_y)), "max(x, y)")
self.assertEqual(str(sym.ceil(e_x)), "ceil(x)")
self.assertEqual(str(sym.floor(e_x)), "floor(x)")
self.assertEqual(str(sym.if_then_else(e_x > e_y, e_x, e_y)),
"(if (x > y) then x else 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
# WARNING: If these math functions have `float` overloads that return
# `float`, then `assertEqual`-like tests are meaningful (current state,
# and before `math` overloads were introduced).
# If these math functions implicitly cast `float` to `Expression`, then
# `assertEqual` tests are meaningless, as it tests `__nonzero__` for
# `Formula`, which will always be True.
self.assertEqual(sym.abs(v_x), 0.5*np.abs(v_x))
self.assertNotEqual(str(sym.abs(v_x)), str(0.5*np.abs(v_x)))
self._check_scalar(sym.abs(v_x), np.abs(v_x))
self._check_scalar(sym.abs(v_x), np.abs(v_x))
self._check_scalar(sym.exp(v_x), np.exp(v_x))
self._check_scalar(sym.sqrt(v_x), np.sqrt(v_x))
self._check_scalar(sym.pow(v_x, v_y), v_x ** v_y)
self._check_scalar(sym.sin(v_x), np.sin(v_x))
self._check_scalar(sym.cos(v_x), np.cos(v_x))
self._check_scalar(sym.tan(v_x), np.tan(v_x))
self._check_scalar(sym.asin(v_x), np.arcsin(v_x))
self._check_scalar(sym.acos(v_x), np.arccos(v_x))
self._check_scalar(sym.atan(v_x), np.arctan(v_x))
self._check_scalar(sym.atan2(v_x, v_y), np.arctan2(v_x, v_y))
self._check_scalar(sym.sinh(v_x), np.sinh(v_x))
self._check_scalar(sym.cosh(v_x), np.cosh(v_x))
self._check_scalar(sym.tanh(v_x), np.tanh(v_x))
self._check_scalar(sym.min(v_x, v_y), min(v_x, v_y))
self._check_scalar(sym.max(v_x, v_y), max(v_x, v_y))
self._check_scalar(sym.ceil(v_x), np.ceil(v_x))
self._check_scalar(sym.floor(v_x), np.floor(v_x))
self._check_scalar(
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 _check_algebra(self, algebra):
xv = algebra.to_algebra(x)
yv = algebra.to_algebra(y)
zv = algebra.to_algebra(z)
wv = algebra.to_algebra(w)
av = algebra.to_algebra(a)
bv = algebra.to_algebra(b)
cv = algebra.to_algebra(c)
e_xv = algebra.to_algebra(e_x)
e_yv = algebra.to_algebra(e_y)
# Addition.
algebra.check_value(e_xv + e_yv, "(x + y)")
algebra.check_value(e_xv + yv, "(x + y)")
algebra.check_value(e_xv + 1, "(1 + x)")
algebra.check_value(xv + e_yv, "(x + y)")
algebra.check_value(1 + e_xv, "(1 + x)")
# - In place.
e = copy(xv)
e += e_yv
algebra.check_value(e, "(x + y)")
e += zv
algebra.check_value(e, "(x + y + z)")
e += 1
algebra.check_value(e, "(1 + x + y + z)")
# Subtraction.
algebra.check_value((e_xv - e_yv), "(x - y)")
algebra.check_value((e_xv - yv), "(x - y)")
algebra.check_value((e_xv - 1), "(-1 + x)")
algebra.check_value((xv - e_yv), "(x - y)")
algebra.check_value((1 - e_xv), "(1 - x)")
# - In place.
e = copy(xv)
e -= e_yv
algebra.check_value(e, (x - y))
e -= zv
algebra.check_value(e, (x - y - z))
e -= 1
algebra.check_value(e, (x - y - z - 1))
# Multiplication.
algebra.check_value((e_xv * e_yv), "(x * y)")
algebra.check_value((e_xv * yv), "(x * y)")
algebra.check_value((e_xv * 1), "x")
algebra.check_value((xv * e_yv), "(x * y)")
algebra.check_value((1 * e_xv), "x")
# - In place.
e = copy(xv)
e *= e_yv
algebra.check_value(e, "(x * y)")
e *= zv
algebra.check_value(e, "(x * y * z)")
e *= 1
algebra.check_value(e, "(x * y * z)")
# Division
algebra.check_value((e_xv / e_yv), (x / y))
algebra.check_value((e_xv / yv), (x / y))
algebra.check_value((e_xv / 1), "x")
algebra.check_value((xv / e_yv), (x / y))
algebra.check_value((1 / e_xv), (1 / x))
# - In place.
e = copy(xv)
e /= e_yv
algebra.check_value(e, (x / y))
e /= zv
algebra.check_value(e, (x / y / z))
e /= 1
algebra.check_value(e, ((x / y) / z))
# Unary
algebra.check_value((+e_xv), "x")
algebra.check_value((-e_xv), "(-1 * x)")
# Math functions.
algebra.check_value((algebra.abs(e_xv)), "abs(x)")
algebra.check_value((algebra.exp(e_xv)), "exp(x)")
algebra.check_value((algebra.sqrt(e_xv)), "sqrt(x)")
algebra.check_value((algebra.pow(e_xv, e_yv)), "pow(x, y)")
algebra.check_value((algebra.sin(e_xv)), "sin(x)")
algebra.check_value((algebra.cos(e_xv)), "cos(x)")
algebra.check_value((algebra.tan(e_xv)), "tan(x)")
algebra.check_value((algebra.arcsin(e_xv)), "asin(x)")
algebra.check_value((algebra.arccos(e_xv)), "acos(x)")
algebra.check_value((algebra.arctan2(e_xv, e_yv)), "atan2(x, y)")
algebra.check_value((algebra.sinh(e_xv)), "sinh(x)")
algebra.check_value((algebra.cosh(e_xv)), "cosh(x)")
algebra.check_value((algebra.tanh(e_xv)), "tanh(x)")
algebra.check_value((algebra.ceil(e_xv)), "ceil(x)")
algebra.check_value((algebra.floor(e_xv)), "floor(x)")
if isinstance(algebra, ScalarAlgebra):
# TODO(eric.cousineau): Uncomment these lines if we can teach numpy
# that reduction is not just selection.
algebra.check_value((algebra.min(e_xv, e_yv)), "min(x, y)")
algebra.check_value((algebra.max(e_xv, e_yv)), "max(x, y)")
# TODO(eric.cousineau): Add broadcasting functions for these
# operations.
algebra.check_value((sym.atan(e_xv)), "atan(x)")
algebra.check_value((sym.if_then_else(e_xv > e_yv, e_xv, e_yv)),
"(if (x > y) then x else y)")
return xv, e_xv
def _check_algebra(self, algebra):
xv = algebra.to_algebra(x)
yv = algebra.to_algebra(y)
zv = algebra.to_algebra(z)
wv = algebra.to_algebra(w)
av = algebra.to_algebra(a)
bv = algebra.to_algebra(b)
cv = algebra.to_algebra(c)
e_xv = algebra.to_algebra(e_x)
e_yv = algebra.to_algebra(e_y)
# Addition.
algebra.check_value(e_xv + e_yv, "(x + y)")
algebra.check_value(e_xv + yv, "(x + y)")
algebra.check_value(e_xv + 1, "(1 + x)")
algebra.check_value(xv + e_yv, "(x + y)")
algebra.check_value(1 + e_xv, "(1 + x)")
# - In place.
e = copy.copy(xv)
e += e_yv
algebra.check_value(e, "(x + y)")
e += zv
algebra.check_value(e, "(x + y + z)")
e += 1
algebra.check_value(e, "(1 + x + y + z)")
# Subtraction.
algebra.check_value((e_xv - e_yv), "(x - y)")
algebra.check_value((e_xv - yv), "(x - y)")
algebra.check_value((e_xv - 1), "(-1 + x)")
algebra.check_value((xv - e_yv), "(x - y)")
algebra.check_value((1 - e_xv), "(1 - x)")
# - In place.
e = copy.copy(xv)
e -= e_yv
algebra.check_value(e, (x - y))
e -= zv
algebra.check_value(e, (x - y - z))
e -= 1
algebra.check_value(e, (x - y - z - 1))
# Multiplication.
algebra.check_value((e_xv * e_yv), "(x * y)")
algebra.check_value((e_xv * yv), "(x * y)")
algebra.check_value((e_xv * 1), "x")
algebra.check_value((xv * e_yv), "(x * y)")
algebra.check_value((1 * e_xv), "x")
# - In place.
e = copy.copy(xv)
e *= e_yv
algebra.check_value(e, "(x * y)")
e *= zv
algebra.check_value(e, "(x * y * z)")
e *= 1
algebra.check_value(e, "(x * y * z)")
# Division
algebra.check_value((e_xv / e_yv), (x / y))
algebra.check_value((e_xv / yv), (x / y))
algebra.check_value((e_xv / 1), "x")
algebra.check_value((xv / e_yv), (x / y))
algebra.check_value((1 / e_xv), (1 / x))
# - In place.
e = copy.copy(xv)
e /= e_yv
algebra.check_value(e, (x / y))
e /= zv
algebra.check_value(e, (x / y / z))
e /= 1
algebra.check_value(e, ((x / y) / z))
# Unary
algebra.check_value((+e_xv), "x")
algebra.check_value((-e_xv), "(-1 * x)")
# Math functions.
algebra.check_value((algebra.abs(e_xv)), "abs(x)")
algebra.check_value((algebra.exp(e_xv)), "exp(x)")
algebra.check_value((algebra.sqrt(e_xv)), "sqrt(x)")
algebra.check_value((algebra.pow(e_xv, e_yv)), "pow(x, y)")
algebra.check_value((algebra.sin(e_xv)), "sin(x)")
algebra.check_value((algebra.cos(e_xv)), "cos(x)")
algebra.check_value((algebra.tan(e_xv)), "tan(x)")
algebra.check_value((algebra.arcsin(e_xv)), "asin(x)")
algebra.check_value((algebra.arccos(e_xv)), "acos(x)")
algebra.check_value((algebra.arctan2(e_xv, e_yv)), "atan2(x, y)")
algebra.check_value((algebra.sinh(e_xv)), "sinh(x)")
algebra.check_value((algebra.cosh(e_xv)), "cosh(x)")
algebra.check_value((algebra.tanh(e_xv)), "tanh(x)")
algebra.check_value((algebra.ceil(e_xv)), "ceil(x)")
algebra.check_value((algebra.floor(e_xv)), "floor(x)")
if isinstance(algebra, ScalarAlgebra):
# TODO(eric.cousineau): Uncomment these lines if we can teach numpy
# that reduction is not just selection.
algebra.check_value((algebra.min(e_xv, e_yv)), "min(x, y)")
algebra.check_value((algebra.max(e_xv, e_yv)), "max(x, y)")
# TODO(eric.cousineau): Add broadcasting functions for these
# operations.
algebra.check_value((sym.atan(e_xv)), "atan(x)")
algebra.check_value((sym.if_then_else(e_xv > e_yv, e_xv, e_yv)),
"(if (x > y) then x else y)")
return xv, e_xv
def _check_algebra(self, algebra):
xv = algebra.to_algebra(x)
yv = algebra.to_algebra(y)
zv = algebra.to_algebra(z)
wv = algebra.to_algebra(w)
av = algebra.to_algebra(a)
bv = algebra.to_algebra(b)
cv = algebra.to_algebra(c)
e_xv = algebra.to_algebra(e_x)
e_yv = algebra.to_algebra(e_y)
# Addition.
numpy_compare.assert_equal(e_xv + e_yv, "(x + y)")
numpy_compare.assert_equal(e_xv + yv, "(x + y)")
numpy_compare.assert_equal(e_xv + 1, "(1 + x)")
numpy_compare.assert_equal(xv + e_yv, "(x + y)")
numpy_compare.assert_equal(1 + e_xv, "(1 + x)")
# - In place.
e = copy.copy(xv)
e += e_yv
numpy_compare.assert_equal(e, "(x + y)")
e += zv
numpy_compare.assert_equal(e, "(x + y + z)")
e += 1
numpy_compare.assert_equal(e, "(1 + x + y + z)")
# Subtraction.
numpy_compare.assert_equal(e_xv - e_yv, "(x - y)")
numpy_compare.assert_equal(e_xv - yv, "(x - y)")
numpy_compare.assert_equal(e_xv - 1, "(-1 + x)")
numpy_compare.assert_equal(xv - e_yv, "(x - y)")
numpy_compare.assert_equal(1 - e_xv, "(1 - x)")
# - In place.
e = copy.copy(xv)
e -= e_yv
numpy_compare.assert_equal(e, (x - y))
e -= zv
numpy_compare.assert_equal(e, (x - y - z))
e -= 1
numpy_compare.assert_equal(e, (x - y - z - 1))
# Multiplication.
numpy_compare.assert_equal(e_xv * e_yv, "(x * y)")
numpy_compare.assert_equal(e_xv * yv, "(x * y)")
numpy_compare.assert_equal(e_xv * 1, "x")
numpy_compare.assert_equal(xv * e_yv, "(x * y)")
numpy_compare.assert_equal(1 * e_xv, "x")
# - In place.
e = copy.copy(xv)
e *= e_yv
numpy_compare.assert_equal(e, "(x * y)")
e *= zv
numpy_compare.assert_equal(e, "(x * y * z)")
e *= 1
numpy_compare.assert_equal(e, "(x * y * z)")
# Division
numpy_compare.assert_equal(e_xv / e_yv, (x / y))
numpy_compare.assert_equal(e_xv / yv, (x / y))
numpy_compare.assert_equal(e_xv / 1, "x")
numpy_compare.assert_equal(xv / e_yv, (x / y))
numpy_compare.assert_equal(1 / e_xv, (1 / x))
# - In place.
e = copy.copy(xv)
e /= e_yv
numpy_compare.assert_equal(e, (x / y))
e /= zv
numpy_compare.assert_equal(e, (x / y / z))
e /= 1
numpy_compare.assert_equal(e, ((x / y) / z))
# Unary
numpy_compare.assert_equal(+e_xv, "x")
numpy_compare.assert_equal(-e_xv, "(-1 * x)")
# Comparison. For `VectorizedAlgebra`, uses `np.vectorize` workaround
# for #8315.
# TODO(eric.cousineau): `BaseAlgebra.check_logical` is designed for
# AutoDiffXd (float-convertible), not for symbolic (not always
# float-convertible).
numpy_compare.assert_equal(algebra.lt(e_xv, e_yv), "(x < y)")
numpy_compare.assert_equal(algebra.le(e_xv, e_yv), "(x <= y)")
numpy_compare.assert_equal(algebra.eq(e_xv, e_yv), "(x == y)")
numpy_compare.assert_equal(algebra.ne(e_xv, e_yv), "(x != y)")
numpy_compare.assert_equal(algebra.ge(e_xv, e_yv), "(x >= y)")
numpy_compare.assert_equal(algebra.gt(e_xv, e_yv), "(x > y)")
# Math functions.
numpy_compare.assert_equal(algebra.abs(e_xv), "abs(x)")
numpy_compare.assert_equal(algebra.exp(e_xv), "exp(x)")
numpy_compare.assert_equal(algebra.sqrt(e_xv), "sqrt(x)")
numpy_compare.assert_equal(algebra.pow(e_xv, e_yv), "pow(x, y)")
numpy_compare.assert_equal(algebra.sin(e_xv), "sin(x)")
numpy_compare.assert_equal(algebra.cos(e_xv), "cos(x)")
numpy_compare.assert_equal(algebra.tan(e_xv), "tan(x)")
numpy_compare.assert_equal(algebra.arcsin(e_xv), "asin(x)")
numpy_compare.assert_equal(algebra.arccos(e_xv), "acos(x)")
numpy_compare.assert_equal(algebra.arctan2(e_xv, e_yv), "atan2(x, y)")
numpy_compare.assert_equal(algebra.sinh(e_xv), "sinh(x)")
numpy_compare.assert_equal(algebra.cosh(e_xv), "cosh(x)")
numpy_compare.assert_equal(algebra.tanh(e_xv), "tanh(x)")
numpy_compare.assert_equal(algebra.ceil(e_xv), "ceil(x)")
numpy_compare.assert_equal(algebra.floor(e_xv), "floor(x)")
if isinstance(algebra, ScalarAlgebra):
# TODO(eric.cousineau): Uncomment these lines if we can teach numpy
# that reduction is not just selection.
numpy_compare.assert_equal(algebra.min(e_xv, e_yv), "min(x, y)")
numpy_compare.assert_equal(algebra.max(e_xv, e_yv), "max(x, y)")
# TODO(eric.cousineau): Add broadcasting functions for these
# operations.
numpy_compare.assert_equal(sym.atan(e_xv), "atan(x)")
numpy_compare.assert_equal(
sym.if_then_else(e_xv > e_yv, e_xv, e_yv),
"(if (x > y) then x else y)")
return xv, e_xv
def _check_algebra(self, algebra):
xv = algebra.to_algebra(x)
yv = algebra.to_algebra(y)
zv = algebra.to_algebra(z)
wv = algebra.to_algebra(w)
av = algebra.to_algebra(a)
bv = algebra.to_algebra(b)
cv = algebra.to_algebra(c)
e_xv = algebra.to_algebra(e_x)
e_yv = algebra.to_algebra(e_y)
# Addition.
npc.assert_equal(e_xv + e_yv, "(x + y)")
npc.assert_equal(e_xv + yv, "(x + y)")
npc.assert_equal(e_xv + 1, "(1 + x)")
npc.assert_equal(xv + e_yv, "(x + y)")
npc.assert_equal(1 + e_xv, "(1 + x)")
# - In place.
e = copy.copy(xv)
e += e_yv
npc.assert_equal(e, "(x + y)")
e += zv
npc.assert_equal(e, "(x + y + z)")
e += 1
npc.assert_equal(e, "(1 + x + y + z)")
# Subtraction.
npc.assert_equal(e_xv - e_yv, "(x - y)")
npc.assert_equal(e_xv - yv, "(x - y)")
npc.assert_equal(e_xv - 1, "(-1 + x)")
npc.assert_equal(xv - e_yv, "(x - y)")
npc.assert_equal(1 - e_xv, "(1 - x)")
# - In place.
e = copy.copy(xv)
e -= e_yv
npc.assert_equal(e, (x - y))
e -= zv
npc.assert_equal(e, (x - y - z))
e -= 1
npc.assert_equal(e, (x - y - z - 1))
# Multiplication.
npc.assert_equal(e_xv * e_yv, "(x * y)")
npc.assert_equal(e_xv * yv, "(x * y)")
npc.assert_equal(e_xv * 1, "x")
npc.assert_equal(xv * e_yv, "(x * y)")
npc.assert_equal(1 * e_xv, "x")
# - In place.
e = copy.copy(xv)
e *= e_yv
npc.assert_equal(e, "(x * y)")
e *= zv
npc.assert_equal(e, "(x * y * z)")
e *= 1
npc.assert_equal(e, "(x * y * z)")
# Division
npc.assert_equal(e_xv / e_yv, (x / y))
npc.assert_equal(e_xv / yv, (x / y))
npc.assert_equal(e_xv / 1, "x")
npc.assert_equal(xv / e_yv, (x / y))
npc.assert_equal(1 / e_xv, (1 / x))
# - In place.
e = copy.copy(xv)
e /= e_yv
npc.assert_equal(e, (x / y))
e /= zv
npc.assert_equal(e, (x / y / z))
e /= 1
npc.assert_equal(e, ((x / y) / z))
# Unary
npc.assert_equal(+e_xv, "x")
npc.assert_equal(-e_xv, "(-1 * x)")
# Comparison. For `VectorizedAlgebra`, uses `np.vectorize` workaround
# for #8315.
# TODO(eric.cousineau): `BaseAlgebra.check_logical` is designed for
# AutoDiffXd (float-convertible), not for symbolic (not always
# float-convertible).
npc.assert_equal(algebra.lt(e_xv, e_yv), "(x < y)")
npc.assert_equal(algebra.le(e_xv, e_yv), "(x <= y)")
npc.assert_equal(algebra.eq(e_xv, e_yv), "(x == y)")
npc.assert_equal(algebra.ne(e_xv, e_yv), "(x != y)")
npc.assert_equal(algebra.ge(e_xv, e_yv), "(x >= y)")
npc.assert_equal(algebra.gt(e_xv, e_yv), "(x > y)")
# Math functions.
npc.assert_equal(algebra.abs(e_xv), "abs(x)")
npc.assert_equal(algebra.exp(e_xv), "exp(x)")
npc.assert_equal(algebra.sqrt(e_xv), "sqrt(x)")
npc.assert_equal(algebra.pow(e_xv, e_yv), "pow(x, y)")
npc.assert_equal(algebra.sin(e_xv), "sin(x)")
npc.assert_equal(algebra.cos(e_xv), "cos(x)")
npc.assert_equal(algebra.tan(e_xv), "tan(x)")
npc.assert_equal(algebra.arcsin(e_xv), "asin(x)")
npc.assert_equal(algebra.arccos(e_xv), "acos(x)")
npc.assert_equal(algebra.arctan2(e_xv, e_yv), "atan2(x, y)")
npc.assert_equal(algebra.sinh(e_xv), "sinh(x)")
npc.assert_equal(algebra.cosh(e_xv), "cosh(x)")
npc.assert_equal(algebra.tanh(e_xv), "tanh(x)")
npc.assert_equal(algebra.ceil(e_xv), "ceil(x)")
npc.assert_equal(algebra.floor(e_xv), "floor(x)")
if isinstance(algebra, ScalarAlgebra):
# TODO(eric.cousineau): Uncomment these lines if we can teach numpy
# that reduction is not just selection.
npc.assert_equal(algebra.min(e_xv, e_yv), "min(x, y)")
npc.assert_equal(algebra.max(e_xv, e_yv), "max(x, y)")
# TODO(eric.cousineau): Add broadcasting functions for these
# operations.
npc.assert_equal(sym.atan(e_xv), "atan(x)")
npc.assert_equal(sym.if_then_else(e_xv > e_yv, e_xv, e_yv),
"(if (x > y) then x else y)")
return xv, e_xv
def __init__(self, m=5, J=500, m_l=1, J_l=0.5, l1=0.0, l2=0.0, k_g=2e3, b_g=20, \
g=9.8, flight_step_size = 1e-2, contact_step_size = 5e-3, descend_step_size_switch_threshold=2e-2, \
ground_height_function=lambda x: 0, initial_state=np.asarray([0.,0.,0.,1.5,1.0,0.,0.,0.,0.,0.])):
'''
2D hopper with actuated piston at the end of the leg.
The model of the hopper follows the one described in "Hopping in Legged Systems" (Raibert, 1984)
'''
self.m = m
self.J = J
self.m_l = m_l
self.J_l = J_l
self.l1 = l1
self.l2 = l2
self.k_g_y = k_g
self.k_g_x = 2e3
self.b_g_x = 200
self.b_g = b_g
self.g = g
self.ground_height_function = ground_height_function
self.r0 = 1.5
# state machine for touchdown detection
self.xTD = sym.Variable('xTD')
self.was_in_contact = False
# Symbolic variables
# State variables are s = [x_ft, y_ft, theta, phi, r]
# self.x = [s, sdot]
# Inputs are self.u = [tau, chi]
self.x = np.array([sym.Variable('x_' + str(i)) for i in range(10)])
self.u = np.array([sym.Variable('u_' + str(i)) for i in range(2)])
# Initial state
self.initial_env = {}
for i, state in enumerate(initial_state):
self.initial_env[self.x[i]] = state
self.initial_env[self.xTD] = 0
self.k0 = 800
self.b_leg = 2
self.k0_stabilize = 40
self.b0_stabilize = 10
self.k0_restore = 60
self.b0_restore = 15
# self.flight_step_size = flight_step_size
# self.contact_step_size = contact_step_size
# self.descend_step_size_switch_threshold = descend_step_size_switch_threshold
# self.hover_step_size_switch_threshold=-0.75
# print(self.initial_env)
# Dynamic modes
Fx_contact = -self.k_g_x * (self.x[0] -
self.xTD) - self.b_g_x * self.x[5]
Fx_flight = 0.
Fy_contact = -self.k_g_y * (self.x[1] - self.ground_height_function(
self.x[0])) - self.b_g * self.x[6] * (1 - np.exp(self.x[1] * 16))
Fy_flight = 0.
R = self.x[4] - self.l1
# EOM is obtained from Russ Tedrake's Thesis
a1 = -self.m_l * R
a2 = (self.J_l - self.m_l * R * self.l1) * sym.cos(self.x[2])
b1 = self.m_l * R
b2 = (self.J_l - self.m_l * R * self.l1) * sym.sin(self.x[2])
c1 = self.m * R
c2 = (self.J_l + self.m * R * self.x[4]) * sym.cos(self.x[2])
c3 = self.m * R * self.l2 * sym.cos(self.x[3])
c4 = self.m * R * sym.sin(self.x[2])
d1 = -self.m * R
d2 = (self.J_l + self.m * R * self.x[4]) * sym.sin(self.x[2])
d3 = self.m * R * self.l2 * sym.sin(self.x[3])
d4 = -self.m * R * sym.cos(self.x[2])
e1 = self.J_l * self.l2 * sym.cos(self.x[2] - self.x[3])
e2 = -self.J * R
self.b_r_ascend = 0.
r_diff = self.x[4] - self.r0
F_leg_flight = -self.k0_restore * r_diff - self.b0_restore * self.x[9]
F_leg_ascend = -self.u[1] * r_diff - self.b_r_ascend * self.x[
9] #self.u[1] * (1-np.exp(10*r_diff_upper)/(np.exp(10*r_diff_upper)+1))#+(- self.k0_stabilize * r_diff_upper - self.b0_stabilize * self.x[9])*(np.exp(10*r_diff_upper)/(np.exp(10*r_diff_upper)+1))
F_leg_descend = -self.k0 * r_diff - self.b_leg * self.x[9]
# F_leg_descend = F_leg_ascend
self.tau_p = 400.
self.tau_d = 10.
hip_x_dot = self.x[5] + self.x[9] * sym.sin(
self.x[2]) + self.x[4] * sym.cos(self.x[2]) * self.x[7]
hip_y_dot = self.x[6] + self.x[9] * sym.cos(
self.x[2]) - self.x[4] * sym.sin(self.x[2]) * self.x[7]
alpha_des_ascend = 0.6 * sym.atan(hip_x_dot / (
-hip_y_dot - 1e-6)) #-sym.atan(self.x[5]/self.x[6]) # point toward
alpha_des_descend = 0.6 * sym.atan(
hip_x_dot / (hip_y_dot + 1e-6)) # point toward landing point
tau_leg_flight_ascend = (self.tau_p * (alpha_des_ascend - self.x[2]) -
self.tau_d * self.x[7]) * -1
tau_leg_flight_descend = (self.tau_p *
(alpha_des_descend - self.x[2]) -
self.tau_d * self.x[7]) * -1
tau_leg_contact = self.u[0]
def get_ddots(Fx, Fy, F_leg, u0):
alpha = (self.l1 * Fy * sym.sin(self.x[2]) -
self.l1 * Fx * sym.cos(self.x[2]) - u0)
A = sym.cos(self.x[2]) * alpha - R * (
Fx - F_leg * sym.sin(self.x[2]) -
self.m_l * self.l1 * self.x[7]**2 * sym.sin(self.x[2]))
B = sym.sin(self.x[2]) * alpha + R * (
self.m_l * self.l1 * self.x[7]**2 * sym.cos(self.x[2]) + Fy -
F_leg * sym.cos(self.x[2]) - self.m_l * self.g)
C = sym.cos(self.x[2]) * alpha + R * F_leg * sym.sin(
self.x[2]) + self.m * R * (
self.x[4] * self.x[7]**2 * sym.sin(self.x[2]) +
self.l2 * self.x[8]**2 * sym.sin(self.x[3]) -
2 * self.x[9] * self.x[7] * sym.cos(self.x[2]))
D = sym.sin(self.x[2]) * alpha - R * (
F_leg * sym.cos(self.x[2]) - self.m * self.g) - self.m * R * (
2 * self.x[9] * self.x[7] * sym.sin(self.x[2]) +
self.x[4] * self.x[7]**2 * sym.cos(self.x[2]) +
self.l2 * self.x[8]**2 * sym.cos(self.x[3]))
E = self.l2 * sym.cos(self.x[2] - self.x[3]) * alpha - R * (
self.l2 * F_leg * sym.sin(self.x[3] - self.x[2]) + u0)
return np.asarray([(A*b1*c2*d4*e2 - A*b1*c3*d4*e1 - A*b1*c4*d2*e2 + A*b1*c4*d3*e1 + A*b2*c4*d1*e2 - B*a2*c4*d1*e2 - C*a2*b1*d4*e2 + D*a2*b1*c4*e2 + E*a2*b1*c3*d4 - E*a2*b1*c4*d3)/(a1*b1*c2*d4*e2 - a1*b1*c3*d4*e1 - a1*b1*c4*d2*e2 + a1*b1*c4*d3*e1 + a1*b2*c4*d1*e2 - a2*b1*c1*d4*e2), \
(A*b2*c1*d4*e2 + B*a1*c2*d4*e2 - B*a1*c3*d4*e1 - B*a1*c4*d2*e2 + B*a1*c4*d3*e1 - B*a2*c1*d4*e2 - C*a1*b2*d4*e2 + D*a1*b2*c4*e2 + E*a1*b2*c3*d4 - E*a1*b2*c4*d3)/(a1*b1*c2*d4*e2 - a1*b1*c3*d4*e1 - a1*b1*c4*d2*e2 + a1*b1*c4*d3*e1 + a1*b2*c4*d1*e2 - a2*b1*c1*d4*e2), \
-(A*b1*c1*d4*e2 - B*a1*c4*d1*e2 - C*a1*b1*d4*e2 + D*a1*b1*c4*e2 + E*a1*b1*c3*d4 - E*a1*b1*c4*d3)/(a1*b1*c2*d4*e2 - a1*b1*c3*d4*e1 - a1*b1*c4*d2*e2 + a1*b1*c4*d3*e1 + a1*b2*c4*d1*e2 - a2*b1*c1*d4*e2), \
(A*b1*c1*d4*e1 - B*a1*c4*d1*e1 - C*a1*b1*d4*e1 + D*a1*b1*c4*e1 + E*a1*b1*c2*d4 - E*a1*b1*c4*d2 + E*a1*b2*c4*d1 - E*a2*b1*c1*d4)/(a1*b1*c2*d4*e2 - a1*b1*c3*d4*e1 - a1*b1*c4*d2*e2 + a1*b1*c4*d3*e1 + a1*b2*c4*d1*e2 - a2*b1*c1*d4*e2), \
(A*b1*c1*d2*e2 - A*b1*c1*d3*e1 - A*b2*c1*d1*e2 - B*a1*c2*d1*e2 + B*a1*c3*d1*e1 + B*a2*c1*d1*e2 - C*a1*b1*d2*e2 + C*a1*b1*d3*e1 + C*a1*b2*d1*e2 + D*a1*b1*c2*e2 - D*a1*b1*c3*e1 - D*a2*b1*c1*e2 - E*a1*b1*c2*d3 + E*a1*b1*c3*d2 - E*a1*b2*c3*d1 + E*a2*b1*c1*d3)/(a1*b1*c2*d4*e2 - a1*b1*c3*d4*e1 - a1*b1*c4*d2*e2 + a1*b1*c4*d3*e1 + a1*b2*c4*d1*e2 - a2*b1*c1*d4*e2)])
flight_ascend_dynamics = np.hstack(
(self.x[5:],
get_ddots(Fx_flight, Fy_flight, F_leg_flight,
tau_leg_flight_ascend)))
flight_descend_dynamics = np.hstack(
(self.x[5:],
get_ddots(Fx_flight, Fy_flight, F_leg_flight,
tau_leg_flight_descend)))
contact_descend_dynamics = np.hstack(
(self.x[5:],
get_ddots(Fx_contact, Fy_contact, F_leg_descend,
tau_leg_contact)))
contact_ascend_dynamics = np.hstack(
(self.x[5:],
get_ddots(Fx_contact, Fy_contact, F_leg_ascend, tau_leg_contact)))
flight_ascend_conditions = np.asarray([
self.x[1] > self.ground_height_function(self.x[0]), hip_y_dot > 0
])
flight_descend_conditions = np.asarray([
self.x[1] > self.ground_height_function(self.x[0]), hip_y_dot <= 0
])
contact_descend_coditions = np.asarray([
self.x[1] <= self.ground_height_function(self.x[0]), self.x[9] < 0
])
contact_ascend_coditions = np.asarray([
self.x[1] <= self.ground_height_function(self.x[0]), self.x[9] >= 0
])
self.f_list = np.asarray([
flight_ascend_dynamics, flight_descend_dynamics,
contact_descend_dynamics, contact_ascend_dynamics
])
self.f_type_list = np.asarray(
['continuous', 'continuous', 'continuous', 'continuous'])
self.c_list = np.asarray([
flight_ascend_conditions, flight_descend_conditions,
contact_descend_coditions, contact_ascend_coditions
])
DTHybridSystem.__init__(self, self.f_list, self.f_type_list, self.x, self.u, self.c_list, \
self.initial_env, input_limits=np.vstack([[-500,1.4e3], [500,2.5e3]]))
