pythonsympy

Solve the kinematic equations for aiming a simulated turret with velocity and acceleration


I am working on a problem for a simulated game. I want my AI to be able to aim at a moving enemy target with a given starting location, starting velocity, and constant acceleration.

The position of the enemy is given by

p_e(t) = s_e + v_e * t + 0.5 * a_e * t ** 2

and the position of a fired projectile is given by

v_b(theta) = spd_b * (sin(theta), cos(theta))
p_b(theta, t) = v_b(theta) * t

The task is to find the angle theta such that there is some t where p_e(t) == p_b(t), if any such theta exists given the values of the various parameters.

I solved the 0-acceleration version of this using Sympy. Sympy's output seemed logically correct and tested correct in the simulation I'm using. However, when I tried to solve the version with acceleration, I had trouble:

from sympy import *

# Define variables.
#
# Note: we do not define variables for the bullet's starting position, because
# we are going to treat the player ship as a fixed frame of reference.
# Therefore, esx, esy, evx, and evy need to be modified by the controlled
# ship's relative speed and position.
esx, esy, evx, evy, eax, eay = symbols('esx esy evx evy eax eay')
bspd, theta = symbols('bspd theta')
bvx, bvy = bspd * sin(theta), bspd * cos(theta)
t = symbols('t', positive=True)

# Define the x and y positions of the bullet and the enemy.
ex = esx + evx * t + 0.5 * eax * t ** 2
ey = esy + evy * t + 0.5 * eay * t ** 2
bx = bvx * t
by = bvy * t

# Solve for the intersection time in each dimension.
tix = solve(Eq(ex, bx), t)
tiy = solve(Eq(ey, by), t)
print(tix)
print(tiy)

# Set the per-dimension intersection times equal to one another 
# and solve for theta.
print(solve(Eq(tix[0], tiy[0]), theta, check=False))

The output of this program is:

[(bspd*sin(theta) - evx - 1.4142135623731*sqrt(0.5*bspd**2*sin(theta)**2 - bspd*evx*sin(theta) - eax*esx + 0.5*evx**2))/eax, (bspd*sin(theta) - evx + 1.4142135623731*sqrt(0.5*bspd**2*sin(theta)**2 - bspd*evx*sin(theta) - eax*esx + 0.5*evx**2))/eax]
[(bspd*cos(theta) - evy - 1.4142135623731*sqrt(0.5*bspd**2*cos(theta)**2 - bspd*evy*cos(theta) - eay*esy + 0.5*evy**2))/eay, (bspd*cos(theta) - evy + 1.4142135623731*sqrt(0.5*bspd**2*cos(theta)**2 - bspd*evy*cos(theta) - eay*esy + 0.5*evy**2))/eay]
[-oo*I, oo*I]

When I run this code the final print outputs that the solution contains infinity times an imaginary number. Now tix and tiy have two solutions, but I get this result no matter which combination of solutions I use.

I am not a mathematics person, which is largely why I am trying to use sympy to solve this. Is it really the case that adding the acceleration term makes it impossible to solve this problem? Or have I done something wrong in my use of sympy? If I have done something wrong in my use of sympy, please let me know what it is and how to fix it.


Solution

  • Let the distance from firing be R, the angle (anticlockwise from x axis) be θ, the initial position of the target be (x0,y0), its initial velocity be (ux,uy) and its acceleration be (ax,ay). Let the bullet's velocity be W. Then, since R=Wt,

    enter image description here

    Squaring and adding gives

    enter image description here

    which is a 4th-order polynomial for t.

    A 4th-order polynomial could have anything between 0 and 4 real, positive roots. (I assume that you don’t want to be reminded that you could have fired earlier!) If you can find one then you get θ from

    enter image description here

    The code below gives you two legitimate solutions. (You could fire twice!) However, if you turn the bullet speed down to 10 you won’t get any.

    NOTE: I have taken theta as anticlockwise from the x axis, so cos and sin will be switched around in the bullet's velocity from yours.

    import math
    from numpy.polynomial import Polynomial
    
    # Initial position, velocity, acceleration in each direction
    x0, ux, ax = 10,  5, 12
    y0, uy, ay =  6, 10, 3
    W = 30
    
    # Get roots of 4th-order polynomial to solve for t
    a0 = x0 ** 2 + y0 ** 2
    a1 = 2 * ( x0 * ux + y0 * uy )
    a2 = ux ** 2 + uy ** 2 + x0 * ax + y0 * ay - W ** 2
    a3 = ux * ax + uy * ay
    a4 = 0.25 * ( ax ** 2 + ay ** 2 )
    p = Polynomial( [ a0, a1, a2, a3, a4 ] )
    
    # If any roots are real (and positive) then you can get a solution for theta
    print( "Roots for t are: ", p.roots(), '\n' )
    
    for t in p.roots():
        # Get real, positive roots for t (if there are any)
        if abs( t.imag ) < 1.0e-10 and t.real > 0:
            tval = t.real
            x = x0 + ux * tval + 0.5 * ax * tval ** 2
            y = y0 + uy * tval + 0.5 * ay * tval ** 2
            theta = math.atan2( y, x )
            print( "Solution for theta (radians) =", theta )
            print( "Time = ", tval )
            print( "Check x coordinate: ", x, " vs ", W * tval * math.cos( theta ) )
            print( "Check y coordinate: ", y, " vs ", W * tval * math.sin( theta ) )
            print()
    

    Output:

    Roots for t are:  [-5.53093413 -0.31664049  0.73593233  2.75870111] 
    
    Solution for theta (radians) = 0.696965833246052
    Time =  0.7359323311986329
    Check x coordinate:  16.92924003261389  vs  16.929240032613894
    Check y coordinate:  14.171717906141511  vs  14.171717906141515
    
    Solution for theta (radians) = 0.5749183577755946
    Time =  2.7587011098833836
    Check x coordinate:  69.45609643144779  vs  69.45609643144778
    Check y coordinate:  45.002658819341555  vs  45.00265881934155