def _solve_type_2(symo, X, Y, Z, th):
"""Solution for the equation:
X*S + Y*C = Z
"""
symo.write_line("# X*sin({0}) + Y*cos({0}) = Z".format(th))
X = symo.replace(symo.CS12_simp(X), "X", th)
Y = symo.replace(symo.CS12_simp(Y), "Y", th)
Z = symo.replace(symo.CS12_simp(Z), "Z", th)
YPS = var("YPS" + str(th))
if X == ZERO and Y != ZERO:
C = symo.replace(Z / Y, "C", th)
symo.add_to_dict(YPS, (ONE, -ONE))
symo.add_to_dict(th, atan2(YPS * sqrt(1 - C ** 2), C))
elif X != ZERO and Y == ZERO:
S = symo.replace(Z / X, "S", th)
symo.add_to_dict(YPS, (ONE, -ONE))
symo.add_to_dict(th, atan2(S, YPS * sqrt(1 - S ** 2)))
elif Z == ZERO:
symo.add_to_dict(YPS, (ONE, ZERO))
symo.add_to_dict(th, atan2(-Y, X) + YPS * pi)
else:
B = symo.replace(X ** 2 + Y ** 2, "B", th)
D = symo.replace(B - Z ** 2, "D", th)
symo.add_to_dict(YPS, (ONE, -ONE))
S = symo.replace((X * Z + YPS * Y * sqrt(D)) / B, "S", th)
C = symo.replace((Y * Z - YPS * X * sqrt(D)) / B, "C", th)
symo.add_to_dict(th, atan2(S, C))
def computeSymbolicModel(self):
"""
Symbollically computes G(X,t) and stores the models and lambda functions.
:return:
"""
# satellite position in ECI
x = self._stateSymb[0]
y = self._stateSymb[1]
z = self._stateSymb[2]
x_dot = self._stateSymb[3]
y_dot = self._stateSymb[4]
z_dot = self._stateSymb[5]
# x, y, z = sp.symbols('x y z')
r_xy = sp.sqrt(x**2+y**2)
#x_dot, y_dot, z_dot = sp.symbols('x_dot y_dot z_dot')
right_ascension = sp.atan2(y,x)
declination = sp.atan2(z,r_xy)
g1 = sp.lambdify((x, y, z, x_dot, y_dot, z_dot), right_ascension, "numpy")
g2 = sp.lambdify((x, y, z, x_dot, y_dot, z_dot), declination, "numpy")
self._modelSymb = [right_ascension, declination]
self._modelLambda = [g1, g2]
return self._modelSymb
def _solve_type_2(symo, X, Y, Z, th):
"""Solution for the equation:
X*S + Y*C = Z
"""
symo.write_line("# X*sin({0}) + Y*cos({0}) = Z".format(th))
X = symo.replace(trigsimp(X), 'X', th)
Y = symo.replace(trigsimp(Y), 'Y', th)
Z = symo.replace(trigsimp(Z), 'Z', th)
YPS = var('YPS'+str(th))
if X == tools.ZERO and Y != tools.ZERO:
C = symo.replace(Z/Y, 'C', th)
symo.add_to_dict(YPS, (tools.ONE, - tools.ONE))
symo.add_to_dict(th, atan2(YPS*sqrt(1-C**2), C))
elif X != tools.ZERO and Y == tools.ZERO:
S = symo.replace(Z/X, 'S', th)
symo.add_to_dict(YPS, (tools.ONE, - tools.ONE))
symo.add_to_dict(th, atan2(S, YPS*sqrt(1-S**2)))
elif Z == tools.ZERO:
symo.add_to_dict(YPS, (tools.ONE, tools.ZERO))
symo.add_to_dict(th, atan2(-Y, X) + YPS*pi)
else:
B = symo.replace(X**2 + Y**2, 'B', th)
D = symo.replace(B - Z**2, 'D', th)
symo.add_to_dict(YPS, (tools.ONE, - tools.ONE))
S = symo.replace((X*Z + YPS * Y * sqrt(D))/B, 'S', th)
C = symo.replace((Y*Z - YPS * X * sqrt(D))/B, 'C', th)
symo.add_to_dict(th, atan2(S, C))
def _set_inv_trans_equations(curv_coord_name):
"""
Store information about inverse transformation equations for
pre-defined coordinate systems.
Parameters
==========
curv_coord_name : str
Name of coordinate system
"""
if curv_coord_name == 'cartesian':
return lambda x, y, z: (x, y, z)
if curv_coord_name == 'spherical':
return lambda x, y, z: (
sqrt(x**2 + y**2 + z**2),
acos(z/sqrt(x**2 + y**2 + z**2)),
atan2(y, x)
)
if curv_coord_name == 'cylindrical':
return lambda x, y, z: (
sqrt(x**2 + y**2),
atan2(y, x),
z
)
raise ValueError('Wrong set of parameters.'
'Type of coordinate system is defined')
def _set_inv_trans_equations(self, curv_coord_name):
"""
Store information about some default, pre-defined inverse
transformation equations.
Parameters
==========
curv_coord_name : str
The type of the new coordinate system.
"""
equations_mapping = {
'cartesian': (self.x, self.y, self.z),
'spherical': (sqrt(self.x**2 + self.y**2 + self.z**2),
acos((self.z) / sqrt(self.x**2 + self.y**2 + self.z**2)),
atan2(self.y, self.x)),
'cylindrical': (sqrt(self.x**2 + self.y**2),
atan2(self.y, self.x),
self.z)
}
if curv_coord_name not in equations_mapping:
raise ValueError('Wrong set of parameters.'
'Type of coordinate system is defined')
self._inv_transformation_eqs = equations_mapping[curv_coord_name]
def test_im():
x, y = symbols('x,y')
a, b = symbols('a,b', real=True)
r = Symbol('r', real=True)
i = Symbol('i', imaginary=True)
assert im(nan) == nan
assert im(oo*I) == oo
assert im(-oo*I) == -oo
assert im(0) == 0
assert im(1) == 0
assert im(-1) == 0
assert im(E*I) == E
assert im(-E*I) == -E
assert im(x) == im(x)
assert im(x*I) == re(x)
assert im(r*I) == r
assert im(r) == 0
assert im(i*I) == 0
assert im(i) == -I * i
assert im(x + y) == im(x + y)
assert im(x + r) == im(x)
assert im(x + r*I) == im(x) + r
assert im(im(x)*I) == im(x)
assert im(2 + I) == 1
assert im(x + I) == im(x) + 1
assert im(x + y*I) == im(x) + re(y)
assert im(x + r*I) == im(x) + r
assert im(log(2*I)) == pi/2
assert im((2 + I)**2).expand(complex=True) == 4
assert im(conjugate(x)) == -im(x)
assert conjugate(im(x)) == im(x)
assert im(x).as_real_imag() == (im(x), 0)
assert im(i*r*x).diff(r) == im(i*x)
assert im(i*r*x).diff(i) == -I * re(r*x)
assert im(
sqrt(a + b*I)) == (a**2 + b**2)**Rational(1, 4)*sin(atan2(b, a)/2)
assert im(a * (2 + b*I)) == a*b
assert im((1 + sqrt(a + b*I))/2) == \
(a**2 + b**2)**Rational(1, 4)*sin(atan2(b, a)/2)/2
assert im(x).rewrite(re) == x - re(x)
assert (x + im(y)).rewrite(im, re) == x + y - re(y)
def get_border_coord(xy, other_xy, wh, n_type):
(x, y) = xy
(other_x, other_y) = other_xy
(w, h) = wh
if n_type == TYPE_REACTION:
edge_angle = degrees(atan2(other_y - y, other_x - x)) if other_y != y or other_x != x else 0
diag_angle = degrees(atan2(h, w))
abs_edge_angle = abs(edge_angle)
if diag_angle < abs_edge_angle < 180 - diag_angle:
y += h if edge_angle > 0 else -h
else:
x += w if abs_edge_angle <= 90 else -w
return x, y
elif n_type == TYPE_COMPARTMENT:
c_bottom_x, c_bottom_y, c_top_x, c_top_y = x - w, y - h, x + w, y + h
inside_y = c_bottom_y <= other_y <= c_top_y
inside_x = c_bottom_x <= other_x <= c_top_x
if inside_x:
return other_x, (c_bottom_y if abs(other_y - c_bottom_y) < abs(other_y - c_top_y) else c_top_y)
elif inside_y:
return (c_bottom_x if abs(other_x - c_bottom_x) < abs(other_x - c_top_x) else c_top_x), other_y
else:
return max(c_bottom_x, min(other_x, c_top_x)), max(c_bottom_y, min(other_y, c_top_y))
else:
diag = pow(pow(x - other_x, 2) + pow(y - other_y, 2), 0.5)
transformation = lambda z, other_z: (w * (((other_z - z) / diag) if diag else 1)) + z
return transformation(x, other_x), transformation(y, other_y)
def test_functional_differential_geometry_ch2():
# From "Functional Differential Geometry" as of 2011
# by Sussman and Wisdom.
x0, y0, r0, theta0 = symbols('x0, y0, r0, theta0', real=True)
x, y, r, theta = symbols('x, y, r, theta', real=True)
f = Function('f')
assert (R2_p.point_to_coords(R2_r.point([x0, y0])) ==
Matrix([sqrt(x0**2+y0**2), atan2(y0, x0)]))
assert (R2_r.point_to_coords(R2_p.point([r0, theta0])) ==
Matrix([r0*cos(theta0), r0*sin(theta0)]))
#TODO jacobian page 12 - 32
field = ScalarField(R2_r, [x, y], f(x, y))
p1_in_rect = R2_r.point([x0, y0])
p1_in_polar = R2_p.point([sqrt(x0**2 + y0**2), atan2(y0,x0)])
assert field(p1_in_rect) == f(x0, y0)
# TODO better simplification for the next one
#print simplify(field(p1_in_polar))
#assert simplify(field(p1_in_polar)) == f(x0, y0)
p_r = R2_r.point([x0, y0])
p_p = R2_p.point([r0, theta0])
assert R2.x(p_r) == x0
assert R2.x(p_p) == r0*cos(theta0)
assert R2.r(p_p) == r0
assert R2.r(p_r) == sqrt(x0**2 + y0**2)
assert R2.theta(p_r) == atan2(y0, x0)
h = R2.x*R2.r**2 + R2.y**3
assert h(p_r) == x0*(x0**2 + y0**2) + y0**3
assert h(p_p) == r0**3*sin(theta0)**3 + r0**3*cos(theta0)
def test_as_real_imag():
n = pi**1000
# the special code for working out the real
# and complex parts of a power with Integer exponent
# should not run if there is no imaginary part, hence
# this should not hang
assert n.as_real_imag() == (n, 0)
# issue 6261
x = Symbol('x')
assert sqrt(x).as_real_imag() == \
((re(x)**2 + im(x)**2)**(S(1)/4)*cos(atan2(im(x), re(x))/2),
(re(x)**2 + im(x)**2)**(S(1)/4)*sin(atan2(im(x), re(x))/2))
# issue 3853
a, b = symbols('a,b', real=True)
assert ((1 + sqrt(a + b*I))/2).as_real_imag() == \
(
(a**2 + b**2)**Rational(
1, 4)*cos(atan2(b, a)/2)/2 + Rational(1, 2),
(a**2 + b**2)**Rational(1, 4)*sin(atan2(b, a)/2)/2)
assert sqrt(a**2).as_real_imag() == (sqrt(a**2), 0)
i = symbols('i', imaginary=True)
assert sqrt(i**2).as_real_imag() == (0, abs(i))
def toeuler(quat):
"""Convert quaternion rotation to roll-pitch-yaw Euler angles."""
q0, q1, q2, q3 = quat
roll = sympy.atan2(2*(q2*q3 + q0*q1), q0**2 - q1**2 - q2**2 + q3**2)
pitch = -sympy.asin(2*(q1*q3 - q0*q2))
yaw = sympy.atan2(2*(q1*q2 + q0*q3), q0**2 + q1**2 - q2**2 - q3**2)
return np.array([roll, pitch, yaw])
def test_twave():
A1, phi1, A2, phi2, f = symbols('A1, phi1, A2, phi2, f')
n = Symbol('n') # Refractive index
t = Symbol('t') # Time
x = Symbol('x') # Spatial varaible
k = Symbol('k') # Wave number
E = Function('E')
w1 = TWave(A1, f, phi1)
w2 = TWave(A2, f, phi2)
assert w1.amplitude == A1
assert w1.frequency == f
assert w1.phase == phi1
assert w1.wavelength == c/(f*n)
assert w1.time_period == 1/f
w3 = w1 + w2
assert w3.amplitude == sqrt(A1**2 + 2*A1*A2*cos(phi1 - phi2) + A2**2)
assert w3.frequency == f
assert w3.wavelength == c/(f*n)
assert w3.time_period == 1/f
assert w3.angular_velocity == 2*pi*f
assert w3.wavenumber == 2*pi*f*n/c
assert simplify(w3.rewrite('sin') - sqrt(A1**2 + 2*A1*A2*cos(phi1 - phi2)
+ A2**2)*sin(pi*f*n*x*s/(149896229*m) - 2*pi*f*t + atan2(A1*cos(phi1)
+ A2*cos(phi2), A1*sin(phi1) + A2*sin(phi2)) + pi/2)) == 0
assert w3.rewrite('pde') == epsilon*mu*Derivative(E(x, t), t, t) + Derivative(E(x, t), x, x)
assert w3.rewrite(cos) == sqrt(A1**2 + 2*A1*A2*cos(phi1 - phi2)
+ A2**2)*cos(pi*f*n*x*s/(149896229*m) - 2*pi*f*t + atan2(A1*cos(phi1)
+ A2*cos(phi2), A1*sin(phi1) + A2*sin(phi2)))
assert w3.rewrite('exp') == sqrt(A1**2 + 2*A1*A2*cos(phi1 - phi2)
+ A2**2)*exp(I*(pi*f*n*x*s/(149896229*m) - 2*pi*f*t
+ atan2(A1*cos(phi1) + A2*cos(phi2), A1*sin(phi1) + A2*sin(phi2))))
def test_re():
x, y = symbols('x,y')
a, b = symbols('a,b', real=True)
r = Symbol('r', real=True)
i = Symbol('i', imaginary=True)
assert re(nan) == nan
assert re(oo) == oo
assert re(-oo) == -oo
assert re(0) == 0
assert re(1) == 1
assert re(-1) == -1
assert re(E) == E
assert re(-E) == -E
assert re(x) == re(x)
assert re(x*I) == -im(x)
assert re(r*I) == 0
assert re(r) == r
assert re(i*I) == I * i
assert re(i) == 0
assert re(x + y) == re(x + y)
assert re(x + r) == re(x) + r
assert re(re(x)) == re(x)
assert re(2 + I) == 2
assert re(x + I) == re(x)
assert re(x + y*I) == re(x) - im(y)
assert re(x + r*I) == re(x)
assert re(log(2*I)) == log(2)
assert re((2 + I)**2).expand(complex=True) == 3
assert re(conjugate(x)) == re(x)
assert conjugate(re(x)) == re(x)
assert re(x).as_real_imag() == (re(x), 0)
assert re(i*r*x).diff(r) == re(i*x)
assert re(i*r*x).diff(i) == I*r*im(x)
assert re(
sqrt(a + b*I)) == (a**2 + b**2)**Rational(1, 4)*cos(atan2(b, a)/2)
assert re(a * (2 + b*I)) == 2*a
assert re((1 + sqrt(a + b*I))/2) == \
(a**2 + b**2)**Rational(1, 4)*cos(atan2(b, a)/2)/2 + Rational(1, 2)
assert re(x).rewrite(im) == x - im(x)
assert (x + re(y)).rewrite(re, im) == x + y - im(y)
def points(self):
from sympy import atan2
res = []
for p in self.boundary_points:
x,y = p.point.x, p.point.y
rotation = atan2(p.next_inner.y - y, p.next_inner.x - x)
angle = atan2(p.prev_inner.y - y, p.prev_inner.x - x) - rotation
while angle < 0:
angle += 2*pi
if angle >= pi:
p.prev_inner, p.next_inner = p.next_inner, p.prev_inner
def test_as_real_imag():
n = pi**1000
# the special code for working out the real
# and complex parts of a power with Integer exponent
# should not run if there is no imaginary part, hence
# this should not hang
assert n.as_real_imag() == (n, 0)
# issue 3162
x = Symbol('x')
assert sqrt(x).as_real_imag() == \
((re(x)**2 + im(x)**2)**(S(1)/4)*cos(atan2(im(x), re(x))/2), \
(re(x)**2 + im(x)**2)**(S(1)/4)*sin(atan2(im(x), re(x))/2))
def assertPairAngle(self, dyn):
x1 = dyn.obja["tr.x"]
y1 = dyn.obja["tr.y"]
x2 = dyn.objb["tr.x"]
y2 = dyn.objb["tr.y"]
a1 = dyn.obja["rt.angle"]
a2 = dyn.objb["rt.angle"]
thetaa = dyn.thetaa
name = dyn.name
if not isTimeConstant(thetaa, self.symbols):
return True
# Convert the attachments to polar, add the body angle
# and then reconvert to rectangular
att1 = dyn.getAttachment(dyn.obja, "p")
if att1[0] != 0:
att1 = (att1[0], a1 + att1[1], att1[2])
i1, j1, m1 = Globals.convertAttachment(att1, "r")
else:
i1 = 0
j1 = 0
att2 = dyn.getAttachment(dyn.objb, "p")
if att2[0] != 0:
att2 = (att2[0], a2 + att2[1], att2[2])
i2, j2, m2 = Globals.convertAttachment(att2, "r")
else:
i2 = 0
j2 = 0
dx = sympy.sympify((x2 + i2) - (x1 + i1))
dy = sympy.sympify((y2 + j2) - (y1 + j1))
if dx == 0 and sympy.simplify(sympy.Mod(thetaa, sympy.pi)) != 0:
self.printer.print_diagnostic(
2,
"thetaa assertion failed for dynamic %s, set=%s, inferred=%s."
% (name, str(thetaa), str(sympy.sympify(sympy.atan2(dx, dy)))),
)
return False
elif dy == 0 and sympy.simplify(sympy.Mod(thetaa + sympy.pi / 2, sympy.pi)) != 0:
self.printer.print_diagnostic(
2,
"thetaa assertion failed for dynamic %s, set=%s, inferred=%s."
% (name, str(thetaa), str(sympy.sympify(sympy.atan2(dx, dy)))),
)
return False
return True
def sym_matrix_to_euler(mat): '''(symbolic!) Converts a rotation matrix (YXZ order) to 3 euler angles XYZ. Note that for simplicity this ignores the singularities.''' #XYZ #x = sympy.atan2(-mat[1, 2], mat[2, 2]) #y = sympy.asin(mat[0, 2]) #z = sympy.atan2(-mat[0, 1], mat[0, 0]) #YXZ x = sympy.asin(-mat[1, 2]) y = sympy.atan2(mat[0, 2], mat[2, 2]) z = sympy.atan2(mat[1, 0], mat[1, 1]) return [x, y, z]
def test_transformation_equations():
from sympy import symbols
x, y, z = symbols('x y z')
a = CoordSys3D('a')
# Str
a._connect_to_standard_cartesian('spherical')
assert a._transformation_equations() == (a.x * sin(a.y) * cos(a.z),
a.x * sin(a.y) * sin(a.z),
a.x * cos(a.y))
assert a.lame_coefficients() == (1, a.x, a.x * sin(a.y))
assert a._inverse_transformation_equations() == (sqrt(a.x**2 + a.y**2 +a.z**2),
acos((a.z) / sqrt(a.x**2 + a.y**2 + a.z**2)),
atan2(a.y, a.x))
a._connect_to_standard_cartesian('cylindrical')
assert a._transformation_equations() == (a.x * cos(a.y), a.x * sin(a.y), a.z)
assert a.lame_coefficients() == (1, a.y, 1)
assert a._inverse_transformation_equations() == (sqrt(a.x**2 + a.y**2),
atan2(a.y, a.x), a.z)
a._connect_to_standard_cartesian('cartesian')
assert a._transformation_equations() == (a.x, a.y, a.z)
assert a.lame_coefficients() == (1, 1, 1)
assert a._inverse_transformation_equations() == (a.x, a.y, a.z)
# Variables and expressions
a._connect_to_standard_cartesian(((x, y, z), (x, y, z)))
assert a._transformation_equations() == (a.x, a.y, a.z)
assert a.lame_coefficients() == (1, 1, 1)
assert a._inverse_transformation_equations() == (a.x, a.y, a.z)
a._connect_to_standard_cartesian(((x, y, z), ((x * cos(y), x * sin(y), z))), inverse=False)
assert a._transformation_equations() == (a.x * cos(a.y), a.x * sin(a.y), a.z)
assert simplify(a.lame_coefficients()) == (1, sqrt(a.x**2), 1)
a._connect_to_standard_cartesian(((x, y, z), (x * sin(y) * cos(z), x * sin(y) * sin(z), x * cos(y))), inverse=False)
assert a._transformation_equations() == (a.x * sin(a.y) * cos(a.z),
a.x * sin(a.y) * sin(a.z),
a.x * cos(a.y))
assert simplify(a.lame_coefficients()) == (1, sqrt(a.x**2), sqrt(sin(a.y)**2*a.x**2))
# Equations
a._connect_to_standard_cartesian((a.x*sin(a.y)*cos(a.z), a.x*sin(a.y)*sin(a.z), a.x*cos(a.y)), inverse=False)
assert a._transformation_equations() == (a.x * sin(a.y) * cos(a.z),
a.x * sin(a.y) * sin(a.z),
a.x * cos(a.y))
assert simplify(a.lame_coefficients()) == (1, sqrt(a.x**2), sqrt(sin(a.y)**2*a.x**2))
a._connect_to_standard_cartesian((a.x, a.y, a.z))
assert a._transformation_equations() == (a.x, a.y, a.z)
assert simplify(a.lame_coefficients()) == (1, 1, 1)
assert a._inverse_transformation_equations() == (a.x, a.y, a.z)
a._connect_to_standard_cartesian((a.x * cos(a.y), a.x * sin(a.y), a.z), inverse=False)
assert a._transformation_equations() == (a.x * cos(a.y), a.x * sin(a.y), a.z)
assert simplify(a.lame_coefficients()) == (1, sqrt(a.x**2), 1)
raises(ValueError, lambda: a._connect_to_standard_cartesian((x, y, z)))
def test_atan2_expansion():
assert cancel(atan2(x + 1, x ** 2).diff(x) - atan((x + 1) / x ** 2).diff(x)) == 0
assert cancel(atan(x / y).series(x, 0, 5) - atan2(x, y).series(x, 0, 5) + atan2(0, y) - atan(0)) == O(x ** 5)
assert cancel(atan(x / y).series(y, 1, 4) - atan2(x, y).series(y, 1, 4) + atan2(x, 1) - atan(x)) == O(y ** 4)
assert cancel(
atan((x + y) / y).series(y, 1, 3) - atan2(x + y, y).series(y, 1, 3) + atan2(1 + x, 1) - atan(1 + x)
) == O(y ** 3)
assert Matrix([atan2(x, y)]).jacobian([x, y]) == Matrix([[y / (x ** 2 + y ** 2), -x / (x ** 2 + y ** 2)]])
def test_atan2_expansion():
assert cancel(atan2(x ** 2, x + 1).diff(x) - atan(x ** 2 / (x + 1)).diff(x)) == 0
assert cancel(atan(y / x).series(y, 0, 5) - atan2(y, x).series(y, 0, 5) + atan2(0, x) - atan(0)) == O(y ** 5)
assert cancel(atan(y / x).series(x, 1, 4) - atan2(y, x).series(x, 1, 4) + atan2(y, 1) - atan(y)) == O(x ** 4)
assert cancel(
atan((y + x) / x).series(x, 1, 3) - atan2(y + x, x).series(x, 1, 3) + atan2(1 + y, 1) - atan(1 + y)
) == O(x ** 3)
assert Matrix([atan2(y, x)]).jacobian([y, x]) == Matrix([[x / (y ** 2 + x ** 2), -y / (y ** 2 + x ** 2)]])
def cartesian_to_polar(x,y, accurate=False):
from sympy import atan2, pi, sqrt
"""
Converts cartesian coordinates to polar coordinates.
INPUT:
- ``x`` -- Distance from origin on horizontal axis
- ``y`` -- Distance from origin on vertical axis
OUTPUT:
Polar coordinates (angle,distance) equivalent
to the given cartesian coordinates.
Angle will be in radians on the range [0, 2*pi[
and radius will be non-negative.
Furthermore the cartesian origin (0,0) will return (0,0)
even though (angle,0) for any angle would be equivalent.
"""
# origin
if x == 0 and y == 0:
return 0,0
r = sqrt(x**2 + y**2)
theta = atan2(y,x)
while theta < 0:
theta += 2*pi
return (theta, r) if accurate else (N(theta), N(r))
def inferPairAngle(self, obja, atta, objb, attb):
x1 = obja["tr.x"]
y1 = obja["tr.y"]
x2 = objb["tr.x"]
y2 = objb["tr.y"]
a1 = obja["rt.angle"]
a2 = objb["rt.angle"]
# Convert the attachments to polar, add the body angle
# and then reconvert to rectangular
att1 = Globals.convertAttachment(atta, "p")
if att1[0] != 0:
att1 = (att1[0], a1 + att1[1], att1[2])
i1, j1, m1 = Globals.convertAttachment(att1, "r")
else:
i1 = 0
j1 = 0
att2 = Globals.convertAttachment(attb, "p")
if att2[0] != 0:
att2 = (att2[0], a2 + att2[1], att2[2])
i2, j2, m2 = Globals.convertAttachment(att2, "r")
else:
i2 = 0
j2 = 0
dx = sympy.sympify((x2 + i2) - (x1 + i1))
dy = sympy.sympify((y2 + j2) - (y1 + j1))
return sympy.sympify(sympy.atan2(dx, dy))
def calculate_eigenvectors(self):
r"""Calculate the two eigenvectors :math:`\nu_i(x)` of the potential :math:`V(x)`.
We can do this by symbolic calculations.
Note: This function is idempotent and the eigenvectors are memoized for later reuse.
"""
# Assumption: The matrix is symmetric
# TODO: Consider generalization for arbitrary 2x2 matrices?
V1 = self._potential_s[0,0]
V2 = self._potential_s[0,1]
theta = sympy.Rational(1,2) * sympy.atan2(V2,V1)
# The two eigenvectors
upper = sympy.Matrix([[ sympy.cos(theta)], [sympy.sin(theta)]])
lower = sympy.Matrix([[-sympy.sin(theta)], [sympy.cos(theta)]])
# The symbolic expressions for the eigenvectors
self._eigenvectors_s = (upper, lower)
# The numerical functions for the eigenvectors
self._eigenvectors_n = []
# Attention, the components get listed in columns-wise order!
for vector in self._eigenvectors_s:
self._eigenvectors_n.append([ sympy.lambdify(self._variables, component, "numpy")
for component in vector ])
def brewster_angle(medium1, medium2):
"""
This function calculates the Brewster's angle of incidence to Medium 2 from
Medium 1 in radians.
Parameters
==========
medium 1 : Medium or sympifiable
Refractive index of Medium 1
medium 2 : Medium or sympifiable
Refractive index of Medium 1
Examples
========
>>> from sympy.physics.optics import brewster_angle
>>> brewster_angle(1, 1.33)
0.926093295503462
"""
if isinstance(medium1, Medium):
n1 = medium1.refractive_index
else:
n1 = sympify(medium1)
if isinstance(medium2, Medium):
n2 = medium2.refractive_index
else:
n2 = sympify(medium2)
return atan2(n2, n1)
def test_atan2():
assert refine(atan2(y, x), Q.real(y) & Q.positive(x)) == atan(y/x)
assert refine(atan2(y, x), Q.negative(y) & Q.positive(x)) == atan(y/x)
assert refine(atan2(y, x), Q.negative(y) & Q.negative(x)) == atan(y/x) - pi
assert refine(atan2(y, x), Q.positive(y) & Q.negative(x)) == atan(y/x) + pi
assert refine(atan2(y, x), Q.zero(y) & Q.negative(x)) == pi
assert refine(atan2(y, x), Q.positive(y) & Q.zero(x)) == pi/2
assert refine(atan2(y, x), Q.negative(y) & Q.zero(x)) == -pi/2
assert refine(atan2(y, x), Q.zero(y) & Q.zero(x)) == nan
def test_cot():
assert cot(nan) == nan
assert cot.nargs == FiniteSet(1)
assert cot(oo*I) == -I
assert cot(-oo*I) == I
assert cot(0) == zoo
assert cot(2*pi) == zoo
assert cot(acot(x)) == x
assert cot(atan(x)) == 1 / x
assert cot(asin(x)) == sqrt(1 - x**2) / x
assert cot(acos(x)) == x / sqrt(1 - x**2)
assert cot(atan2(y, x)) == x/y
assert cot(pi*I) == -coth(pi)*I
assert cot(-pi*I) == coth(pi)*I
assert cot(-2*I) == coth(2)*I
assert cot(pi) == cot(2*pi) == cot(3*pi)
assert cot(-pi) == cot(-2*pi) == cot(-3*pi)
assert cot(pi/2) == 0
assert cot(-pi/2) == 0
assert cot(5*pi/2) == 0
assert cot(7*pi/2) == 0
assert cot(pi/3) == 1/sqrt(3)
assert cot(-2*pi/3) == 1/sqrt(3)
assert cot(pi/4) == S.One
assert cot(-pi/4) == -S.One
assert cot(17*pi/4) == S.One
assert cot(-3*pi/4) == S.One
assert cot(pi/6) == sqrt(3)
assert cot(-pi/6) == -sqrt(3)
assert cot(7*pi/6) == sqrt(3)
assert cot(-5*pi/6) == sqrt(3)
assert cot(x*I) == -coth(x)*I
assert cot(k*pi*I) == -coth(k*pi)*I
assert cot(r).is_real is True
assert cot(a).is_algebraic is None
assert cot(na).is_algebraic is False
assert cot(10*pi/7) == cot(3*pi/7)
assert cot(11*pi/7) == -cot(3*pi/7)
assert cot(-11*pi/7) == cot(3*pi/7)
assert cot(x).is_finite is None
assert cot(r).is_finite is None
i = Symbol('i', imaginary=True)
assert cot(i).is_finite is True
assert cot(x).subs(x, 3*pi) == zoo
def test_tan():
x, y = symbols('x,y')
r = Symbol('r', real=True)
k = Symbol('k', integer=True)
assert tan(nan) == nan
assert tan(oo*I) == I
assert tan(-oo*I) == -I
assert tan(0) == 0
assert tan(atan(x)) == x
assert tan(asin(x)) == x / sqrt(1 - x**2)
assert tan(acos(x)) == sqrt(1 - x**2) / x
assert tan(acot(x)) == 1 / x
assert tan(atan2(y, x)) == y/x
assert tan(pi*I) == tanh(pi)*I
assert tan(-pi*I) == -tanh(pi)*I
assert tan(-2*I) == -tanh(2)*I
assert tan(pi) == 0
assert tan(-pi) == 0
assert tan(2*pi) == 0
assert tan(-2*pi) == 0
assert tan(-3*10**73*pi) == 0
assert tan(pi/2) == zoo
assert tan(3*pi/2) == zoo
assert tan(pi/3) == sqrt(3)
assert tan(-2*pi/3) == sqrt(3)
assert tan(pi/4) == S.One
assert tan(-pi/4) == -S.One
assert tan(17*pi/4) == S.One
assert tan(-3*pi/4) == S.One
assert tan(pi/6) == 1/sqrt(3)
assert tan(-pi/6) == -1/sqrt(3)
assert tan(7*pi/6) == 1/sqrt(3)
assert tan(-5*pi/6) == 1/sqrt(3)
assert tan(x*I) == tanh(x)*I
assert tan(k*pi) == 0
assert tan(17*k*pi) == 0
assert tan(k*pi*I) == tanh(k*pi)*I
assert tan(r).is_real is True
assert tan(10*pi/7) == tan(3*pi/7)
assert tan(11*pi/7) == -tan(3*pi/7)
assert tan(-11*pi/7) == tan(3*pi/7)
def test_tan():
assert tan(nan) == nan
assert tan.nargs == FiniteSet(1)
assert tan(oo*I) == I
assert tan(-oo*I) == -I
assert tan(0) == 0
assert tan(atan(x)) == x
assert tan(asin(x)) == x / sqrt(1 - x**2)
assert tan(acos(x)) == sqrt(1 - x**2) / x
assert tan(acot(x)) == 1 / x
assert tan(atan2(y, x)) == y/x
assert tan(pi*I) == tanh(pi)*I
assert tan(-pi*I) == -tanh(pi)*I
assert tan(-2*I) == -tanh(2)*I
assert tan(pi) == 0
assert tan(-pi) == 0
assert tan(2*pi) == 0
assert tan(-2*pi) == 0
assert tan(-3*10**73*pi) == 0
assert tan(pi/2) == zoo
assert tan(3*pi/2) == zoo
assert tan(pi/3) == sqrt(3)
assert tan(-2*pi/3) == sqrt(3)
assert tan(pi/4) == S.One
assert tan(-pi/4) == -S.One
assert tan(17*pi/4) == S.One
assert tan(-3*pi/4) == S.One
assert tan(pi/6) == 1/sqrt(3)
assert tan(-pi/6) == -1/sqrt(3)
assert tan(7*pi/6) == 1/sqrt(3)
assert tan(-5*pi/6) == 1/sqrt(3)
assert tan(x*I) == tanh(x)*I
assert tan(k*pi) == 0
assert tan(17*k*pi) == 0
assert tan(k*pi*I) == tanh(k*pi)*I
assert tan(r).is_real is True
assert tan(0, evaluate=False).is_algebraic
assert tan(a).is_algebraic is None
assert tan(na).is_algebraic is False
assert tan(10*pi/7) == tan(3*pi/7)
assert tan(11*pi/7) == -tan(3*pi/7)
assert tan(-11*pi/7) == tan(3*pi/7)
def translate_from(self, src_expansion, src_coeff_exprs, dvec):
from sumpy.symbolic import sympy_real_norm_2
k = sp.Symbol(self.kernel.get_base_kernel().helmholtz_k_name)
if isinstance(src_expansion, H2DLocalExpansion):
dvec_len = sympy_real_norm_2(dvec)
bessel_j = sp.Function("bessel_j")
new_center_angle_rel_old_center = sp.atan2(dvec[1], dvec[0])
translated_coeffs = []
for l in self.get_coefficient_identifiers():
translated_coeffs.append(
sum(
src_coeff_exprs[src_expansion.get_storage_index(m)]
* bessel_j(m - l, k * dvec_len)
* sp.exp(sp.I * (m - l) * -new_center_angle_rel_old_center)
for m in src_expansion.get_coefficient_identifiers()
)
)
return translated_coeffs
from sumpy.expansion.multipole import H2DMultipoleExpansion
if isinstance(src_expansion, H2DMultipoleExpansion):
dvec_len = sympy_real_norm_2(dvec)
hankel_1 = sp.Function("hankel_1")
new_center_angle_rel_old_center = sp.atan2(dvec[1], dvec[0])
translated_coeffs = []
for l in self.get_coefficient_identifiers():
translated_coeffs.append(
sum(
(-1) ** l
* hankel_1(m + l, k * dvec_len)
* sp.exp(sp.I * (m + l) * new_center_angle_rel_old_center)
* src_coeff_exprs[src_expansion.get_storage_index(m)]
for m in src_expansion.get_coefficient_identifiers()
)
)
return translated_coeffs
raise RuntimeError(
"do not know how to translate %s to " "local 2D Helmholtz Bessel expansion" % type(src_expansion).__name__
)
def test_functions_subs():
x, y = map(Symbol, 'xy')
f, g = map(Function, 'fg')
l = Lambda(x, y, sin(x) + y)
assert (g(y, x)+cos(x)).subs(g, l) == sin(y) + x + cos(x)
assert (f(x)**2).subs(f, sin) == sin(x)**2
assert (f(x,y)).subs(f,log) == log(x,y)
assert (f(x,y)).subs(f,sin) == f(x,y)
assert (sin(x)+atan2(x,y)).subs([[atan2,f],[sin,g]]) == f(x,y) + g(x)
assert (g(f(x+y, x))).subs([[f, l], [g, exp]]) == exp(x + sin(x + y))
def test_functions_subs():
x, y = symbols("x y")
f, g = symbols("f g", cls=Function)
l = Lambda((x, y), sin(x) + y)
assert (g(y, x) + cos(x)).subs(g, l) == sin(y) + x + cos(x)
assert (f(x) ** 2).subs(f, sin) == sin(x) ** 2
assert (f(x, y)).subs(f, log) == log(x, y)
assert (f(x, y)).subs(f, sin) == f(x, y)
assert (sin(x) + atan2(x, y)).subs([[atan2, f], [sin, g]]) == f(x, y) + g(x)
assert (g(f(x + y, x))).subs([[f, l], [g, exp]]) == exp(x + sin(x + y))
format_output_function=format_dict_title(
"Factor", "Times"),
eval_method=eval_factorization)
float_approximation = ResultCard("Floating-point approximation",
"(%s).evalf()", no_pre_output)
fractional_approximation = ResultCard("Fractional approximation",
"nsimplify(%s)", no_pre_output)
absolute_value = ResultCard("Absolute value", "Abs(%s)",
lambda s, *args: "|{}|".format(s))
polar_angle = ResultCard(
"Angle in the complex plane", "atan2(*(%s).as_real_imag()).evalf()",
lambda s, *args: sympy.atan2(*sympy.sympify(s).as_real_imag()))
conjugate = ResultCard("Complex conjugate", "conjugate(%s)",
lambda s, *args: sympy.conjugate(s))
trigexpand = ResultCard("Alternate form", "(%s).expand(trig=True)",
lambda statement, var, *args: statement)
trigsimp = ResultCard("Alternate form", "trigsimp(%s)",
lambda statement, var, *args: statement)
trigsincos = ResultCard("Alternate form",
"(%s).rewrite(csc, sin, sec, cos, cot, tan)",
lambda statement, var, *args: statement)
trigexp = ResultCard("Alternate form",
import pytest
import sympy as sp
import numpy as np
from shenfun import FunctionSpace, Function, project, TensorProductSpace, \
comm
x, y = sp.symbols('x,y', real=True)
f = sp.sin(sp.cos(x))
h = sp.sin(sp.cos(x)) * sp.atan2(y, x)
N = 16
def test_backward():
T = FunctionSpace(N, 'C')
L = FunctionSpace(N, 'L')
uT = Function(T, buffer=f)
uL = Function(L, buffer=f)
uLT = uL.backward(kind=T)
uT2 = project(uLT, T)
assert np.linalg.norm(uT2 - uT) < 1e-8
uTL = uT.backward(kind=L)
uL2 = project(uTL, L)
assert np.linalg.norm(uL2 - uL) < 1e-8
T2 = FunctionSpace(N, 'C', bc=(f.subs(x, -1), f.subs(x, 1)))
L = FunctionSpace(N, 'L')
uT = Function(T2, buffer=f)
def tick(self, tick):
Tm = tick.blackboard.get('Tm') # the current matrix equation
unknowns = tick.blackboard.get('unknowns')
R = tick.blackboard.get('Robot')
u = tick.blackboard.get('curr_unk')
#create a list of symbols for (unsolved) variables
# will use it for get_variable
unk_unsol = []
for unk in unknowns:
if not unk.solved:
unk_unsol.append(unk)
print 'tan_solve(): Not yet solved: ', unk.symbol
fsolved = False
if u.solvable_tan:
if (self.BHdebug):
print "tan_solve: I'm trying to solve: ", u.symbol
print " Using tangent() and 2 eqns:"
print u.eqntosolve
print u.secondeqn
fs = "tan_solve: Somethings Wrong!"
try:
x = u.eqntosolve.LHS
except:
print "problematic step: %s" % u.symbol
print u.eqntosolve
rhs = u.eqntosolve.RHS
Aw = sp.Wild("Aw")
Bw = sp.Wild("Bw")
d = rhs.match(Aw * sp.sin(u.symbol) + Bw)
assert (d != None), fs
assert (count_unknowns(unknowns, d[Bw]) == 0), fs
# now the second equation for this variable
x2 = u.secondeqn.LHS # it's 0
rhs2 = u.secondeqn.RHS
d2 = rhs2.match(Aw * sp.cos(u.symbol) + Bw)
assert (d2 != None), fs
assert (count_unknowns(unknowns, d2[Bw]) == 0), fs
#construct solutions
print 'tan_solver Denominators: ', d[Aw], d2[Aw]
co = d[Aw] / d2[Aw] #coefficients of Y and X
Y = x - d[Bw]
X = x2 - d2[Bw]
# the reason it can only test one d[Aw] is that
# the two eqn are pre-screened by the ID
# safer way to do it is to get the unsolved unknown number
# from d[Aw] and d2[Aw] and use the max
co_unk = get_variables(
unk_unsol,
d[Aw]) #get the cancelled unknown in the coefficients
fsolved = True
# if coefficient doesn't have unsolved unknowns
if len(co_unk) == 0:
# this is critical for "hidden dependency"
# can't use 'co', since it might have cancelled the parent (solved) variable
sol = sp.atan2(Y / d[Aw], X / d2[Aw])
u.solutions.append(sol)
u.tan_solutions.append(sol)
u.tan_eqnlist.append(u.eqntosolve)
u.tan_eqnlist.append(u.secondeqn)
u.nsolutions = 1
else:
sol1 = sp.atan2(Y / co, X)
sol2 = sp.atan2(-Y / co, -X)
u.solutions.append(sol1) #co_unk > 0
u.solutions.append(sol2) #co_unk < 0
u.tan_solutions.append(sol1)
u.tan_solutions.append(sol2)
u.tan_eqnlist.append(u.eqntosolve)
u.tan_eqnlist.append(u.secondeqn)
u.assumption.append(sp.Q.positive(
d[Aw])) # right way to say "non-zero"?
u.assumption.append(sp.Q.negative(d[Aw]))
u.nsolutions = 2
# note that set_solved is doen in ranker (ranking sincos, and tan sols)
if (fsolved):
tick.blackboard.set('curr_unk', u)
tick.blackboard.set('unknowns', unknowns)
tick.blackboard.set('Robot', R)
return b3.SUCCESS
else:
return b3.FAILURE
def Spherical_phi(x, y, z):
return atan2(y, x)
def test_im():
x, y = symbols('x,y')
a, b = symbols('a,b', real=True)
r = Symbol('r', real=True)
i = Symbol('i', imaginary=True)
assert im(nan) == nan
assert im(oo * I) == oo
assert im(-oo * I) == -oo
assert im(0) == 0
assert im(1) == 0
assert im(-1) == 0
assert im(E * I) == E
assert im(-E * I) == -E
assert im(x) == im(x)
assert im(x * I) == re(x)
assert im(r * I) == r
assert im(r) == 0
assert im(i * I) == 0
assert im(i) == -I * i
assert im(x + y) == im(x + y)
assert im(x + r) == im(x)
assert im(x + r * I) == im(x) + r
assert im(im(x) * I) == im(x)
assert im(2 + I) == 1
assert im(x + I) == im(x) + 1
assert im(x + y * I) == im(x) + re(y)
assert im(x + r * I) == im(x) + r
assert im(log(2 * I)) == pi / 2
assert im((2 + I)**2).expand(complex=True) == 4
assert im(conjugate(x)) == -im(x)
assert conjugate(im(x)) == im(x)
assert im(x).as_real_imag() == (im(x), 0)
assert im(i * r * x).diff(r) == im(i * x)
assert im(i * r * x).diff(i) == -I * re(r * x)
assert im(
sqrt(a +
b * I)) == (a**2 + b**2)**Rational(1, 4) * sin(atan2(b, a) / 2)
assert im(a * (2 + b * I)) == a * b
assert im((1 + sqrt(a + b*I))/2) == \
(a**2 + b**2)**Rational(1, 4)*sin(atan2(b, a)/2)/2
assert im(x).rewrite(re) == x - re(x)
assert (x + im(y)).rewrite(im, re) == x + y - re(y)
a = Symbol('a', algebraic=True)
t = Symbol('t', transcendental=True)
x = Symbol('x')
assert re(a).is_algebraic
assert re(x).is_algebraic is None
assert re(t).is_algebraic is False
def reference_metric():
global have_already_called_reference_metric_function # setting to global enables other modules to see updated value.
have_already_called_reference_metric_function = True
CoordSystem = par.parval_from_str("reference_metric::CoordSystem")
M_PI = par.Cparameters("REAL", thismodule, "M_PI")
global UnitVectors
UnitVectors = ixp.zerorank2(3)
# Set up hatted metric tensor, rescaling matrix, and rescaling vector
if CoordSystem == "Spherical" or CoordSystem == "SinhSpherical" or CoordSystem == "SinhSphericalv2":
# Assuming the spherical radial & theta coordinates
# are positive makes nice simplifications of
# unit vectors possible.
xx[0] = sp.symbols("xx0", real=True)
xx[1] = sp.symbols("xx1", real=True)
r = xx[0]
th = xx[1]
ph = xx[2]
if CoordSystem == "Spherical":
RMAX = par.Cparameters("REAL", thismodule, ["RMAX"])
global xxmin
global xxmax
xxmin = [sp.sympify(0), sp.sympify(0), -M_PI]
xxmax = [RMAX, M_PI, M_PI]
Cart_to_xx[0] = sp.sqrt(Cartx**2 + Carty**2 + Cartz**2)
Cart_to_xx[1] = sp.acos(Cartz / Cart_to_xx[0])
Cart_to_xx[2] = sp.atan2(Carty, Cartx)
elif CoordSystem == "SinhSpherical" or CoordSystem == "SinhSphericalv2":
AMPL, SINHW = par.Cparameters("REAL", thismodule,
["AMPL", "SINHW"])
xxmin = [0, 0, 0]
xxmax = [1, M_PI, 2 * M_PI]
# Set SinhSpherical radial coordinate by default; overwrite later if CoordSystem == "SinhSphericalv2".
r = AMPL * (sp.exp(xx[0] / SINHW) - sp.exp(-xx[0] / SINHW)) / \
(sp.exp(1 / SINHW) - sp.exp(-1 / SINHW))
Cart_to_xx[0] = SINHW * sp.asinh(
sp.sqrt(Cartx**2 + Carty**2 + Cartz**2) * sp.sinh(1 / SINHW) /
AMPL)
Cart_to_xx[1] = sp.acos(Cartz / Cart_to_xx[0])
Cart_to_xx[2] = sp.atan2(Carty, Cartx)
# SinhSphericalv2 adds the parameter "const_dr", which allows for a region near xx[0]=0 to have
# constant radial resolution of const_dr, provided the sinh() term does not dominate near xx[0]=0.
if CoordSystem == "SinhSphericalv2":
const_dr = par.Cparameters("REAL", thismodule, ["const_dr"])
r = AMPL * (const_dr * xx[0] +
(sp.exp(xx[0] / SINHW) - sp.exp(-xx[0] / SINHW)) /
(sp.exp(1 / SINHW) - sp.exp(-1 / SINHW)))
Cart_to_xx[0] = "NewtonRaphson"
Cart_to_xx[
1] = "NewtonRaphson" #sp.acos(Cartz / Cart_to_xx[0])
Cart_to_xx[2] = sp.atan2(Carty, Cartx)
xxSph[0] = r
xxSph[1] = th
xxSph[2] = ph
# Now define xCart, yCart, and zCart in terms of x0,xx[1],xx[2].
# Note that the relation between r and x0 is not necessarily trivial in SinhSpherical coordinates. See above.
xxCart[0] = xxSph[0] * sp.sin(xxSph[1]) * sp.cos(xxSph[2])
xxCart[1] = xxSph[0] * sp.sin(xxSph[1]) * sp.sin(xxSph[2])
xxCart[2] = xxSph[0] * sp.cos(xxSph[1])
scalefactor_orthog[0] = sp.diff(xxSph[0], xx[0])
scalefactor_orthog[1] = xxSph[0]
scalefactor_orthog[2] = xxSph[0] * sp.sin(xxSph[1])
# Set the unit vectors
UnitVectors = [[
sp.sin(xxSph[1]) * sp.cos(xxSph[2]),
sp.sin(xxSph[1]) * sp.sin(xxSph[2]),
sp.cos(xxSph[1])
],
[
sp.cos(xxSph[1]) * sp.cos(xxSph[2]),
sp.cos(xxSph[1]) * sp.sin(xxSph[2]),
-sp.sin(xxSph[1])
], [-sp.sin(xxSph[2]),
sp.cos(xxSph[2]),
sp.sympify(0)]]
elif CoordSystem == "Cylindrical" or CoordSystem == "SinhCylindrical" or CoordSystem == "SinhCylindricalv2":
# Assuming the cylindrical radial coordinate
# is positive makes nice simplifications of
# unit vectors possible.
xx[0] = sp.symbols("xx0", real=True)
RHOCYL = xx[0]
PHICYL = xx[1]
ZCYL = xx[2]
if CoordSystem == "Cylindrical":
RHOMAX, ZMIN, ZMAX = par.Cparameters("REAL", thismodule,
["RHOMAX", "ZMIN", "ZMAX"])
xxmin = [sp.sympify(0), -M_PI, ZMIN]
xxmax = [RHOMAX, M_PI, ZMAX]
Cart_to_xx[0] = sp.sqrt(Cartx**2 + Carty**2)
Cart_to_xx[1] = sp.atan2(Carty, Cartx)
Cart_to_xx[2] = Cartz
elif CoordSystem == "SinhCylindrical" or CoordSystem == "SinhCylindricalv2":
AMPLRHO, SINHWRHO, AMPLZ, SINHWZ = par.Cparameters(
"REAL", thismodule, ["AMPLRHO", "SINHWRHO", "AMPLZ", "SINHWZ"])
# Set SinhCylindrical radial & z coordinates by default; overwrite later if CoordSystem == "SinhCylindricalv2".
RHOCYL = AMPLRHO * (sp.exp(xx[0] / SINHWRHO) - sp.exp(
-xx[0] / SINHWRHO)) / (sp.exp(1 / SINHWRHO) -
sp.exp(-1 / SINHWRHO))
# phi coordinate remains unchanged.
PHICYL = xx[1]
ZCYL = AMPLZ * (sp.exp(xx[2] / SINHWZ) - sp.exp(
-xx[2] / SINHWZ)) / (sp.exp(1 / SINHWZ) - sp.exp(-1 / SINHWZ))
# SinhCylindricalv2 adds the parameters "const_drho", "const_dz", which allows for regions near xx[0]=0
# and xx[2]=0 to have constant rho and z resolution of const_drho and const_dz, provided the sinh() terms
# do not dominate near xx[0]=0 and xx[2]=0.
if CoordSystem == "SinhCylindricalv2":
const_drho, const_dz = par.Cparameters(
"REAL", thismodule, ["const_drho", "const_dz"])
RHOCYL = AMPLRHO * (
const_drho * xx[0] +
(sp.exp(xx[0] / SINHWRHO) - sp.exp(-xx[0] / SINHWRHO)) /
(sp.exp(1 / SINHWRHO) - sp.exp(-1 / SINHWRHO)))
ZCYL = AMPLZ * (
const_dz * xx[2] +
(sp.exp(xx[2] / SINHWZ) - sp.exp(-xx[2] / SINHWZ)) /
(sp.exp(1 / SINHWZ) - sp.exp(-1 / SINHWZ)))
xxmin = [sp.sympify(0), -M_PI, sp.sympify(-1)]
xxmax = [sp.sympify(1), M_PI, sp.sympify(+1)]
xxCart[0] = RHOCYL * sp.cos(PHICYL)
xxCart[1] = RHOCYL * sp.sin(PHICYL)
xxCart[2] = ZCYL
xxSph[0] = sp.sqrt(RHOCYL**2 + ZCYL**2)
xxSph[1] = sp.acos(ZCYL / xxSph[0])
xxSph[2] = PHICYL
scalefactor_orthog[0] = sp.diff(RHOCYL, xx[0])
scalefactor_orthog[1] = RHOCYL
scalefactor_orthog[2] = sp.diff(ZCYL, xx[2])
# Set the unit vectors
UnitVectors = [[sp.cos(PHICYL),
sp.sin(PHICYL),
sp.sympify(0)],
[-sp.sin(PHICYL),
sp.cos(PHICYL),
sp.sympify(0)],
[sp.sympify(0),
sp.sympify(0),
sp.sympify(1)]]
elif CoordSystem == "SymTP" or CoordSystem == "SinhSymTP":
var1, var2 = sp.symbols('var1 var2', real=True)
bScale, AW, AA, AMAX, RHOMAX, ZMIN, ZMAX = par.Cparameters(
"REAL", thismodule,
["bScale", "AW", "AA", "AMAX", "RHOMAX", "ZMIN", "ZMAX"])
# Assuming xx0, xx1, and bScale
# are positive makes nice simplifications of
# unit vectors possible.
xx[0], xx[1], bScale = sp.symbols("xx0 xx1 bScale", real=True)
xxmin = ["0.0", "0.0", "0.0"]
xxmax = ["params.AMAX", "M_PI", "2.0*M_PI"]
AA = xx[0]
if CoordSystem == "SinhSymTP":
AA = (sp.exp(xx[0] / AW) - sp.exp(-xx[0] / AW)) / 2
var1 = sp.sqrt(AA**2 + (bScale * sp.sin(xx[1]))**2)
var2 = sp.sqrt(AA**2 + bScale**2)
RHOSYMTP = AA * sp.sin(xx[1])
PHSYMTP = xx[2]
ZSYMTP = var2 * sp.cos(xx[1])
xxCart[0] = AA * sp.sin(xx[1]) * sp.cos(xx[2])
xxCart[1] = AA * sp.sin(xx[1]) * sp.sin(xx[2])
xxCart[2] = var2 * sp.cos(xx[1])
xxSph[0] = sp.sqrt(RHOSYMTP**2 + ZSYMTP**2)
xxSph[1] = sp.acos(ZSYMTP / xxSph[0])
xxSph[2] = PHSYMTP
scalefactor_orthog[0] = sp.diff(AA, xx[0]) * var1 / var2
scalefactor_orthog[1] = var1
scalefactor_orthog[2] = AA * sp.sin(xx[1])
# Set the transpose of the matrix of unit vectors
UnitVectors = [[
sp.sin(xx[1]) * sp.cos(xx[2]) * var2 / var1,
sp.sin(xx[1]) * sp.sin(xx[2]) * var2 / var1,
AA * sp.cos(xx[1]) / var1
],
[
AA * sp.cos(xx[1]) * sp.cos(xx[2]) / var1,
AA * sp.cos(xx[1]) * sp.sin(xx[2]) / var1,
-sp.sin(xx[1]) * var2 / var1
], [-sp.sin(xx[2]), sp.cos(xx[2]), 0]]
elif CoordSystem == "Cartesian":
xmin, xmax, ymin, ymax, zmin, zmax = par.Cparameters(
"REAL", thismodule,
["xmin", "xmax", "ymin", "ymax", "zmin", "zmax"])
xxmin = ["xmin", "ymin", "zmin"]
xxmax = ["xmax", "ymax", "zmax"]
xxCart[0] = xx[0]
xxCart[1] = xx[1]
xxCart[2] = xx[2]
xxSph[0] = sp.sqrt(xx[0]**2 + xx[1]**2 + xx[2]**2)
xxSph[1] = sp.acos(xx[2] / xxSph[0])
xxSph[2] = sp.atan2(xx[1], xx[0])
scalefactor_orthog[0] = sp.sympify(1)
scalefactor_orthog[1] = sp.sympify(1)
scalefactor_orthog[2] = sp.sympify(1)
# Set the transpose of the matrix of unit vectors
UnitVectors = [[sp.sympify(1),
sp.sympify(0),
sp.sympify(0)],
[sp.sympify(0),
sp.sympify(1),
sp.sympify(0)],
[sp.sympify(0),
sp.sympify(0),
sp.sympify(1)]]
else:
print("CoordSystem == " + CoordSystem + " is not supported.")
exit(1)
# Finally, call ref_metric__hatted_quantities()
# to construct hatted metric, derivs of hatted
# metric, and Christoffel symbols
ref_metric__hatted_quantities()
def test_re():
x, y = symbols('x,y')
a, b = symbols('a,b', real=True)
r = Symbol('r', real=True)
i = Symbol('i', imaginary=True)
assert re(nan) is nan
assert re(oo) is oo
assert re(-oo) is -oo
assert re(0) == 0
assert re(1) == 1
assert re(-1) == -1
assert re(E) == E
assert re(-E) == -E
assert unchanged(re, x)
assert re(x * I) == -im(x)
assert re(r * I) == 0
assert re(r) == r
assert re(i * I) == I * i
assert re(i) == 0
assert re(x + y) == re(x) + re(y)
assert re(x + r) == re(x) + r
assert re(re(x)) == re(x)
assert re(2 + I) == 2
assert re(x + I) == re(x)
assert re(x + y * I) == re(x) - im(y)
assert re(x + r * I) == re(x)
assert re(log(2 * I)) == log(2)
assert re((2 + I)**2).expand(complex=True) == 3
assert re(conjugate(x)) == re(x)
assert conjugate(re(x)) == re(x)
assert re(x).as_real_imag() == (re(x), 0)
assert re(i * r * x).diff(r) == re(i * x)
assert re(i * r * x).diff(i) == I * r * im(x)
assert re(
sqrt(a +
b * I)) == (a**2 + b**2)**Rational(1, 4) * cos(atan2(b, a) / 2)
assert re(a * (2 + b * I)) == 2 * a
assert re((1 + sqrt(a + b*I))/2) == \
(a**2 + b**2)**Rational(1, 4)*cos(atan2(b, a)/2)/2 + S.Half
assert re(x).rewrite(im) == x - S.ImaginaryUnit * im(x)
assert (x + re(y)).rewrite(re, im) == x + y - S.ImaginaryUnit * im(y)
a = Symbol('a', algebraic=True)
t = Symbol('t', transcendental=True)
x = Symbol('x')
assert re(a).is_algebraic
assert re(x).is_algebraic is None
assert re(t).is_algebraic == False
assert re(S.ComplexInfinity) is S.NaN
n, m, l = symbols('n m l')
A = MatrixSymbol('A', n, m)
assert re(A) == (S.Half) * (A + conjugate(A))
A = Matrix([[1 + 4 * I, 2], [0, -3 * I]])
assert re(A) == Matrix([[1, 2], [0, 0]])
A = ImmutableMatrix([[1 + 3 * I, 3 - 2 * I], [0, 2 * I]])
assert re(A) == ImmutableMatrix([[1, 3], [0, 0]])
X = SparseMatrix([[2 * j + i * I for i in range(5)] for j in range(5)])
assert re(X) - Matrix([[0, 0, 0, 0, 0], [2, 2, 2, 2, 2], [4, 4, 4, 4, 4],
[6, 6, 6, 6, 6], [8, 8, 8, 8, 8]
]) == Matrix.zeros(5)
assert im(X) - Matrix([[0, 1, 2, 3, 4], [0, 1, 2, 3, 4], [0, 1, 2, 3, 4],
[0, 1, 2, 3, 4], [0, 1, 2, 3, 4]
]) == Matrix.zeros(5)
X = FunctionMatrix(3, 3, Lambda((n, m), n + m * I))
assert re(X) == Matrix([[0, 0, 0], [1, 1, 1], [2, 2, 2]])
vcm = pCcm.time_derivative()
vcp=pCcp.time_derivative()
vcp2 = vcp.dot(vcp)
ve=pE.time_derivative()
ve2 = ve.dot(ve)
IC = Dyadic.build(C,I_11,I_11,I_11)
Body('BodyC',C,pCcm,mC,IC)
vcx = vcp.dot(C.x)
vcy = vcp.dot(-C.y)
angle_of_attack_C = sympy.atan2(vcy,vcx)
vex = ve.dot(E.x)
vey = ve.dot(-E.y)
angle_of_attack_E = sympy.atan2(vey,vex)
#cl = 2*sin(angle_of_attack)*cos(angle_of_attack)
#cd = 2*sin(angle_of_attack)**2
#fl = .5*rho*vcp2*cl*A
#fd = .5*rho*vcp2*cd*A
Area = 2*pi*r**2
f_aero_C = rho*vcp2*sympy.sin(angle_of_attack_C)*Area *C.y
def test_atan2():
assert atan2.nargs == FiniteSet(2)
assert atan2(0, 0) == S.NaN
assert atan2(0, 1) == 0
assert atan2(1, 1) == pi / 4
assert atan2(1, 0) == pi / 2
assert atan2(1, -1) == 3 * pi / 4
assert atan2(0, -1) == pi
assert atan2(-1, -1) == -3 * pi / 4
assert atan2(-1, 0) == -pi / 2
assert atan2(-1, 1) == -pi / 4
i = symbols('i', imaginary=True)
r = symbols('r', real=True)
eq = atan2(r, i)
ans = -I * log((i + I * r) / sqrt(i**2 + r**2))
reps = ((r, 2), (i, I))
assert eq.subs(reps) == ans.subs(reps)
u = Symbol("u", positive=True)
assert atan2(0, u) == 0
u = Symbol("u", negative=True)
assert atan2(0, u) == pi
assert atan2(y, oo) == 0
assert atan2(y, -oo) == 2 * pi * Heaviside(re(y)) - pi
assert atan2(y, x).rewrite(log) == -I * log(
(x + I * y) / sqrt(x**2 + y**2))
assert atan2(y, x).rewrite(atan) == 2 * atan(y / (x + sqrt(x**2 + y**2)))
ex = atan2(y, x) - arg(x + I * y)
assert ex.subs({x: 2, y: 3}).rewrite(arg) == 0
assert ex.subs({
x: 2,
y: 3 * I
}).rewrite(arg) == -pi - I * log(sqrt(5) * I / 5)
assert ex.subs({
x: 2 * I,
y: 3
}).rewrite(arg) == -pi / 2 - I * log(sqrt(5) * I)
assert ex.subs({
x: 2 * I,
y: 3 * I
}).rewrite(arg) == -pi + atan(2 / S(3)) + atan(3 / S(2))
i = symbols('i', imaginary=True)
r = symbols('r', real=True)
e = atan2(i, r)
rewrite = e.rewrite(arg)
reps = {i: I, r: -2}
assert rewrite == -I * log(abs(I * i + r) / sqrt(abs(i**2 + r**2))) + arg(
(I * i + r) / sqrt(i**2 + r**2))
assert (e - rewrite).subs(reps).equals(0)
assert conjugate(atan2(x, y)) == atan2(conjugate(x), conjugate(y))
assert diff(atan2(y, x), x) == -y / (x**2 + y**2)
assert diff(atan2(y, x), y) == x / (x**2 + y**2)
assert simplify(diff(atan2(y, x).rewrite(log), x)) == -y / (x**2 + y**2)
assert simplify(diff(atan2(y, x).rewrite(log), y)) == x / (x**2 + y**2)
def test_cos():
x, y = symbols('x y')
assert cos.nargs == FiniteSet(1)
assert cos(nan) == nan
assert cos(oo * I) == oo
assert cos(-oo * I) == oo
assert cos(0) == 1
assert cos(acos(x)) == x
assert cos(atan(x)) == 1 / sqrt(1 + x**2)
assert cos(asin(x)) == sqrt(1 - x**2)
assert cos(acot(x)) == 1 / sqrt(1 + 1 / x**2)
assert cos(atan2(y, x)) == x / sqrt(x**2 + y**2)
assert cos(pi * I) == cosh(pi)
assert cos(-pi * I) == cosh(pi)
assert cos(-2 * I) == cosh(2)
assert cos(pi / 2) == 0
assert cos(-pi / 2) == 0
assert cos(pi / 2) == 0
assert cos(-pi / 2) == 0
assert cos((-3 * 10**73 + 1) * pi / 2) == 0
assert cos((7 * 10**103 + 1) * pi / 2) == 0
n = symbols('n', integer=True)
assert cos(pi * n / 2) == 0
assert cos(pi) == -1
assert cos(-pi) == -1
assert cos(2 * pi) == 1
assert cos(5 * pi) == -1
assert cos(8 * pi) == 1
assert cos(pi / 3) == S.Half
assert cos(-2 * pi / 3) == -S.Half
assert cos(pi / 4) == S.Half * sqrt(2)
assert cos(-pi / 4) == S.Half * sqrt(2)
assert cos(11 * pi / 4) == -S.Half * sqrt(2)
assert cos(-3 * pi / 4) == -S.Half * sqrt(2)
assert cos(pi / 6) == S.Half * sqrt(3)
assert cos(-pi / 6) == S.Half * sqrt(3)
assert cos(7 * pi / 6) == -S.Half * sqrt(3)
assert cos(-5 * pi / 6) == -S.Half * sqrt(3)
assert cos(1 * pi / 5) == (sqrt(5) + 1) / 4
assert cos(2 * pi / 5) == (sqrt(5) - 1) / 4
assert cos(3 * pi / 5) == -cos(2 * pi / 5)
assert cos(4 * pi / 5) == -cos(1 * pi / 5)
assert cos(6 * pi / 5) == -cos(1 * pi / 5)
assert cos(8 * pi / 5) == cos(2 * pi / 5)
assert cos(-1273 * pi / 5) == -cos(2 * pi / 5)
assert cos(pi / 8) == sqrt((2 + sqrt(2)) / 4)
assert cos(104 * pi / 105) == -cos(pi / 105)
assert cos(106 * pi / 105) == -cos(pi / 105)
assert cos(-104 * pi / 105) == -cos(pi / 105)
assert cos(-106 * pi / 105) == -cos(pi / 105)
assert cos(x * I) == cosh(x)
assert cos(k * pi * I) == cosh(k * pi)
assert cos(r).is_real is True
assert cos(k * pi) == (-1)**k
assert cos(2 * k * pi) == 1
for d in list(range(1, 22)) + [60, 85]:
for n in xrange(0, 2 * d + 1):
x = n * pi / d
e = abs(float(cos(x)) - cos(float(x)))
assert e < 1e-12
def test_sin():
x, y = symbols('x y')
assert sin.nargs == FiniteSet(1)
assert sin(nan) == nan
assert sin(oo * I) == oo * I
assert sin(-oo * I) == -oo * I
assert sin(oo).args[0] == oo
assert sin(0) == 0
assert sin(asin(x)) == x
assert sin(atan(x)) == x / sqrt(1 + x**2)
assert sin(acos(x)) == sqrt(1 - x**2)
assert sin(acot(x)) == 1 / (sqrt(1 + 1 / x**2) * x)
assert sin(atan2(y, x)) == y / sqrt(x**2 + y**2)
assert sin(pi * I) == sinh(pi) * I
assert sin(-pi * I) == -sinh(pi) * I
assert sin(-2 * I) == -sinh(2) * I
assert sin(pi) == 0
assert sin(-pi) == 0
assert sin(2 * pi) == 0
assert sin(-2 * pi) == 0
assert sin(-3 * 10**73 * pi) == 0
assert sin(7 * 10**103 * pi) == 0
assert sin(pi / 2) == 1
assert sin(-pi / 2) == -1
assert sin(5 * pi / 2) == 1
assert sin(7 * pi / 2) == -1
n = symbols('n', integer=True)
assert sin(pi * n / 2) == (-1)**(n / 2 - S.Half)
assert sin(pi / 3) == S.Half * sqrt(3)
assert sin(-2 * pi / 3) == -S.Half * sqrt(3)
assert sin(pi / 4) == S.Half * sqrt(2)
assert sin(-pi / 4) == -S.Half * sqrt(2)
assert sin(17 * pi / 4) == S.Half * sqrt(2)
assert sin(-3 * pi / 4) == -S.Half * sqrt(2)
assert sin(pi / 6) == S.Half
assert sin(-pi / 6) == -S.Half
assert sin(7 * pi / 6) == -S.Half
assert sin(-5 * pi / 6) == -S.Half
assert sin(1 * pi / 5) == sqrt((5 - sqrt(5)) / 8)
assert sin(2 * pi / 5) == sqrt((5 + sqrt(5)) / 8)
assert sin(3 * pi / 5) == sin(2 * pi / 5)
assert sin(4 * pi / 5) == sin(1 * pi / 5)
assert sin(6 * pi / 5) == -sin(1 * pi / 5)
assert sin(8 * pi / 5) == -sin(2 * pi / 5)
assert sin(-1273 * pi / 5) == -sin(2 * pi / 5)
assert sin(pi / 8) == sqrt((2 - sqrt(2)) / 4)
assert sin(104 * pi / 105) == sin(pi / 105)
assert sin(106 * pi / 105) == -sin(pi / 105)
assert sin(-104 * pi / 105) == -sin(pi / 105)
assert sin(-106 * pi / 105) == sin(pi / 105)
assert sin(x * I) == sinh(x) * I
assert sin(k * pi) == 0
assert sin(17 * k * pi) == 0
assert sin(k * pi * I) == sinh(k * pi) * I
assert sin(r).is_real is True
assert isinstance(sin(re(x) - im(y)), sin) is True
assert isinstance(sin(-re(x) + im(y)), sin) is False
for d in list(range(1, 22)) + [60, 85]:
for n in xrange(0, d * 2 + 1):
x = n * pi / d
e = abs(float(sin(x)) - sin(float(x)))
assert e < 1e-12
def angle(p1, p2):
ang = sp.atan2(p2[1] - p1[1], p2[0] - p1[0])
return ang
def handle_calculate_IK(req):
rospy.loginfo("Received %s eef-poses from the plan" % len(req.poses))
if len(req.poses) < 1:
print "No valid poses received"
return -1
else:
q1, q2, q3, q4, q5, q6, q7 = symbols('q1:8') #Theta
d1, d2, d3, d4, d5, d6, d7 = symbols('d1:8') #d-offset
a0, a1, a2, a3, a4, a5, a6 = symbols('a0:7') #
alpha0, alpha1, alpha2, alpha3, alpha4, alpha5, alpha6 = symbols(
'alpha0:7') # alpha twist angle..
'''dh = {
a0: 0, alpha0: 0, q1: q1, d1: 0.75,
a1: 0.35, alpha1: pi/2, q2: q2, d2: 0.00,
a2: 1.25, alpha2: 0, q3: q3 + pi/2, d3: 0.00,
a3: -0.054, alpha3: pi/2, q4: q4, d4: 1.50,
a4: 0, alpha4: pi/2, q5: q5, d5: 0.00,
a5: 0, alpha5: pi/2, q6: q6, d6: 0.00,
a6: 0, alpha6: 0, q7: 0, d7: 0.303
}'''
dh = {
a0: 0,
alpha0: 0,
q1: q1,
d1: 0.75,
a1: 0.35,
alpha1: -pi / 2,
q2: q2 - pi / 2,
d2: 0.00,
a2: 1.25,
alpha2: 0,
q3: q3,
d3: 0.00,
a3: -0.054,
alpha3: -pi / 2,
q4: q4,
d4: 1.50,
a4: 0,
alpha4: pi / 2,
q5: q5,
d5: 0.00,
a5: 0,
alpha5: -pi / 2,
q6: q6,
d6: 0.00,
a6: 0,
alpha6: 0,
q7: 0,
d7: 0.303
}
def homTransform(a, alpha, q, d):
Hm = Matrix([[cos(q), -sin(q), 0, a],
[
sin(q) * cos(alpha),
cos(alpha) * cos(q), -sin(alpha), -sin(alpha) * d
],
[
sin(alpha) * sin(q),
sin(alpha) * cos(q),
cos(alpha),
cos(alpha) * d
], [0, 0, 0, 1]])
return Hm
# Define Rotation Matrices around X, Y, and Z
def Rot_X(q):
R_x = Matrix([[1, 0, 0], [0, cos(q), -sin(q)], [0,
sin(q),
cos(q)]])
return R_x
def Rot_Y(q):
R_y = Matrix([[cos(q), 0, sin(q)], [0, 1, 0], [-sin(q), 0,
cos(q)]])
return R_y
def Rot_Z(q):
R_z = Matrix([
[cos(q), -sin(q), 0],
[sin(q), cos(q), 0],
[0, 0, 0],
])
return R_z
def calculateJointAngles(Wc):
Wc_x = Wc[0]
Wc_y = Wc[1]
Wc_z = Wc[2]
sqd = sqrt(Wc_x**2 + Wc_y**2)
theta_1 = atan2(Wc[1], Wc[0])
a = 1.501
b = sqrt(pow((sqd - 0.35), 2) + pow((Wc_z - 0.75), 2))
c = 1.25
angle_a = acos((b * b + c * c - a * a) / (2 * b * c))
angle_b = acos((a * a + c * c - b * b) / (2 * a * c))
angle_c = acos((a * a - c * c + b * b) / (2 * a * b))
delta = atan2(Wc_z - 0.75, sqd - 0.35)
theta2 = pi / 2 - (angle_a + delta)
theta3 = pi / 2 - (angle_b + 0.036)
return (theta1, theta2, theta3)
#Transformation matrices:
T0_1 = homTransform(a0, alpha0, q1, d1)
T0_1 = T0_1.subs(dh)
T1_2 = homTransform(a1, alpha1, q2, d2)
T1_2 = T1_2.subs(dh)
T2_3 = homTransform(a2, alpha2, q3, d3)
T2_3 = T2_3.subs(dh)
T3_4 = homTransform(a3, alpha3, q4, d4)
T3_4 = T3_4.subs(dh)
T5_6 = homTransform(a5, alpha5, q6, d6)
T5_6 = T5_6.subs(dh)
T6_G = homTransform(a6, alpha6, q7, d7)
T6_G = T6_G.subs(dh)
#FInal TRansformation matrix from base to gripper..
T0_G = simplify(T0_1 * T1_2 * T2_3 * T3_4 * T5_6 * T6_G)
#------------------------------------------------------------------------------------
# Inverse Kinematics Part starts here.......
#------------------------------------------------------------------------------------
# Initialize service response
joint_trajectory_list = []
for x in xrange(0, len(req.poses)):
joint_trajectory_point = JointTrajectoryPoint()
#End Effector Position
px = req.poses[x].position.x
py = req.poses[x].position.y
pz = req.poses[x].position.z
EE_Matrix = Matrix([[px], [py], [pz]])
#End Effector Orientation angles
(roll, pitch, yaw) = tf.transformations.euler_from_quaternion([
req.poses[x].orientation.x, req.poses[x].orientation.y,
req.poses[x].orientation.z, req.poses[x].orientation.w
])
r, p, y = symbols('r, p, y')
#Intrinsic rotation applied on end-effector.
R_EE = Rot_Z(y) * Rot_Y(p) * Rot_X(r)
#Rotation Error
RotationError = Rot_Z(pi) * Rot_Y(-pi / 2)
R_EE = R_EE * RotationError
# Substitute the End Effector Orientation angles for r, p, y
R_EE = R_EE.subs({'r': roll, 'p': pitch, 'y': yaw})
#Wrist Center Position
Wc = EE_Matrix - 0.303 * R_EE[:, 2]
#Compute the Joint angles 1,2 & 3 from wrist center positions
theta1, theta2, theta3 = calculateJointAngles(Wc)
# Evaluate the Rotation Matrix from {0} to {3} with the obtained
# theta1, theta2 & theta3 values.
R0_3 = T0_1[0:3, 0:3] * T1_2[0:3, 0:3] * T2_3[0:3, 0:3]
R0_3 = R0_3.evalf(subs={q1: theta1, q2: theta2, q3: theta3})
R0_3_Tp = R0_3.T
# As we know that R_EE = R0_3 * R3_6 and inv(R0_3) = Transpose(R3_6) we can write,
R3_6 = R0_3_Tp * R_EE
# Now that we know the Rotation matrix from {3} to {6} and the
# End Effector Orientation at {6}. So from R3_6, Euler angles can be extracted
# and equalled with obtained roll, pitch and yaw angles.
theta4 = atan2(R3_6[2, 2], -R3_6[0, 2])
theta5 = atan2(sqrt(R3_6[0, 2]**2 + R3_6[2, 2]**2), R3_6[1, 2])
theta6 = atan2(-R3_6[1, 1], R3_6[1, 0])
joint_trajectory_point.positions = [
theta1, theta2, theta3, theta4, theta5, theta6
]
joint_trajectory_list.append(joint_trajectory_point)
rospy.loginfo("length of Joint Trajectory List: %s" %
len(joint_trajectory_list))
return CalculateIKResponse(joint_trajectory_list)
def test_transformation_equations():
x, y, z = symbols('x y z')
# Str
a = CoordSys3D('a',
transformation='spherical',
variable_names=["r", "theta", "phi"])
r, theta, phi = a.base_scalars()
assert r == a.r
assert theta == a.theta
assert phi == a.phi
raises(AttributeError, lambda: a.x)
raises(AttributeError, lambda: a.y)
raises(AttributeError, lambda: a.z)
assert a.transformation_to_parent() == (r * sin(theta) * cos(phi),
r * sin(theta) * sin(phi),
r * cos(theta))
assert a.lame_coefficients() == (1, r, r * sin(theta))
assert a.transformation_from_parent_function()(
x, y, z) == (sqrt(x**2 + y**2 + z**2),
acos((z) / sqrt(x**2 + y**2 + z**2)), atan2(y, x))
a = CoordSys3D('a',
transformation='cylindrical',
variable_names=["r", "theta", "z"])
r, theta, z = a.base_scalars()
assert a.transformation_to_parent() == (r * cos(theta), r * sin(theta), z)
assert a.lame_coefficients() == (1, a.r, 1)
assert a.transformation_from_parent_function()(x, y,
z) == (sqrt(x**2 + y**2),
atan2(y, x), z)
a = CoordSys3D('a', 'cartesian')
assert a.transformation_to_parent() == (a.x, a.y, a.z)
assert a.lame_coefficients() == (1, 1, 1)
assert a.transformation_from_parent_function()(x, y, z) == (x, y, z)
# Variables and expressions
# Cartesian with equation tuple:
x, y, z = symbols('x y z')
a = CoordSys3D('a', ((x, y, z), (x, y, z)))
a._calculate_inv_trans_equations()
assert a.transformation_to_parent() == (a.x1, a.x2, a.x3)
assert a.lame_coefficients() == (1, 1, 1)
assert a.transformation_from_parent_function()(x, y, z) == (x, y, z)
r, theta, z = symbols("r theta z")
# Cylindrical with equation tuple:
a = CoordSys3D('a', [(r, theta, z), (r * cos(theta), r * sin(theta), z)],
variable_names=["r", "theta", "z"])
r, theta, z = a.base_scalars()
assert a.transformation_to_parent() == (r * cos(theta), r * sin(theta), z)
assert a.lame_coefficients() == (
sqrt(sin(theta)**2 + cos(theta)**2),
sqrt(r**2 * sin(theta)**2 + r**2 * cos(theta)**2), 1
) # ==> this should simplify to (1, r, 1), tests are too slow with `simplify`.
# Definitions with `lambda`:
# Cartesian with `lambda`
a = CoordSys3D('a', lambda x, y, z: (x, y, z))
assert a.transformation_to_parent() == (a.x1, a.x2, a.x3)
assert a.lame_coefficients() == (1, 1, 1)
a._calculate_inv_trans_equations()
assert a.transformation_from_parent_function()(x, y, z) == (x, y, z)
# Spherical with `lambda`
a = CoordSys3D(
'a',
lambda r, theta, phi:
(r * sin(theta) * cos(phi), r * sin(theta) * sin(phi), r * cos(theta)),
variable_names=["r", "theta", "phi"])
r, theta, phi = a.base_scalars()
assert a.transformation_to_parent() == (r * sin(theta) * cos(phi),
r * sin(phi) * sin(theta),
r * cos(theta))
assert a.lame_coefficients() == (
sqrt(
sin(phi)**2 * sin(theta)**2 + sin(theta)**2 * cos(phi)**2 +
cos(theta)**2),
sqrt(r**2 * sin(phi)**2 * cos(theta)**2 + r**2 * sin(theta)**2 +
r**2 * cos(phi)**2 * cos(theta)**2),
sqrt(r**2 * sin(phi)**2 * sin(theta)**2 +
r**2 * sin(theta)**2 * cos(phi)**2)
) # ==> this should simplify to (1, r, sin(theta)*r), `simplify` is too slow.
# Cylindrical with `lambda`
a = CoordSys3D('a',
lambda r, theta, z: (r * cos(theta), r * sin(theta), z),
variable_names=["r", "theta", "z"])
r, theta, z = a.base_scalars()
assert a.transformation_to_parent() == (r * cos(theta), r * sin(theta), z)
assert a.lame_coefficients() == (
sqrt(sin(theta)**2 + cos(theta)**2),
sqrt(r**2 * sin(theta)**2 + r**2 * cos(theta)**2), 1
) # ==> this should simplify to (1, a.x, 1)
raises(
TypeError, lambda: CoordSys3D('a',
transformation={
x: x * sin(y) * cos(z),
y: x * sin(y) * sin(z),
z: x * cos(y)
}))
def add_secular_terms(self,
order=2,
fixed_Lambdas=True,
indexIn=1,
indexOut=2):
G = symbols('G')
mOut, MOut, LambdaOut, lambdaOut, GammaOut, gammaOut, XOut, YOut = symbols(
'm{0},M{0},Lambda{0},lambda{0},Gamma{0},gamma{0},X{0},Y{0}'.format(
indexOut))
mIn, MIn, LambdaIn, lambdaIn, GammaIn, gammaIn, XIn, YIn = symbols(
'm{0},M{0},Lambda{0},lambda{0},Gamma{0},gamma{0},X{0},Y{0}'.format(
indexIn))
eIn, eOut, gammaIn, gammaOut = symbols("e e' gamma gamma'")
hIn, kIn, hOut, kOut = symbols("h,k,h',k'")
# Work smarter not harder! Use an expression it is already available...
if order not in self.secular_terms.keys():
print("Computing secular expansion to order %d..." % order)
subdict = {
eIn: sqrt(hIn * hIn + kIn * kIn),
eOut: sqrt(hOut * hOut + kOut * kOut),
gammaIn: atan2(-1 * kIn, hIn),
gammaOut: atan2(-1 * kOut, hOut),
}
self.secular_terms[order] = secular_DF(
eIn, eOut, gammaIn, gammaOut, order).subs(subdict,
simultaneous=True)
exprn = self.secular_terms[order]
salpha = S("alpha{0}{1}".format(indexIn, indexOut))
if order == 2:
subdict = {
hIn: XIn / sqrt(LambdaIn),
kIn: (-1) * YIn / sqrt(LambdaIn),
hOut: XOut / sqrt(LambdaOut),
kOut: (-1) * YOut / sqrt(LambdaOut),
S("alpha"): salpha
}
else:
# fix this to include higher order terms
subdict = {
hIn: XIn / sqrt(LambdaIn),
kIn: (-1) * YIn / sqrt(LambdaIn),
hOut: XOut / sqrt(LambdaOut),
kOut: (-1) * YOut / sqrt(LambdaOut),
S("alpha"): salpha
}
alpha = self.particles[indexIn].a / self.particles[indexOut].a
self.Hparams[salpha] = alpha
self.Hparams[Function('b')] = laplace_B
exprn = exprn.subs(subdict)
# substitute a fixed value for Lambdas in DF terms
prefactor = -G**2 * MOut**2 * mOut**3 * (mIn / MIn) / (LambdaOut**2)
exprn = prefactor * exprn
if fixed_Lambdas:
LambdaIn0, LambdaOut0 = symbols("Lambda{0}0 Lambda{1}0".format(
indexIn, indexOut))
self.Hparams[LambdaIn0] = self.state.particles[indexIn].Lambda
self.Hparams[LambdaOut0] = self.state.particles[indexOut].Lambda
exprn = exprn.subs([(LambdaIn, LambdaIn0),
(LambdaOut, LambdaOut0)])
self.H += exprn
self._update()
def test_im():
x, y = symbols('x,y')
a, b = symbols('a,b', real=True)
r = Symbol('r', real=True)
i = Symbol('i', imaginary=True)
assert im(nan) is nan
assert im(oo * I) is oo
assert im(-oo * I) is -oo
assert im(0) == 0
assert im(1) == 0
assert im(-1) == 0
assert im(E * I) == E
assert im(-E * I) == -E
assert unchanged(im, x)
assert im(x * I) == re(x)
assert im(r * I) == r
assert im(r) == 0
assert im(i * I) == 0
assert im(i) == -I * i
assert im(x + y) == im(x) + im(y)
assert im(x + r) == im(x)
assert im(x + r * I) == im(x) + r
assert im(im(x) * I) == im(x)
assert im(2 + I) == 1
assert im(x + I) == im(x) + 1
assert im(x + y * I) == im(x) + re(y)
assert im(x + r * I) == im(x) + r
assert im(log(2 * I)) == pi / 2
assert im((2 + I)**2).expand(complex=True) == 4
assert im(conjugate(x)) == -im(x)
assert conjugate(im(x)) == im(x)
assert im(x).as_real_imag() == (im(x), 0)
assert im(i * r * x).diff(r) == im(i * x)
assert im(i * r * x).diff(i) == -I * re(r * x)
assert im(
sqrt(a +
b * I)) == (a**2 + b**2)**Rational(1, 4) * sin(atan2(b, a) / 2)
assert im(a * (2 + b * I)) == a * b
assert im((1 + sqrt(a + b*I))/2) == \
(a**2 + b**2)**Rational(1, 4)*sin(atan2(b, a)/2)/2
assert im(x).rewrite(re) == -S.ImaginaryUnit * (x - re(x))
assert (x + im(y)).rewrite(im, re) == x - S.ImaginaryUnit * (y - re(y))
a = Symbol('a', algebraic=True)
t = Symbol('t', transcendental=True)
x = Symbol('x')
assert re(a).is_algebraic
assert re(x).is_algebraic is None
assert re(t).is_algebraic == False
assert im(S.ComplexInfinity) is S.NaN
n, m, l = symbols('n m l')
A = MatrixSymbol('A', n, m)
assert im(A) == (S.One / (2 * I)) * (A - conjugate(A))
A = Matrix([[1 + 4 * I, 2], [0, -3 * I]])
assert im(A) == Matrix([[4, 0], [0, -3]])
A = ImmutableMatrix([[1 + 3 * I, 3 - 2 * I], [0, 2 * I]])
assert im(A) == ImmutableMatrix([[3, -2], [0, 2]])
X = ImmutableSparseMatrix([[i * I + i for i in range(5)]
for i in range(5)])
Y = SparseMatrix([[i for i in range(5)] for i in range(5)])
assert im(X).as_immutable() == Y
X = FunctionMatrix(3, 3, Lambda((n, m), n + m * I))
assert im(X) == Matrix([[0, 1, 2], [0, 1, 2], [0, 1, 2]])
def test_create_new():
a = CoordSys3D('a')
c = a.create_new('c', transformation='spherical')
assert c._parent == a
assert c.transformation_to_parent() == \
(c.r*sin(c.theta)*cos(c.phi), c.r*sin(c.theta)*sin(c.phi), c.r*cos(c.theta))
assert c.transformation_from_parent() == \
(sqrt(a.x**2 + a.y**2 + a.z**2), acos(a.z/sqrt(a.x**2 + a.y**2 + a.z**2)), atan2(a.y, a.x))
for key in keys:
sorted_ls.extend(list_d[key])
return sorted_ls
############# main test the library #########################
#
if __name__ == "__main__": # tester code for the classes in this file
#j1 = joint_var(th_12)
sp.var('a b c d e')
a = sp.atan2(b,c) # make sure this function compiles/loads
# Test .subs operator on atan2() function
print 'Original Function: ', a
print 'Substitute b<-e: ', a.subs(b,e), ' (Expect atanw(e, c))'
assert(a.subs(b,e) == sp.atan2(e,c))
###Test the Left Hand Side Generator
m = ik_lhs()
fs = 'ik_lhs() matrix generator FAIL'
assert (m[0,0] == sp.var('r_11')), fs
assert (m[0,1] == sp.var('r_12')),fs
assert (m[3,3] == 1), fs
class MathematicaParser:
"""
An instance of this class converts a string of a Wolfram Mathematica
expression to a SymPy expression.
The main parser acts internally in three stages:
1. tokenizer: tokenizes the Mathematica expression and adds the missing *
operators. Handled by ``_from_mathematica_to_tokens(...)``
2. full form list: sort the list of strings output by the tokenizer into a
syntax tree of nested lists and strings, equivalent to Mathematica's
``FullForm`` expression output. This is handled by the function
``_from_tokens_to_fullformlist(...)``.
3. SymPy expression: the syntax tree expressed as full form list is visited
and the nodes with equivalent classes in SymPy are replaced. Unknown
syntax tree nodes are cast to SymPy ``Function`` objects. This is
handled by ``_from_fullformlist_to_sympy(...)``.
"""
# left: Mathematica, right: SymPy
CORRESPONDENCES = {
'Sqrt[x]': 'sqrt(x)',
'Exp[x]': 'exp(x)',
'Log[x]': 'log(x)',
'Log[x,y]': 'log(y,x)',
'Log2[x]': 'log(x,2)',
'Log10[x]': 'log(x,10)',
'Mod[x,y]': 'Mod(x,y)',
'Max[*x]': 'Max(*x)',
'Min[*x]': 'Min(*x)',
'Pochhammer[x,y]': 'rf(x,y)',
'ArcTan[x,y]': 'atan2(y,x)',
'ExpIntegralEi[x]': 'Ei(x)',
'SinIntegral[x]': 'Si(x)',
'CosIntegral[x]': 'Ci(x)',
'AiryAi[x]': 'airyai(x)',
'AiryAiPrime[x]': 'airyaiprime(x)',
'AiryBi[x]': 'airybi(x)',
'AiryBiPrime[x]': 'airybiprime(x)',
'LogIntegral[x]': ' li(x)',
'PrimePi[x]': 'primepi(x)',
'Prime[x]': 'prime(x)',
'PrimeQ[x]': 'isprime(x)'
}
# trigonometric, e.t.c.
for arc, tri, h in product(
('', 'Arc'), ('Sin', 'Cos', 'Tan', 'Cot', 'Sec', 'Csc'), ('', 'h')):
fm = arc + tri + h + '[x]'
if arc: # arc func
fs = 'a' + tri.lower() + h + '(x)'
else: # non-arc func
fs = tri.lower() + h + '(x)'
CORRESPONDENCES.update({fm: fs})
REPLACEMENTS = {
' ': '',
'^': '**',
'{': '[',
'}': ']',
}
RULES = {
# a single whitespace to '*'
'whitespace': (re.compile(
r'''
(?:(?<=[a-zA-Z\d])|(?<=\d\.)) # a letter or a number
\s+ # any number of whitespaces
(?:(?=[a-zA-Z\d])|(?=\.\d)) # a letter or a number
''', re.VERBOSE), '*'),
# add omitted '*' character
'add*_1': (re.compile(
r'''
(?:(?<=[])\d])|(?<=\d\.)) # ], ) or a number
# ''
(?=[(a-zA-Z]) # ( or a single letter
''', re.VERBOSE), '*'),
# add omitted '*' character (variable letter preceding)
'add*_2': (re.compile(
r'''
(?<=[a-zA-Z]) # a letter
\( # ( as a character
(?=.) # any characters
''', re.VERBOSE), '*('),
# convert 'Pi' to 'pi'
'Pi': (re.compile(
r'''
(?:
\A|(?<=[^a-zA-Z])
)
Pi # 'Pi' is 3.14159... in Mathematica
(?=[^a-zA-Z])
''', re.VERBOSE), 'pi'),
}
# Mathematica function name pattern
FM_PATTERN = re.compile(
r'''
(?:
\A|(?<=[^a-zA-Z]) # at the top or a non-letter
)
[A-Z][a-zA-Z\d]* # Function
(?=\[) # [ as a character
''', re.VERBOSE)
# list or matrix pattern (for future usage)
ARG_MTRX_PATTERN = re.compile(
r'''
\{.*\}
''', re.VERBOSE)
# regex string for function argument pattern
ARGS_PATTERN_TEMPLATE = r'''
(?:
\A|(?<=[^a-zA-Z])
)
{arguments} # model argument like x, y,...
(?=[^a-zA-Z])
'''
# will contain transformed CORRESPONDENCES dictionary
TRANSLATIONS = {} # type: tDict[tTuple[str, int], tDict[str, Any]]
# cache for a raw users' translation dictionary
cache_original = {} # type: tDict[tTuple[str, int], tDict[str, Any]]
# cache for a compiled users' translation dictionary
cache_compiled = {} # type: tDict[tTuple[str, int], tDict[str, Any]]
@classmethod
def _initialize_class(cls):
# get a transformed CORRESPONDENCES dictionary
d = cls._compile_dictionary(cls.CORRESPONDENCES)
cls.TRANSLATIONS.update(d)
def __init__(self, additional_translations=None):
self.translations = {}
# update with TRANSLATIONS (class constant)
self.translations.update(self.TRANSLATIONS)
if additional_translations is None:
additional_translations = {}
# check the latest added translations
if self.__class__.cache_original != additional_translations:
if not isinstance(additional_translations, dict):
raise ValueError('The argument must be dict type')
# get a transformed additional_translations dictionary
d = self._compile_dictionary(additional_translations)
# update cache
self.__class__.cache_original = additional_translations
self.__class__.cache_compiled = d
# merge user's own translations
self.translations.update(self.__class__.cache_compiled)
@classmethod
def _compile_dictionary(cls, dic):
# for return
d = {}
for fm, fs in dic.items():
# check function form
cls._check_input(fm)
cls._check_input(fs)
# uncover '*' hiding behind a whitespace
fm = cls._apply_rules(fm, 'whitespace')
fs = cls._apply_rules(fs, 'whitespace')
# remove whitespace(s)
fm = cls._replace(fm, ' ')
fs = cls._replace(fs, ' ')
# search Mathematica function name
m = cls.FM_PATTERN.search(fm)
# if no-hit
if m is None:
err = "'{f}' function form is invalid.".format(f=fm)
raise ValueError(err)
# get Mathematica function name like 'Log'
fm_name = m.group()
# get arguments of Mathematica function
args, end = cls._get_args(m)
# function side check. (e.g.) '2*Func[x]' is invalid.
if m.start() != 0 or end != len(fm):
err = "'{f}' function form is invalid.".format(f=fm)
raise ValueError(err)
# check the last argument's 1st character
if args[-1][0] == '*':
key_arg = '*'
else:
key_arg = len(args)
key = (fm_name, key_arg)
# convert '*x' to '\\*x' for regex
re_args = [x if x[0] != '*' else '\\' + x for x in args]
# for regex. Example: (?:(x|y|z))
xyz = '(?:(' + '|'.join(re_args) + '))'
# string for regex compile
patStr = cls.ARGS_PATTERN_TEMPLATE.format(arguments=xyz)
pat = re.compile(patStr, re.VERBOSE)
# update dictionary
d[key] = {}
d[key]['fs'] = fs # SymPy function template
d[key]['args'] = args # args are ['x', 'y'] for example
d[key]['pat'] = pat
return d
def _convert_function(self, s):
'''Parse Mathematica function to SymPy one'''
# compiled regex object
pat = self.FM_PATTERN
scanned = '' # converted string
cur = 0 # position cursor
while True:
m = pat.search(s)
if m is None:
# append the rest of string
scanned += s
break
# get Mathematica function name
fm = m.group()
# get arguments, and the end position of fm function
args, end = self._get_args(m)
# the start position of fm function
bgn = m.start()
# convert Mathematica function to SymPy one
s = self._convert_one_function(s, fm, args, bgn, end)
# update cursor
cur = bgn
# append converted part
scanned += s[:cur]
# shrink s
s = s[cur:]
return scanned
def _convert_one_function(self, s, fm, args, bgn, end):
# no variable-length argument
if (fm, len(args)) in self.translations:
key = (fm, len(args))
# x, y,... model arguments
x_args = self.translations[key]['args']
# make CORRESPONDENCES between model arguments and actual ones
d = {k: v for k, v in zip(x_args, args)}
# with variable-length argument
elif (fm, '*') in self.translations:
key = (fm, '*')
# x, y,..*args (model arguments)
x_args = self.translations[key]['args']
# make CORRESPONDENCES between model arguments and actual ones
d = {}
for i, x in enumerate(x_args):
if x[0] == '*':
d[x] = ','.join(args[i:])
break
d[x] = args[i]
# out of self.translations
else:
err = "'{f}' is out of the whitelist.".format(f=fm)
raise ValueError(err)
# template string of converted function
template = self.translations[key]['fs']
# regex pattern for x_args
pat = self.translations[key]['pat']
scanned = ''
cur = 0
while True:
m = pat.search(template)
if m is None:
scanned += template
break
# get model argument
x = m.group()
# get a start position of the model argument
xbgn = m.start()
# add the corresponding actual argument
scanned += template[:xbgn] + d[x]
# update cursor to the end of the model argument
cur = m.end()
# shrink template
template = template[cur:]
# update to swapped string
s = s[:bgn] + scanned + s[end:]
return s
@classmethod
def _get_args(cls, m):
'''Get arguments of a Mathematica function'''
s = m.string # whole string
anc = m.end() + 1 # pointing the first letter of arguments
square, curly = [], [] # stack for brakets
args = []
# current cursor
cur = anc
for i, c in enumerate(s[anc:], anc):
# extract one argument
if c == ',' and (not square) and (not curly):
args.append(s[cur:i]) # add an argument
cur = i + 1 # move cursor
# handle list or matrix (for future usage)
if c == '{':
curly.append(c)
elif c == '}':
curly.pop()
# seek corresponding ']' with skipping irrevant ones
if c == '[':
square.append(c)
elif c == ']':
if square:
square.pop()
else: # empty stack
args.append(s[cur:i])
break
# the next position to ']' bracket (the function end)
func_end = i + 1
return args, func_end
@classmethod
def _replace(cls, s, bef):
aft = cls.REPLACEMENTS[bef]
s = s.replace(bef, aft)
return s
@classmethod
def _apply_rules(cls, s, bef):
pat, aft = cls.RULES[bef]
return pat.sub(aft, s)
@classmethod
def _check_input(cls, s):
for bracket in (('[', ']'), ('{', '}'), ('(', ')')):
if s.count(bracket[0]) != s.count(bracket[1]):
err = "'{f}' function form is invalid.".format(f=s)
raise ValueError(err)
if '{' in s:
err = "Currently list is not supported."
raise ValueError(err)
def _parse_old(self, s):
# input check
self._check_input(s)
# uncover '*' hiding behind a whitespace
s = self._apply_rules(s, 'whitespace')
# remove whitespace(s)
s = self._replace(s, ' ')
# add omitted '*' character
s = self._apply_rules(s, 'add*_1')
s = self._apply_rules(s, 'add*_2')
# translate function
s = self._convert_function(s)
# '^' to '**'
s = self._replace(s, '^')
# 'Pi' to 'pi'
s = self._apply_rules(s, 'Pi')
# '{', '}' to '[', ']', respectively
# s = cls._replace(s, '{') # currently list is not taken into account
# s = cls._replace(s, '}')
return s
def parse(self, s):
s2 = self._from_mathematica_to_tokens(s)
s3 = self._from_tokens_to_fullformlist(s2)
s4 = self._from_fullformlist_to_sympy(s3)
return s4
INFIX = "Infix"
PREFIX = "Prefix"
POSTFIX = "Postfix"
FLAT = "Flat"
RIGHT = "Right"
LEFT = "Left"
_mathematica_op_precedence: List[tTuple[str, Optional[str], tDict[
str, tUnion[str, Callable]]]] = [
(POSTFIX, None, {
";":
lambda x: x + ["Null"] if isinstance(x, list) and x and x[0] ==
"CompoundExpression" else ["CompoundExpression", x, "Null"]
}),
(INFIX, FLAT, {
";": "CompoundExpression"
}),
(INFIX, RIGHT, {
"=": "Set",
":=": "SetDelayed",
"+=": "AddTo",
"-=": "SubtractFrom",
"*=": "TimesBy",
"/=": "DivideBy"
}),
(INFIX, LEFT, {
"//": lambda x, y: [x, y]
}),
(POSTFIX, None, {
"&": "Function"
}),
(INFIX, LEFT, {
"/.": "ReplaceAll"
}),
(INFIX, RIGHT, {
"->": "Rule",
":>": "RuleDelayed"
}),
(INFIX, LEFT, {
"/;": "Condition"
}),
(INFIX, FLAT, {
"|": "Alternatives"
}),
(POSTFIX, None, {
"..": "Repeated",
"...": "RepeatedNull"
}),
(INFIX, FLAT, {
"||": "Or"
}),
(INFIX, FLAT, {
"&&": "And"
}),
(PREFIX, None, {
"!": "Not"
}),
(INFIX, FLAT, {
"===": "SameQ",
"=!=": "UnsameQ"
}),
(INFIX, FLAT, {
"==": "Equal",
"!=": "Unequal",
"<=": "LessEqual",
"<": "Less",
">=": "GreaterEqual",
">": "Greater"
}),
(INFIX, None, {
";;": "Span"
}),
(INFIX, FLAT, {
"+": "Plus",
"-": "Plus"
}),
(INFIX, FLAT, {
"*": "Times",
"/": "Times"
}),
(INFIX, FLAT, {
".": "Dot"
}),
(PREFIX, None, {
"-": lambda x: MathematicaParser._get_neg(x),
"+": lambda x: x
}),
(INFIX, RIGHT, {
"^": "Power"
}),
(INFIX, RIGHT, {
"@@": "Apply",
"/@": "Map",
"//@": "MapAll",
"@@@": lambda x, y: ["Apply", x, y, ["List", "1"]]
}),
(POSTFIX, None, {
"'": "Derivative",
"!": "Factorial",
"!!": "Factorial2",
"--": "Decrement"
}),
(INFIX, None, {
"[": lambda x, y: [x, *y],
"[[": lambda x, y: ["Part", x, *y]
}),
(PREFIX, None, {
"{": lambda x: ["List", *x],
"(": lambda x: x[0]
}),
(INFIX, None, {
"?": "PatternTest"
}),
(POSTFIX, None, {
"_": lambda x: ["Pattern", x, ["Blank"]],
"_.": lambda x: ["Optional", ["Pattern", x, ["Blank"]]],
"__": lambda x: ["Pattern", x, ["BlankSequence"]],
"___": lambda x: ["Pattern", x, ["BlankNullSequence"]],
}),
(INFIX, None, {
"_": lambda x, y: ["Pattern", x, ["Blank", y]]
}),
(PREFIX, None, {
"#": "Slot",
"##": "SlotSequence"
}),
]
_missing_arguments_default = {
"#": lambda: ["Slot", "1"],
"##": lambda: ["SlotSequence", "1"],
}
_literal = r"[A-Za-z][A-Za-z0-9]*"
_number = r"(?:[0-9]+(?:\.[0-9]*)?|\.[0-9]+)"
_enclosure_open = ["(", "[", "[[", "{"]
_enclosure_close = [")", "]", "]]", "}"]
@classmethod
def _get_neg(cls, x):
return f"-{x}" if isinstance(x, str) and re.match(
MathematicaParser._number, x) else ["Times", "-1", x]
@classmethod
def _get_inv(cls, x):
return ["Power", x, "-1"]
_regex_tokenizer = None
def _get_tokenizer(self):
if self._regex_tokenizer is not None:
# Check if the regular expression has already been compiled:
return self._regex_tokenizer
tokens = [self._literal, self._number]
tokens_escape = self._enclosure_open[:] + self._enclosure_close[:]
for typ, strat, symdict in self._mathematica_op_precedence:
for k in symdict:
tokens_escape.append(k)
tokens_escape.sort(key=lambda x: -len(x))
tokens.extend(map(re.escape, tokens_escape))
tokens.append(",")
tokens.append("\n")
tokenizer = re.compile("(" + "|".join(tokens) + ")")
self._regex_tokenizer = tokenizer
return self._regex_tokenizer
def _from_mathematica_to_tokens(self, code: str):
tokenizer = self._get_tokenizer()
# Find strings:
code_splits: List[typing.Union[str, list]] = []
while True:
string_start = code.find("\"")
if string_start == -1:
if len(code) > 0:
code_splits.append(code)
break
match_end = re.search(r'(?<!\\)"', code[string_start + 1:])
if match_end is None:
raise SyntaxError('mismatch in string " " expression')
string_end = string_start + match_end.start() + 1
if string_start > 0:
code_splits.append(code[:string_start])
code_splits.append([
"_Str", code[string_start + 1:string_end].replace('\\"', '"')
])
code = code[string_end + 1:]
# Remove comments:
for i, code_split in enumerate(code_splits):
if isinstance(code_split, list):
continue
while True:
pos_comment_start = code_split.find("(*")
if pos_comment_start == -1:
break
pos_comment_end = code_split.find("*)")
if pos_comment_end == -1 or pos_comment_end < pos_comment_start:
raise SyntaxError("mismatch in comment (* *) code")
code_split = code_split[:pos_comment_start] + code_split[
pos_comment_end + 2:]
code_splits[i] = code_split
# Tokenize the input strings with a regular expression:
token_lists = [
tokenizer.findall(i) if isinstance(i, str) else [i]
for i in code_splits
]
tokens = [j for i in token_lists for j in i]
# Remove newlines at the beginning
while tokens and tokens[0] == "\n":
tokens.pop(0)
# Remove newlines at the end
while tokens and tokens[-1] == "\n":
tokens.pop(-1)
return tokens
def _is_op(self, token: tUnion[str, list]) -> bool:
if isinstance(token, list):
return False
if re.match(self._literal, token):
return False
if re.match("-?" + self._number, token):
return False
return True
def _is_valid_star1(self, token: tUnion[str, list]) -> bool:
if token in (")", "}"):
return True
return not self._is_op(token)
def _is_valid_star2(self, token: tUnion[str, list]) -> bool:
if token in ("(", "{"):
return True
return not self._is_op(token)
def _from_tokens_to_fullformlist(self, tokens: list):
stack: List[list] = [[]]
open_seq = []
pointer: int = 0
while pointer < len(tokens):
token = tokens[pointer]
if token in self._enclosure_open:
stack[-1].append(token)
open_seq.append(token)
stack.append([])
elif token == ",":
if len(stack[-1]) == 0 and stack[-2][-1] == open_seq[-1]:
raise SyntaxError("%s cannot be followed by comma ," %
open_seq[-1])
stack[-1] = self._parse_after_braces(stack[-1])
stack.append([])
elif token in self._enclosure_close:
ind = self._enclosure_close.index(token)
if self._enclosure_open[ind] != open_seq[-1]:
unmatched_enclosure = SyntaxError("unmatched enclosure")
if token == "]]" and open_seq[-1] == "[":
if open_seq[-2] == "[":
# These two lines would be logically correct, but are
# unnecessary:
# token = "]"
# tokens[pointer] = "]"
tokens.insert(pointer + 1, "]")
elif open_seq[-2] == "[[":
if tokens[pointer + 1] == "]":
tokens[pointer + 1] = "]]"
elif tokens[pointer + 1] == "]]":
tokens[pointer + 1] = "]]"
tokens.insert(pointer + 2, "]")
else:
raise unmatched_enclosure
else:
raise unmatched_enclosure
if len(stack[-1]) == 0 and stack[-2][-1] == "(":
raise SyntaxError("( ) not valid syntax")
last_stack = self._parse_after_braces(stack[-1], True)
stack[-1] = last_stack
new_stack_element = []
while stack[-1][-1] != open_seq[-1]:
new_stack_element.append(stack.pop())
new_stack_element.reverse()
if open_seq[-1] == "(" and len(new_stack_element) != 1:
raise SyntaxError(
"( must be followed by one expression, %i detected" %
len(new_stack_element))
stack[-1].append(new_stack_element)
open_seq.pop(-1)
else:
stack[-1].append(token)
pointer += 1
assert len(stack) == 1
return self._parse_after_braces(stack[0])
def _util_remove_newlines(self, lines: list, tokens: list,
inside_enclosure: bool):
pointer = 0
size = len(tokens)
while pointer < size:
token = tokens[pointer]
if token == "\n":
if inside_enclosure:
# Ignore newlines inside enclosures
tokens.pop(pointer)
size -= 1
continue
if pointer == 0:
tokens.pop(0)
size -= 1
continue
if pointer > 1:
try:
prev_expr = self._parse_after_braces(
tokens[:pointer], inside_enclosure)
except SyntaxError:
tokens.pop(pointer)
size -= 1
continue
else:
prev_expr = tokens[0]
if len(prev_expr) > 0 and prev_expr[0] == "CompoundExpression":
lines.extend(prev_expr[1:])
else:
lines.append(prev_expr)
for i in range(pointer):
tokens.pop(0)
size -= pointer
pointer = 0
continue
pointer += 1
def _util_add_missing_asterisks(self, tokens: list):
size: int = len(tokens)
pointer: int = 0
while pointer < size:
if (pointer > 0 and self._is_valid_star1(tokens[pointer - 1])
and self._is_valid_star2(tokens[pointer])):
# This is a trick to add missing * operators in the expression,
# `"*" in op_dict` makes sure the precedence level is the same as "*",
# while `not self._is_op( ... )` makes sure this and the previous
# expression are not operators.
if tokens[pointer] == "(":
# ( has already been processed by now, replace:
tokens[pointer] = "*"
tokens[pointer + 1] = tokens[pointer + 1][0]
else:
tokens.insert(pointer, "*")
pointer += 1
size += 1
pointer += 1
def _parse_after_braces(self,
tokens: list,
inside_enclosure: bool = False):
op_dict: dict
changed: bool = False
lines: list = []
self._util_remove_newlines(lines, tokens, inside_enclosure)
for op_type, grouping_strat, op_dict in reversed(
self._mathematica_op_precedence):
if "*" in op_dict:
self._util_add_missing_asterisks(tokens)
size: int = len(tokens)
pointer: int = 0
while pointer < size:
token = tokens[pointer]
if isinstance(token, str) and token in op_dict:
op_name: tUnion[str, Callable] = op_dict[token]
node: list
first_index: int
if isinstance(op_name, str):
node = [op_name]
first_index = 1
else:
node = []
first_index = 0
if token in (
"+", "-"
) and op_type == self.PREFIX and pointer > 0 and not self._is_op(
tokens[pointer - 1]):
# Make sure that PREFIX + - don't match expressions like a + b or a - b,
# the INFIX + - are supposed to match that expression:
pointer += 1
continue
if op_type == self.INFIX:
if pointer == 0 or pointer == size - 1 or self._is_op(
tokens[pointer - 1]) or self._is_op(
tokens[pointer + 1]):
pointer += 1
continue
changed = True
tokens[pointer] = node
if op_type == self.INFIX:
arg1 = tokens.pop(pointer - 1)
arg2 = tokens.pop(pointer)
if token == "/":
arg2 = self._get_inv(arg2)
elif token == "-":
arg2 = self._get_neg(arg2)
pointer -= 1
size -= 2
node.append(arg1)
node_p = node
if grouping_strat == self.FLAT:
while pointer + 2 < size and self._check_op_compatible(
tokens[pointer + 1], token):
node_p.append(arg2)
other_op = tokens.pop(pointer + 1)
arg2 = tokens.pop(pointer + 1)
if other_op == "/":
arg2 = self._get_inv(arg2)
elif other_op == "-":
arg2 = self._get_neg(arg2)
size -= 2
node_p.append(arg2)
elif grouping_strat == self.RIGHT:
while pointer + 2 < size and tokens[pointer +
1] == token:
node_p.append([op_name, arg2])
node_p = node_p[-1]
tokens.pop(pointer + 1)
arg2 = tokens.pop(pointer + 1)
size -= 2
node_p.append(arg2)
elif grouping_strat == self.LEFT:
while pointer + 1 < size and tokens[pointer +
1] == token:
if isinstance(op_name, str):
node_p[first_index] = [
op_name, node_p[first_index], arg2
]
else:
node_p[first_index] = op_name(
node_p[first_index], arg2)
tokens.pop(pointer + 1)
arg2 = tokens.pop(pointer + 1)
size -= 2
node_p.append(arg2)
else:
node.append(arg2)
elif op_type == self.PREFIX:
assert grouping_strat is None
if pointer == size - 1 or self._is_op(
tokens[pointer + 1]):
tokens[pointer] = self._missing_arguments_default[
token]()
else:
node.append(tokens.pop(pointer + 1))
size -= 1
elif op_type == self.POSTFIX:
assert grouping_strat is None
if pointer == 0 or self._is_op(tokens[pointer - 1]):
tokens[pointer] = self._missing_arguments_default[
token]()
else:
node.append(tokens.pop(pointer - 1))
pointer -= 1
size -= 1
if isinstance(op_name, Callable): # type: ignore
op_call: Callable = typing.cast(Callable, op_name)
new_node = op_call(*node)
node.clear()
if isinstance(new_node, list):
node.extend(new_node)
else:
tokens[pointer] = new_node
pointer += 1
if len(tokens) > 1 or (len(lines) == 0 and len(tokens) == 0):
if changed:
# Trick to deal with cases in which an operator with lower
# precedence should be transformed before an operator of higher
# precedence. Such as in the case of `#&[x]` (that is
# equivalent to `Lambda(d_, d_)(x)` in SymPy). In this case the
# operator `&` has lower precedence than `[`, but needs to be
# evaluated first because otherwise `# (&[x])` is not a valid
# expression:
return self._parse_after_braces(tokens, inside_enclosure)
raise SyntaxError(
"unable to create a single AST for the expression")
if len(lines) > 0:
if tokens[0] and tokens[0][0] == "CompoundExpression":
tokens = tokens[0][1:]
compound_expression = ["CompoundExpression", *lines, *tokens]
return compound_expression
return tokens[0]
def _check_op_compatible(self, op1: str, op2: str):
if op1 == op2:
return True
muldiv = {"*", "/"}
addsub = {"+", "-"}
if op1 in muldiv and op2 in muldiv:
return True
if op1 in addsub and op2 in addsub:
return True
return False
def _from_fullform_to_fullformlist(self, wmexpr: str):
"""
Parses FullForm[Downvalues[]] generated by Mathematica
"""
out: list = []
stack = [out]
generator = re.finditer(r'[\[\],]', wmexpr)
last_pos = 0
for match in generator:
if match is None:
break
position = match.start()
last_expr = wmexpr[last_pos:position].replace(',', '').replace(
']', '').replace('[', '').strip()
if match.group() == ',':
if last_expr != '':
stack[-1].append(last_expr)
elif match.group() == ']':
if last_expr != '':
stack[-1].append(last_expr)
stack.pop()
elif match.group() == '[':
stack[-1].append([last_expr])
stack.append(stack[-1][-1])
last_pos = match.end()
return out[0]
def _from_fullformlist_to_fullformsympy(self, pylist: list):
from sympy import Function, Symbol
def converter(expr):
if isinstance(expr, list):
if len(expr) > 0:
head = expr[0]
args = [converter(arg) for arg in expr[1:]]
return Function(head)(*args)
else:
raise ValueError("Empty list of expressions")
elif isinstance(expr, str):
return Symbol(expr)
else:
return _sympify(expr)
return converter(pylist)
_node_conversions = dict(
Times=Mul,
Plus=Add,
Power=Pow,
Log=lambda *a: log(*reversed(a)),
Log2=lambda x: log(x, 2),
Log10=lambda x: log(x, 10),
Exp=exp,
Sqrt=sqrt,
Sin=sin,
Cos=cos,
Tan=tan,
Cot=cot,
Sec=sec,
Csc=csc,
ArcSin=asin,
ArcCos=acos,
ArcTan=lambda *a: atan2(*reversed(a)) if len(a) == 2 else atan(*a),
ArcCot=acot,
ArcSec=asec,
ArcCsc=acsc,
Sinh=sinh,
Cosh=cosh,
Tanh=tanh,
Coth=coth,
Sech=sech,
Csch=csch,
ArcSinh=asinh,
ArcCosh=acosh,
ArcTanh=atanh,
ArcCoth=acoth,
ArcSech=asech,
ArcCsch=acsch,
Expand=expand,
Im=im,
Re=sympy.re,
Flatten=flatten,
Polylog=polylog,
Cancel=cancel,
# Gamma=gamma,
TrigExpand=expand_trig,
Sign=sign,
Simplify=simplify,
Defer=UnevaluatedExpr,
Identity=S,
# Sum=Sum_doit,
# Module=With,
# Block=With,
Null=lambda *a: S.Zero,
Mod=Mod,
Max=Max,
Min=Min,
Pochhammer=rf,
ExpIntegralEi=Ei,
SinIntegral=Si,
CosIntegral=Ci,
AiryAi=airyai,
AiryAiPrime=airyaiprime,
AiryBi=airybi,
AiryBiPrime=airybiprime,
LogIntegral=li,
PrimePi=primepi,
Prime=prime,
PrimeQ=isprime,
List=Tuple,
Greater=StrictGreaterThan,
GreaterEqual=GreaterThan,
Less=StrictLessThan,
LessEqual=LessThan,
Equal=Equality,
Or=Or,
And=And,
Function=_parse_Function,
)
_atom_conversions = {
"I": I,
"Pi": pi,
}
def _from_fullformlist_to_sympy(self, full_form_list):
def recurse(expr):
if isinstance(expr, list):
if isinstance(expr[0], list):
head = recurse(expr[0])
else:
head = self._node_conversions.get(expr[0],
Function(expr[0]))
return head(*list(recurse(arg) for arg in expr[1:]))
else:
return self._atom_conversions.get(expr, sympify(expr))
return recurse(full_form_list)
def _from_fullformsympy_to_sympy(self, mform):
expr = mform
for mma_form, sympy_node in self._node_conversions.items():
expr = expr.replace(Function(mma_form), sympy_node)
return expr
from diffgeom import Manifold, Patch, CoordSystem
from sympy import sqrt, atan2, acos, sin, cos, Dummy
###############################################################################
# R2
###############################################################################
R2 = Manifold('R^2', 2)
# Patch and coordinate systems.
R2_origin = Patch('origin', R2)
R2_r = CoordSystem('rectangular', R2_origin, ['x', 'y'])
R2_p = CoordSystem('polar', R2_origin, ['r', 'theta'])
# Connecting the coordinate charts.
x, y, r, theta = [Dummy(s) for s in ['x', 'y', 'r', 'theta']]
R2_r.connect_to(R2_p, [x, y],
[sqrt(x**2 + y**2), atan2(y, x)],
inverse=False,
fill_in_gaps=False)
R2_p.connect_to(R2_r, [r, theta], [r * cos(theta), r * sin(theta)],
inverse=False,
fill_in_gaps=False)
del x, y, r, theta
# Defining the basis coordinate functions and adding shortcuts for them to the
# manifold and the patch.
R2.x, R2.y = R2_origin.x, R2_origin.y = R2_r.x, R2_r.y = R2_r.coord_functions()
R2.r, R2.theta = R2_origin.r, R2_origin.theta = R2_p.r, R2_p.theta = R2_p.coord_functions(
)
# Defining the basis vector fields and adding shortcuts for them to the
# manifold and the patch.
def test_re():
x, y = symbols('x,y')
a, b = symbols('a,b', real=True)
r = Symbol('r', real=True)
i = Symbol('i', imaginary=True)
assert re(nan) == nan
assert re(oo) == oo
assert re(-oo) == -oo
assert re(0) == 0
assert re(1) == 1
assert re(-1) == -1
assert re(E) == E
assert re(-E) == -E
assert re(x) == re(x)
assert re(x * I) == -im(x)
assert re(r * I) == 0
assert re(r) == r
assert re(i * I) == I * i
assert re(i) == 0
assert re(x + y) == re(x + y)
assert re(x + r) == re(x) + r
assert re(re(x)) == re(x)
assert re(2 + I) == 2
assert re(x + I) == re(x)
assert re(x + y * I) == re(x) - im(y)
assert re(x + r * I) == re(x)
assert re(log(2 * I)) == log(2)
assert re((2 + I)**2).expand(complex=True) == 3
assert re(conjugate(x)) == re(x)
assert conjugate(re(x)) == re(x)
assert re(x).as_real_imag() == (re(x), 0)
assert re(i * r * x).diff(r) == re(i * x)
assert re(i * r * x).diff(i) == I * r * im(x)
assert re(
sqrt(a +
b * I)) == (a**2 + b**2)**Rational(1, 4) * cos(atan2(b, a) / 2)
assert re(a * (2 + b * I)) == 2 * a
assert re((1 + sqrt(a + b*I))/2) == \
(a**2 + b**2)**Rational(1, 4)*cos(atan2(b, a)/2)/2 + Rational(1, 2)
assert re(x).rewrite(im) == x - im(x)
assert (x + re(y)).rewrite(re, im) == x + y - im(y)
a = Symbol('a', algebraic=True)
t = Symbol('t', transcendental=True)
x = Symbol('x')
assert re(a).is_algebraic
assert re(x).is_algebraic is None
assert re(t).is_algebraic is False
###############################################################
# Elasticity parameter for the Neo-Hookean cylinder (intima)
mu_i, nu_i, beta_i, eta_i, rho_i, phi_i = 27.9, 0.49, 170.88, 263.66, 0.51, pi / 3
# Elasticity parameter for the Hopzafel cylinder (Media)
mu_m, nu_m, beta_m, eta_m, rho_m, phi_m = 1.27, 0.49, 8.21, 21.60, 0.25, pi / 8
## Elasticity parameter for the Hopzafel cylinder (adventitia)
mu_a, nu_a, beta_a, eta_a, rho_a, phi_a = 7.56, 0.49, 85.03, 38.57, 0.5, pi / 2.5
###############################################################
#fibers for intitma
#in case of 3-D you should add x[2] for z
x, y, z = sym.symbols('x[0], x[1], x[2]')
theta = sym.atan2(y, x)
t = theta + pi
r_1 = (0.591998) + (0.013983) * sym.cos(t) + (0.016970) * sym.cos(
2 * t) + (0.011831) * sym.cos(3 * t) + (0.000296) * sym.cos(
4 * t) + (0.002825) * sym.cos(5 * t) + (0.000094) * sym.cos(
6 * t) + (0.000966) * sym.cos(7 * t) + (-0.000696) * sym.cos(
8 * t) + (-0.027813) * sym.sin(t) + (-0.005089) * sym.sin(
2 * t) + (-0.013440) * sym.sin(3 * t) + (
-0.005000) * sym.sin(4 * t) + (-0.001729) * sym.sin(
5 * t) + (-0.001526) * sym.sin(
6 * t) + (0.002450) * sym.sin(
7 * t) + (-0.000253) * sym.sin(8 * t)
r_2 = (0.925868) + (-0.013229) * sym.cos(t) + (0.031791) * sym.cos(
2 * t) + (0.004869) * sym.cos(3 * t) + (-0.000852) * sym.cos(
4 * t) + (-0.000998) * sym.cos(5 * t) + (-0.005195) * sym.cos(
6 * t) + (-0.003236) * sym.cos(7 * t) + (-0.006026) * sym.cos(
def test_tansolver(self):
ik_tester = b3.BehaviorTree()
tan_setup = test_tan_id()
tanID = tan_id()
tanID.Name = "tan ID"
tanID.BHdebug = False
tanSOL = tan_solve()
tanSOL.BHdebug = False
tanSOL.Name = 'Tangent Solver'
asgn = assigner()
subtree = b3.Sequence([asgn, tanID, tanSOL])
repeats = b3.Repeater(subtree, max_loop=15)
#updateL01.name = 'update Transform and eqn lists'
bb = b3.Blackboard()
ik_tester.root = b3.Sequence([tan_setup, repeats])
sp.var('r_11 r_12 r_13 r_21 r_22 r_23 r_31 r_32 r_33 Px Py Pz'
) # needed for test results
print '\n\n ---------- tan solver TEST 1 --------------\n\n'
bb.set('test_number', 1) # go to test 1
ik_tester.tick("tan_id test", bb)
# Test the results
variables = bb.get('unknowns')
fs = 'tan_solver test 1 FAIL'
ntests = 0
for v in variables:
if (v.symbol == th_5):
self.assertTrue(v.solved == False, fs)
if (self.DB):
print '\n-------------------- ', v.symbol
if (self.DB and v.solved):
sp.pprint(v.solutions[0])
if (v.nsolutions == 2):
sp.pprint(v.solutions[1])
if (v.symbol == th_1):
ntests += 1
self.assertTrue(not v.solved, fs + ' [th_1]')
if (v.symbol == th_2):
ntests += 1
#self.assertTrue(v.solved, fs+' [th_2]')
self.assertTrue(
v.solutions[0] - sp.atan2((r_22 - 15) / l_1,
(r_23 - 99) / l_3) == 0,
fs + ' [th_2]')
if (v.symbol == th_3):
ntests += 1
#self.assertTrue(v.solved, fs + ' [th_3]')
sp.pprint(v.solutions[0])
self.assertTrue(
v.solutions[0] == sp.atan2((r_31 - l_2) / l_1,
(r_32 - l_4) / l_3),
fs + ' [th_3]')
if (v.symbol == th_4):
ntests += 1
#self.assertTrue(v.solved, fs + ' [th_4]')
self.assertTrue(
v.solutions[0] == sp.atan2(r_13 / (l_1 + l_2), Px / l_3),
fs + ' [th_4]')
if (v.symbol == th_23):
ntests += 1
#self.assertTrue(v.solved, fs + ' [th_4]')
self.assertTrue(
v.solutions[0] == sp.atan2(r_33 / (l_5), (Pz - 50) / l_1),
fs + ' [th_23]')
self.assertTrue(ntests == 5, fs + ' assertion count failure ')
print '\n\n ---------- tan solver TEST 2 --------------\n\n'
bb2 = b3.Blackboard()
bb2.set('test_number', 2) # go to test 2
ik_tester.tick("tan_id test", bb2)
# Test the results
variables = bb2.get('unknowns')
print '>> Test 2 Asserts'
fs = 'tan_solver test 2 FAIL'
fs2 = 'wrong assumption'
ntests = 0
for v in variables:
if v.solved:
print '\n-------------------- ', v.symbol
sp.pprint(v.solutions[0])
if (v.nsolutions == 2):
sp.pprint(v.solutions[1])
if (v.symbol == th_1):
ntests += 1
self.assertTrue(not v.solved, fs + ' [th_1]')
if (v.symbol == th_2):
ntests += 1
#self.assertTrue(v.solved, fs + ' [th_2]')
self.assertTrue(v.nsolutions == 1, fs + ' [th_2]')
self.assertTrue(
v.solutions[0] == sp.atan2(
(r_22 - 15) / l_1, (r_23 - 99) / l_3), fs + ' [th_2]')
if v.symbol == th_4:
ntests += 1
self.assertTrue(not v.solved, fs + ' [th_4]')
if v.symbol == th_5:
ntests += 1
self.assertTrue(not v.solved, fs + ' [th_5]')
if v.symbol == th_6:
ntests += 1
#self.assertTrue(v.solved, fs + ' [th_6]')
self.assertTrue(v.solutions[0] == sp.atan2(-r_33, r_31), fs)
self.assertTrue(v.solutions[1] == sp.atan2(r_33, -r_31), fs)
print 'Assumptions for ', v.symbol # should set assumptions if canceling an unk.
print ' ', sp.pprint(v.assumption[0])
print ' ', sp.pprint(v.assumption[1])
self.assertTrue(ntests == 5, 'tan_solver: Assert count FAIL')
print "global assumptions"
print global_assumptions
def equations_of_motion(self, states, controls, t: Optional[float] = None):
# All variables are in body frame (except, which psi isn't used)
# Inputs
f_long_f = controls[0]
f_long_r = controls[1]
delta_f = controls[2]
delta_r = controls[3]
# States
# x = states[0]
x_dot = states[1]
# y = states[2]
y_dot = states[3]
psi = states[4]
psi_dot = states[5]
# Helpers
v = sp.sqrt(x_dot**2 + y_dot**2)
beta_f = sp.atan2(y_dot + self.l_f * psi_dot, x_dot + 0.00000001)
beta_r = sp.atan2(y_dot - self.l_r * psi_dot, x_dot + 0.00000001)
beta = sp.atan2(self.l_r * sp.tan(beta_f) + self.l_f * sp.tan(beta_r),
self.l_f + self.l_r)
alpha_f = delta_f - beta_f
alpha_r = delta_r - beta_r
f_lat_f = 2.0 * self.c_f * alpha_f
f_lat_r = 2.0 * self.c_r * alpha_r
f_long_x_f = f_long_f * sp.cos(delta_f)
f_lat_x_f = -f_lat_f * sp.sin(delta_f)
f_long_y_f = -f_long_f * sp.sin(delta_f)
f_lat_y_f = f_lat_f * sp.cos(delta_f)
f_long_x_r = f_long_r * sp.cos(delta_r)
f_lat_x_r = -f_lat_r * sp.sin(delta_r)
f_long_y_r = -f_long_r * sp.sin(delta_r)
f_lat_y_r = f_lat_r * sp.cos(delta_r)
# Equations of Motion
x_rate = (
x_dot # v * sp.cos(beta)
)
x_dot_rate = ((f_long_x_f + f_lat_x_f + f_long_x_r + f_lat_x_r) /
self.m)
y_rate = (
y_dot # v * sp.sin(beta)
)
y_dot_rate = ((f_long_y_f + f_lat_y_f + f_long_y_r + f_lat_y_r) /
self.m)
psi_rate = (psi_dot)
psi_dot_rate = (((f_long_y_f + f_lat_y_f) * self.l_f -
(f_long_y_r + f_lat_y_r) * self.l_r) / self.I_z)
# Path construction
s_rate = (v)
x_fixed_rate = (v * sp.cos(psi + beta))
y_fixed_rate = (v * sp.sin(psi + beta))
return [
x_rate,
x_dot_rate,
y_rate,
y_dot_rate,
psi_rate,
psi_dot_rate,
x_fixed_rate,
y_fixed_rate,
s_rate,
], [
beta_f,
beta_r,
beta,
alpha_f,
alpha_r,
v,
]
def test_implicit():
x, y = symbols('x,y')
assert fcode(sin(x)) == " sin(x)"
assert fcode(atan2(x, y)) == " atan2(x, y)"
assert fcode(conjugate(x)) == " conjg(x)"
def SolutionSetUp():
tic()
l = 0.54448373678246
omega = (3. / 2) * np.pi
z = sy.symbols('z')
x = sy.symbols('x[0]')
y = sy.symbols('x[1]')
rho = sy.sqrt(x**2 + y**2)
phi = sy.atan2(y, x)
# looked at all the exact solutions and they seems to be the same as the paper.....
psi = (sy.sin((1 + l) * phi) * sy.cos(l * omega)) / (1 + l) - sy.cos(
(1 + l) * phi) - (sy.sin(
(1 - l) * phi) * sy.cos(l * omega)) / (1 - l) + sy.cos(
(1 - l) * phi)
psi_prime = polart(psi, x, y)
psi_3prime = polart(polart(psi_prime, x, y), x, y)
u = rho**l * ((1 + l) * sy.sin(phi) * psi + sy.cos(phi) * psi_prime)
v = rho**l * (-(1 + l) * sy.cos(phi) * psi + sy.sin(phi) * psi_prime)
uu0 = Expression((sy.ccode(u), sy.ccode(v)))
ub0 = Expression((str(sy.ccode(u)).replace('atan2(x[1], x[0])',
'(atan2(x[1], x[0])+2*pi)'),
str(sy.ccode(v)).replace('atan2(x[1], x[0])',
'(atan2(x[1], x[0])+2*pi)')))
p = -rho**(l - 1) * ((1 + l)**2 * psi_prime + psi_3prime) / (1 - l)
pu0 = Expression(sy.ccode(p))
pb0 = Expression(
str(sy.ccode(p)).replace('atan2(x[1], x[0])',
'(atan2(x[1], x[0])+2*pi)'))
f = rho**(2. / 3) * sy.sin((2. / 3) * phi)
b = sy.diff(f, x)
d = sy.diff(f, y)
bu0 = Expression((sy.ccode(b), sy.ccode(d)))
bb0 = Expression((str(sy.ccode(b)).replace('atan2(x[1], x[0])',
'(atan2(x[1], x[0])+2*pi)'),
str(sy.ccode(d)).replace('atan2(x[1], x[0])',
'(atan2(x[1], x[0])+2*pi)')))
ru0 = Expression('0.0')
#Laplacian
L1 = sy.diff(u, x, x) + sy.diff(u, y, y)
L2 = sy.diff(v, x, x) + sy.diff(v, y, y)
A1 = u * sy.diff(u, x) + v * sy.diff(u, y)
A2 = u * sy.diff(v, x) + v * sy.diff(v, y)
P1 = sy.diff(p, x)
P2 = sy.diff(p, y)
# Curl-curl
C1 = sy.diff(d, x, y) - sy.diff(b, y, y)
C2 = sy.diff(b, x, y) - sy.diff(d, x, x)
NS1 = -d * (sy.diff(d, x) - sy.diff(b, y))
NS2 = b * (sy.diff(d, x) - sy.diff(b, y))
M1 = sy.diff(u * d - v * b, y)
M2 = -sy.diff(u * d - v * b, x)
print ' ', toc()
# graduu0 = Expression(sy.ccode(sy.diff(u, rho) + (1./rho)*sy.diff(u, phi)))
# graduu0 = Expression((sy.ccode(sy.diff(u, rho)),sy.ccode(sy.diff(v, rho))))
tic()
Laplacian = Expression((sy.ccode(L1), sy.ccode(L2)))
Advection = Expression((sy.ccode(A1), sy.ccode(A2)))
gradPres = Expression((sy.ccode(P1), sy.ccode(P2)))
CurlCurl = Expression((sy.ccode(C1), sy.ccode(C2)))
gradR = Expression(('0.0', '0.0'))
NS_Couple = Expression((sy.ccode(NS1), sy.ccode(NS2)))
M_Couple = Expression((sy.ccode(M1), sy.ccode(M2)))
print ' ', toc()
return uu0, ub0, pu0, pb0, bu0, bb0, ru0, Laplacian, Advection, gradPres, CurlCurl, gradR, NS_Couple, M_Couple
def test_updateL(self):
#
# Set up robot equations for further solution by BT
#
# Check for a pickle file of pre-computed Mech object. If the pickle
# file is not there, compute the kinematic equations
# Using PUMA 560 also tests scan_for_equations() and sum_of_angles_transform() in ik_classes.py
#
# The famous Puma 560 (solved in Craig)
#
import os as os
print '\n------------'
print 'Current dir: ', os.getcwd()
pickname = 'fk_eqns/Puma_pickle.p'
if(os.path.isfile(pickname)):
print 'a pickle file will be used to speed up'
else:
print 'There was no pickle file'
print '------------'
# return [dh, vv, params, pvals, variables]
robot = 'Puma'
[dh, vv, params, pvals, unknowns] = robot_params(
robot) # see ik_robots.py
# def kinematics_pickle(rname, dh, constants, pvals, vv, unks, test):
Test = True
[M, R, unk_Puma] = kinematics_pickle(
robot, dh, params, pvals, vv, unknowns, Test)
print 'GOT HERE: updateL robot name: ', R.name
R.name = 'test: ' + robot # ??? TODO: get rid of this (but fix report)
# check the pickle in case DH params were changed
# check that two mechanisms have identical DH params
check_the_pickle(M.DH, dh)
testerbt = b3.BehaviorTree()
setup = updateL()
setup.BHdebug = True
bb = b3.Blackboard()
# this just runs updateL - not real solver
testerbt.root = b3.Sequence([setup])
bb.set('Robot', R)
bb.set('unknowns', unk_Puma)
testerbt.tick('test', bb)
L1 = bb.get('eqns_1u')
L2 = bb.get('eqns_2u')
print L2[0].RHS
# print them all out(!)
sp.var('Px Py Pz')
fs = 'updateL: equation list building FAIL'
# these self.assertTrues are not conditional - no self.assertTrueion counting needed
self.assertTrue(L1[0].RHS == d_3, fs)
self.assertTrue(L1[0].LHS == -Px * sp.sin(th_1) +
Py * sp.cos(th_1), fs)
self.assertTrue(L2[0].RHS == -a_2 * sp.sin(th_2) -
a_3 * sp.sin(th_23) + d_1 - d_4 * (sp.cos(th_23)), fs)
self.assertTrue(L2[0].LHS == Pz, fs)
#########################################
# test R.set_solved
# here's what should happen when we set up two solutions
u = unk_Puma[2]
sp.var('Y X B')
u.solutions.append(sp.atan2(Y, X)) # make up some equations
u.solutions.append(sp.atan2(-Y, X))
# assumptions are used when a common denominator is factored out
u.assumption.append(sp.Q.positive(B)) # right way to say "non-zero"?
u.assumption.append(sp.Q.negative(B))
u.nsolutions = 2
u.set_solved(R, unk_Puma) # test the set solved function
fs = 'updateL: testing R.set_solved FAIL '
self.assertTrue(not u.readytosolve, fs)
self.assertTrue(u.solved, fs)
# when initialized solveN=0 set_solved should increment it
self.assertTrue(R.solveN == 1, fs)
def test_arg_rewrite():
assert arg(1 + I) == atan2(1, 1)
x = Symbol('x', real=True)
y = Symbol('y', real=True)
assert arg(x + I * y).rewrite(atan2) == atan2(y, x)
def cylinder_stream_funcion(U=1, R=1):
'''cylinder_stream_funcion'''
r = sympy.sqrt(x ** 2 + y ** 2)
theta = sympy.atan2(y, x)
return U * (r - R ** 2 / r) * sympy.sin(theta)
def gouy(self):
"""The Gouy phase."""
return atan2(self.z, self.z_r)
