def test_conv7():
x = Symbol("x")
y = Symbol("y")
assert sin(x/3) == sin(sympy.Symbol("x") / 3)
assert cos(x/3) == cos(sympy.Symbol("x") / 3)
assert tan(x/3) == tan(sympy.Symbol("x") / 3)
assert cot(x/3) == cot(sympy.Symbol("x") / 3)
assert csc(x/3) == csc(sympy.Symbol("x") / 3)
assert sec(x/3) == sec(sympy.Symbol("x") / 3)
assert asin(x/3) == asin(sympy.Symbol("x") / 3)
assert acos(x/3) == acos(sympy.Symbol("x") / 3)
assert atan(x/3) == atan(sympy.Symbol("x") / 3)
assert acot(x/3) == acot(sympy.Symbol("x") / 3)
assert acsc(x/3) == acsc(sympy.Symbol("x") / 3)
assert asec(x/3) == asec(sympy.Symbol("x") / 3)
assert sin(x/3)._sympy_() == sympy.sin(sympy.Symbol("x") / 3)
assert sin(x/3)._sympy_() != sympy.cos(sympy.Symbol("x") / 3)
assert cos(x/3)._sympy_() == sympy.cos(sympy.Symbol("x") / 3)
assert tan(x/3)._sympy_() == sympy.tan(sympy.Symbol("x") / 3)
assert cot(x/3)._sympy_() == sympy.cot(sympy.Symbol("x") / 3)
assert csc(x/3)._sympy_() == sympy.csc(sympy.Symbol("x") / 3)
assert sec(x/3)._sympy_() == sympy.sec(sympy.Symbol("x") / 3)
assert asin(x/3)._sympy_() == sympy.asin(sympy.Symbol("x") / 3)
assert acos(x/3)._sympy_() == sympy.acos(sympy.Symbol("x") / 3)
assert atan(x/3)._sympy_() == sympy.atan(sympy.Symbol("x") / 3)
assert acot(x/3)._sympy_() == sympy.acot(sympy.Symbol("x") / 3)
assert acsc(x/3)._sympy_() == sympy.acsc(sympy.Symbol("x") / 3)
assert asec(x/3)._sympy_() == sympy.asec(sympy.Symbol("x") / 3)
def test_conv7():
x = Symbol("x")
y = Symbol("y")
assert sin(x / 3) == sin(sympy.Symbol("x") / 3)
assert cos(x / 3) == cos(sympy.Symbol("x") / 3)
assert tan(x / 3) == tan(sympy.Symbol("x") / 3)
assert cot(x / 3) == cot(sympy.Symbol("x") / 3)
assert csc(x / 3) == csc(sympy.Symbol("x") / 3)
assert sec(x / 3) == sec(sympy.Symbol("x") / 3)
assert asin(x / 3) == asin(sympy.Symbol("x") / 3)
assert acos(x / 3) == acos(sympy.Symbol("x") / 3)
assert atan(x / 3) == atan(sympy.Symbol("x") / 3)
assert acot(x / 3) == acot(sympy.Symbol("x") / 3)
assert acsc(x / 3) == acsc(sympy.Symbol("x") / 3)
assert asec(x / 3) == asec(sympy.Symbol("x") / 3)
assert sin(x / 3)._sympy_() == sympy.sin(sympy.Symbol("x") / 3)
assert sin(x / 3)._sympy_() != sympy.cos(sympy.Symbol("x") / 3)
assert cos(x / 3)._sympy_() == sympy.cos(sympy.Symbol("x") / 3)
assert tan(x / 3)._sympy_() == sympy.tan(sympy.Symbol("x") / 3)
assert cot(x / 3)._sympy_() == sympy.cot(sympy.Symbol("x") / 3)
assert csc(x / 3)._sympy_() == sympy.csc(sympy.Symbol("x") / 3)
assert sec(x / 3)._sympy_() == sympy.sec(sympy.Symbol("x") / 3)
assert asin(x / 3)._sympy_() == sympy.asin(sympy.Symbol("x") / 3)
assert acos(x / 3)._sympy_() == sympy.acos(sympy.Symbol("x") / 3)
assert atan(x / 3)._sympy_() == sympy.atan(sympy.Symbol("x") / 3)
assert acot(x / 3)._sympy_() == sympy.acot(sympy.Symbol("x") / 3)
assert acsc(x / 3)._sympy_() == sympy.acsc(sympy.Symbol("x") / 3)
assert asec(x / 3)._sympy_() == sympy.asec(sympy.Symbol("x") / 3)
def acos(expr):
"""Arccosine"""
if type(expr) == GC:
return GC(se.acos(expr.expr), {
s: -d / se.sqrt(1 - expr.expr**2)
for s, d in expr.gradients.items()
})
return se.acos(expr)
def diffable_slerp(q1, q2, t):
"""
!takes a long time to compile!
:param q1: 4x1 Matrix
:type q1: Matrix
:param q2: 4x1 Matrix
:type q2: Matrix
:param t: float, 0-1
:type t: Union[float, Symbol]
:return: 4x1 Matrix; Return spherical linear interpolation between two quaternions.
:rtype: Matrix
"""
cos_half_theta = dot(q1, q2)
if0 = -cos_half_theta
q2 = diffable_if_greater_zero(if0, -q2, q2)
cos_half_theta = diffable_if_greater_zero(if0, -cos_half_theta, cos_half_theta)
if1 = diffable_abs(cos_half_theta) - 1.0
# enforce acos(x) with -1 < x < 1
cos_half_theta = diffable_min_fast(1, cos_half_theta)
cos_half_theta = diffable_max_fast(-1, cos_half_theta)
half_theta = acos(cos_half_theta)
sin_half_theta = sqrt(1.0 - cos_half_theta * cos_half_theta)
# prevent /0
sin_half_theta = diffable_if_eq_zero(sin_half_theta, 1, sin_half_theta)
if2 = 0.001 - diffable_abs(sin_half_theta)
ratio_a = sin((1.0 - t) * half_theta) / sin_half_theta
ratio_b = sin(t * half_theta) / sin_half_theta
return diffable_if_greater_eq_zero(if1, se.Matrix(q1),
diffable_if_greater_zero(if2, 0.5 * q1 + 0.5 * q2, ratio_a * q1 + ratio_b * q2))
def axis_angle_from_matrix(rotation_matrix):
"""
:param rotation_matrix: 4x4 or 3x3 Matrix
:type rotation_matrix: Matrix
:return: 3x1 Matrix, angle
:rtype: (Matrix, Union[float, Symbol])
"""
rm = rotation_matrix
angle = (trace(rm[:3, :3]) - 1) / 2
angle = se.Min(angle, 1)
angle = se.Max(angle, -1)
angle = se.acos(angle)
x = (rm[2, 1] - rm[1, 2])
y = (rm[0, 2] - rm[2, 0])
z = (rm[1, 0] - rm[0, 1])
n = se.sqrt(x * x + y * y + z * z)
m = if_eq_zero(n, 1, n)
axis = se.Matrix([
if_eq_zero(n, 0, x / m),
if_eq_zero(n, 0, y / m),
if_eq_zero(n, 1, z / m)
])
sign = diffable_sign(angle)
axis *= sign
angle = sign * angle
return axis, angle
def test_symengine(arr, sig3):
"""Test symengine."""
try:
import symengine as sge
x, y, z = sge.var("x y z")
fn = sge.acos(x) / y + sge.exp(-z)
func = numbafy(fn, (x, y, z), compiler="vectorize", signatures=sig3)
result = func(arr, arr, arr)
check = np.arccos(arr) / arr + np.exp(-arr)
assert np.allclose(result, check) == True
except ImportError:
pass
def test_symengine(arr, sig3):
"""Test symengine."""
try:
import symengine as sge
x, y, z = sge.var("x y z")
fn = sge.acos(x)/y + sge.exp(-z)
func = numbafy(fn, (x, y, z), compiler="vectorize", signatures=sig3)
result = func(arr, arr, arr)
check = np.arccos(arr)/arr + np.exp(-arr)
assert np.allclose(result, check) == True
except ImportError:
pass
def rotation_distance(a_R_b, a_R_c):
"""
:param a_R_b: 4x4 or 3x3 Matrix
:type a_R_b: Matrix
:param a_R_c: 4x4 or 3x3 Matrix
:type a_R_c: Matrix
:return: angle of axis angle representation of b_R_c
:rtype: Union[float, Symbol]
"""
difference = a_R_b.T * a_R_c
angle = (trace(difference[:3, :3]) - 1) / 2
angle = se.Min(angle, 1)
angle = se.Max(angle, -1)
return se.acos(angle)
def test_conv7b():
x = sympy.Symbol("x")
y = sympy.Symbol("y")
assert sympify(sympy.sin(x / 3)) == sin(Symbol("x") / 3)
assert sympify(sympy.sin(x / 3)) != cos(Symbol("x") / 3)
assert sympify(sympy.cos(x / 3)) == cos(Symbol("x") / 3)
assert sympify(sympy.tan(x / 3)) == tan(Symbol("x") / 3)
assert sympify(sympy.cot(x / 3)) == cot(Symbol("x") / 3)
assert sympify(sympy.csc(x / 3)) == csc(Symbol("x") / 3)
assert sympify(sympy.sec(x / 3)) == sec(Symbol("x") / 3)
assert sympify(sympy.asin(x / 3)) == asin(Symbol("x") / 3)
assert sympify(sympy.acos(x / 3)) == acos(Symbol("x") / 3)
assert sympify(sympy.atan(x / 3)) == atan(Symbol("x") / 3)
assert sympify(sympy.acot(x / 3)) == acot(Symbol("x") / 3)
assert sympify(sympy.acsc(x / 3)) == acsc(Symbol("x") / 3)
assert sympify(sympy.asec(x / 3)) == asec(Symbol("x") / 3)
def test_conv7b():
x = sympy.Symbol("x")
y = sympy.Symbol("y")
assert sympify(sympy.sin(x/3)) == sin(Symbol("x") / 3)
assert sympify(sympy.sin(x/3)) != cos(Symbol("x") / 3)
assert sympify(sympy.cos(x/3)) == cos(Symbol("x") / 3)
assert sympify(sympy.tan(x/3)) == tan(Symbol("x") / 3)
assert sympify(sympy.cot(x/3)) == cot(Symbol("x") / 3)
assert sympify(sympy.csc(x/3)) == csc(Symbol("x") / 3)
assert sympify(sympy.sec(x/3)) == sec(Symbol("x") / 3)
assert sympify(sympy.asin(x/3)) == asin(Symbol("x") / 3)
assert sympify(sympy.acos(x/3)) == acos(Symbol("x") / 3)
assert sympify(sympy.atan(x/3)) == atan(Symbol("x") / 3)
assert sympify(sympy.acot(x/3)) == acot(Symbol("x") / 3)
assert sympify(sympy.acsc(x/3)) == acsc(Symbol("x") / 3)
assert sympify(sympy.asec(x/3)) == asec(Symbol("x") / 3)
def axis_angle_from_quaternion(x, y, z, w):
"""
:type x: Union[float, Symbol]
:type y: Union[float, Symbol]
:type z: Union[float, Symbol]
:type w: Union[float, Symbol]
:return: 4x1 Matrix
:rtype: Matrix
"""
# TODO buggy, angle goes from 0 - 2*pi instead of -pi - +pi
w2 = sp.sqrt(1 - w**2)
angle = (2 * sp.acos(w))
x = x / w2
y = y / w2
z = z / w2
return sp.Matrix([x, y, z]), angle
def rotation_distance(rotation_matrix1, rotation_matrix2):
"""
:param rotation_matrix1: 4x4 or 3x3 Matrix
:type rotation_matrix1: Matrix
:param rotation_matrix2: 4x4 or 3x3 Matrix
:type rotation_matrix2: Matrix
:return: angle from the axis/angle representation of rotation_matrix1 * rotation_matrix2.T
:rtype: Union[float, Symbol]
"""
# TODO test me
difference = rotation_matrix1 * rotation_matrix2.T
# return -(((trace(difference) - 1)/2)-1)
v = (trace(difference[:3, :3]) - 1) / 2
# v = Max(-1, v)
# v = Min(1, v)
return sp.acos(v)
def axis_angle_from_quaternion(x, y, z, w):
"""
:type x: Union[float, Symbol]
:type y: Union[float, Symbol]
:type z: Union[float, Symbol]
:type w: Union[float, Symbol]
:return: 4x1 Matrix
:rtype: Matrix
"""
l = norm([x, y, z, w])
x, y, z, w = x / l, y / l, z / l, w / l
w2 = se.sqrt(1 - w**2)
angle = (2 * se.acos(se.Min(se.Max(-1, w), 1)))
m = if_eq_zero(w2, 1, w2) # avoid /0
x = if_eq_zero(w2, 0, x / m)
y = if_eq_zero(w2, 0, y / m)
z = if_eq_zero(w2, 1, z / m)
return se.Matrix([x, y, z]), angle
def axis_angle_from_matrix(rotation_matrix):
"""
:param rotation_matrix: 4x4 or 3x3 Matrix
:type rotation_matrix: Matrix
:return: 3x1 Matrix, angle
:rtype: (Matrix, Union[float, Symbol])
"""
# TODO nan if angle 0
# TODO use 'if' to make angle always positive?
rm = rotation_matrix
angle = (trace(rm[:3, :3]) - 1) / 2
angle = sp.acos(angle)
x = (rm[2, 1] - rm[1, 2])
y = (rm[0, 2] - rm[2, 0])
z = (rm[1, 0] - rm[0, 1])
n = sp.sqrt(x * x + y * y + z * z)
axis = sp.Matrix([x / n, y / n, z / n])
return axis, angle
def axisanglecompose(axis_angle, axis_angle2):
a = axisanglemagnitude(axis_angle)
ah = axisanglenormalize(axis_angle)
b = axisanglemagnitude(axis_angle2)
bh = axisanglenormalize(axis_angle2)
sina = sin(a / 2)
asina = [ah[0] * sina, ah[1] * sina, ah[2] * sina]
sinb = sin(b / 2)
bsinb = [bh[0] * sinb, bh[1] * sinb, bh[2] * sinb]
c = 2 * acos(cos(a / 2)*cos(b / 2) - dot3d(asina, bsinb))
d = axisanglenormalize(cos(a / 2) * sp.Matrix(bsinb) + cos(b / 2) * sp.Matrix(asina) + cross3d(asina, bsinb))
return sp.Matrix([
c * d[0],
c * d[1],
c * d[2]
])
def diffable_axis_angle_from_matrix_stable(rotation_matrix):
"""
:param rotation_matrix: 4x4 or 3x3 Matrix
:type rotation_matrix: Matrix
:return: 3x1 Matrix, angle
:rtype: (Matrix, Union[float, Symbol])
"""
# TODO buggy when angle == pi
rm = rotation_matrix
angle = (trace(rm[:3, :3]) - 1) / 2
angle = diffable_min_fast(angle, 1)
angle = diffable_max_fast(angle, -1)
angle = se.acos(angle)
x = (rm[2, 1] - rm[1, 2])
y = (rm[0, 2] - rm[2, 0])
z = (rm[1, 0] - rm[0, 1])
n = se.sqrt(x * x + y * y + z * z)
m = diffable_if_eq_zero(n, 1, n)
axis = se.Matrix([diffable_if_eq_zero(n, 0, x / m),
diffable_if_eq_zero(n, 0, y / m),
diffable_if_eq_zero(n, 1, z / m)])
return axis, angle
