Esempio n. 1
0
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))
Esempio n. 2
0
    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
Esempio n. 3
0
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))
Esempio n. 4
0
    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')
Esempio n. 5
0
    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]
Esempio n. 6
0
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)
Esempio n. 7
0
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)
Esempio n. 8
0
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)
Esempio n. 9
0
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))
Esempio n. 10
0
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])
Esempio n. 11
0
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))))
Esempio n. 12
0
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
Esempio n. 14
0
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))
Esempio n. 15
0
    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
Esempio n. 16
0
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]
Esempio n. 17
0
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)))
Esempio n. 18
0
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)]])
Esempio n. 19
0
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)]])
Esempio n. 20
0
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))
Esempio n. 21
0
    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 ])
Esempio n. 23
0
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)
Esempio n. 24
0
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
Esempio n. 25
0
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
Esempio n. 26
0
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)
Esempio n. 27
0
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)
Esempio n. 28
0
    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__
        )
Esempio n. 29
0
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))
Esempio n. 30
0
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))
Esempio n. 31
0
                               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",
Esempio n. 32
0
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)
Esempio n. 33
0
    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
Esempio n. 34
0
def Spherical_phi(x, y, z):
    return atan2(y, x)
Esempio n. 35
0
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
Esempio n. 36
0
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()
Esempio n. 37
0
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]])
Esempio n. 38
0
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
Esempio n. 39
0
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)
Esempio n. 40
0
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
Esempio n. 41
0
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
Esempio n. 42
0
def angle(p1, p2):
    ang = sp.atan2(p2[1] - p1[1], p2[0] - p1[0])
    return ang
Esempio n. 43
0
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)
Esempio n. 44
0
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)
                                      }))
Esempio n. 45
0
    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()
Esempio n. 46
0
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]])
Esempio n. 47
0
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))
Esempio n. 48
0
    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
Esempio n. 49
0
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
Esempio n. 50
0
File: rn.py Progetto: vchekan/sympy
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.
Esempio n. 51
0
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
Esempio n. 52
0
###############################################################

# 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(
Esempio n. 53
0
    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
Esempio n. 54
0
    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,
        ]
Esempio n. 55
0
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)"
Esempio n. 56
0
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
Esempio n. 57
0
File: updateL.py Progetto: iory/IKBT
    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)
Esempio n. 58
0
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)
Esempio n. 59
0
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)
Esempio n. 60
0
 def gouy(self):
     """The Gouy phase."""
     return atan2(self.z, self.z_r)