def pterminalcoastU(ic, flow, target, step=1e-2, damping=10, tol=1e-2, iLimit=25, start=0, end=1, param=0, TU=1):
    """
        - editing to make it (U)niversal..... 2014_05_16
        - could hand function a list of burns with index numbers
        related to flow
        """

    DEBUG0 = False
    DEBUG1 = True
    DEBUG2 = False

    if DEBUG1:
        print ("")
    if DEBUG1:
        print ("----------------------------------------")
    if DEBUG1:
        print ("        TESTING.................        ")
    if DEBUG1:
        print ("----------------------------------------")
    if DEBUG1:
        print ("")

    # import statements
    from flow import P2B
    from auxiliary import lambert
    from numpy import array, zeros, eye, concatenate
    from shootingmodule import firstorder as shoot

    # initialize
    dt = flow["x"][-1] - flow["x"][0]
    dx = step
    i = 1
    # set initial step direction based on slope
    if start == 1 and end == 0:
        if res < 0:
            dir = -1
        if res > 0:
            dir = 1

    solver = "rk4p"
    tstep = 1e-3
    mu = ic1[4]

    if DEBUG0:
        print ("mu is: %5.2f \n" % mu)
    if DEBUG0:
        print ic1
    if DEBUG0:
        print ic2
    if DEBUG0:
        print target

    # use absolute value of residual
    res = abs(res)

    # while-loop unitl tolerance is met
    """ modify while-loop statement to run first time through
        (i.e. i = 0) like in shootingmodule::firstorder ? ? ?
        or maybe not necessary if p, flow is provided.....
        """
    while abs(res) > tol:

        if DEBUG0:
            print ("\n Debug::Iteration is: %i \n" % i)

        # save old
        if i == 2:
            temp = res
        if i > 2:
            res0 = temp
            temp = res

        # flip the direction of step if necessary
        if i > 120:
            if abs(res) > abs(res0):
                dir = -1 * dir
            # calculate damping needed
            elif abs(res) < abs(res0):
                slope = (res - res0) / step
                dx = -res / slope
                """
                    if res > 0: sgn = -1; else: sgn = 1
                    dx = sgn*(res/slope)
                    if abs(dx/step) < damping: step = step/2
                    """
                if abs(dx / step) < damping:
                    step = dx / 2

        # ---------------
        #      flows
        # ---------------
        # update target #1 and #2 start conditions
        # based on updated param
        """
            if param > 0:
            tspan = [0, param]
            elif param < 0:
            tspan = [0, -param]
            """
        tspan = [0, dir * step]
        """ DEBUG """
        if DEBUG2:
            print ("(dir*step) is: %f \n" % (dir * step))

        if DEBUG0:
            print ("\n Debug::flow1 \n")
        # target #1 flow and ic update
        """ could get rid of these if(len()) lines by moving outside while()
            and using trim_output = False during while-loop
            """
        if len(ic1) == 4:
            ic1 = concatenate((ic1.reshape(4, 1), array([[mu]])))
        """ choose a time step that is smaller than tspan!!!!"""
        if abs(dir * step) < 1e-3:
            ministep = abs(tspan[1] - tspan[0]) / 10
        else:
            ministep = 1e-4
        if DEBUG2:
            print ("delta-time is %e " % tspan[1])
        if DEBUG2:
            print ("ministep is: %e " % ministep)
        flow1 = P2B(ic1, tspan, tstep=ministep, trim_output=False)
        """ DEBUG """
        if DEBUG2:
            print ("flow(t)")
        if DEBUG2:
            print flow1["x"][-1] - flow1["x"][0]
        if DEBUG2:
            print ("flow1(0)....")
        if DEBUG2:
            print flow1["y"][0][0:4]
        if DEBUG2:
            print ("flow1(-1)....")
        if DEBUG2:
            print flow1["y"][-1][0:4]

        ic1 = array(flow1["y"][-1])

        """ DEBUG """
        if DEBUG2:
            print ("ic1....")
        if DEBUG2:
            print ic1[0:4]

        if DEBUG0:
            print ("\n Debug::flow2 \n")
        # target #2 flow and ic update
        """ could get rid of these if(len()) lines by moving outside while()
            and using trim_output = False during while-loop
            """
        if len(ic2) == 4:
            ic2 = concatenate((ic2.reshape(4, 1), array([[mu]])))
        flow2 = P2B(ic2, tspan, tstep=ministep, trim_output=False)
        """ DEBUG """
        if DEBUG2:
            print ("flow2(-1)....")
        if DEBUG2:
            print flow2["y"][-1][0:4]

        ic2 = array(flow2["y"][-1])

        """ DEBUG """
        if DEBUG2:
            print ("ic2....")
        if DEBUG2:
            print ic2[0:4]

        # solve for new lambert arc
        #        dt   = dt + param
        #        lamb = prussing_conway(ic1, target, dt, mu, iLimit = 1500, tol = 1E-6);

        """ need to update param here because target #1 and target #2
            may not have fully stepped the delta-time"""
        # update param
        param = param + (flow1["x"][-1] - flow1["x"][0])

        # skip lambert arc and just applying shooting method
        ic3 = zeros(21)
        ic3[0:2] = ic1[0:2]
        ic3[2:4] = flow["y"][0][2:4]
        ic3[4] = mu
        # tack identity matrix on end for STM
        ic3[5:] = eye(4).reshape(16)
        tspan = [0.0, dt - param]
        # apply shooting method
        if DEBUG0:
            print ("\n Debug::ShootingMethod \n")
        flow = shoot(
            ic3,
            target[0:2],
            tspan,
            eom="P2BP_varEqns",
            tstep=1e-2,
            trim_output=False,
            print_status=False,
            damping=1 / 3.0,
        )
        if DEBUG2:
            print ("flow(t)")
        if DEBUG2:
            print flow["x"][-1] - flow["x"][0]

        #        if DEBUG0: print ('\n Debug::flow3 \n')
        #        # target #3 flow
        #        flow  = P2B(solver, tspan, tstep, ic3, TU, \
        #                    eom = 'P2BP_varEqns')

        if DEBUG0:
            print ("\n Debug::DeltaVs \n")
        # -----------------------
        #        Delta Vs
        # -----------------------
        dv1 = array([flow["y"][0][2], flow["y"][0][3]]) - ic1[2:4]
        dv2 = -array([flow["y"][-1][2], flow["y"][-1][3]]) + target[2:4]

        # initial primer vector condition
        #        dv1 = lamb['v1'] - array([ic1[2], ic1[3]])
        # update velocity condition at target
        # and calculate final primer vector condition

        #        if DEBUG0: print ('\n Debug::flow2 Update \n')

        #        if len(ic2) == 4: ic2 = concatenate((ic2.reshape(4,1), array([[mu]])))
        #        flow2  = P2B(solver, tspan, tstep, ic2, TU, \
        #                     eom = 'P2BP')
        #        dv2 = -lamb['v2'] + array([flow2['y'][-1][2], flow2['y'][-1][3]])

        if DEBUG0:
            print ("\n Debug::Primer History \n")

        # primer vector history
        p = phistory(dv1, dv2, flow)

        if DEBUG0:
            print ("\n Debug::Primer Slope (Residual) \n")

        # calculate residual
        # (i.e. primer vector slope at terminal condition)
        res = pslope(flow, p, start, end)
        #        res = abs(res)

        if DEBUG1:
            print (
                "Iter: %4i \t Res: %9.5f \t Param: %9.6f \t"
                "Dir: %2i \t Step: %.6f \t dx: %.6f" % (i, res, param, dir, step, dx)
            )

        # if limit reached -> break
        if i >= iLimit:
            break

        # update i
        i += 1

    if DEBUG2:
        print ("PVM::2: A BLOCK PRINT ------------ :)")
        print (" ~~~~ USING IC ~~~~~")
        print ("ic1....")
        print ic1[0:4]
        print ("ic2....")
        print ic2[0:4]
        print ("ic3....")
        print ic3[0:4]
        print (" ~~~~ USING flow ~~~~~")
        print ("flow1(-1)....")
        print flow1["y"][-1][0:4]
        print ("flow2(-1)....")
        print flow2["y"][-1][0:4]
        print ("flow3(0)....")
        print flow["y"][0][0:4]

    """ update ic3 for output !!!"""
    ic3 = array(flow["y"][0][0:5])

    # initialze output dictionary
    """ do the following if we request trim_output = True,
        otherwise should remove"""
    if len(ic1) == 4:
        ic1 = concatenate((ic1.reshape(4, 1), array([[mu]])))
    if len(ic2) == 4:
        ic2 = concatenate((ic2.reshape(4, 1), array([[mu]])))
    if i == 1:
        ic3 = 0
    out = {
        "param": param,
        "tol": tol,
        "res": res,
        "iter": i - 1,
        "start": start,
        "end": end,
        "ic1": ic1,
        "ic2": ic2,
        "ic3": ic3,
        "dt": dt,
        "x": flow["x"],
        "y": flow["y"],
        "p": p,
    }

    if DEBUG1:
        print ("")
    if DEBUG1:
        print ("----------------------------------------")
    if DEBUG1:
        print ("        FINISHED   ------   FIN         ")
    if DEBUG1:
        print ("----------------------------------------")
    if DEBUG1:
        print ("")

    return out
Beispiel #2
0
 def transfer(self, target_position, target_velocity, flight_time = None, \
              r_history = None, v_history = None, t_history = None, \
              Minimize_Energy = False, SaveFlow = False):
     #  giving lists to r_history, v_history and t_history arguments will
     #+ populate them with the r, v state and time histories
     #  given export_flow will store the entire flow in export_flow
     #  Minimize_Energy = True will override 'flight_time' with the
     #  minimum energy time of flight given from lambert
     from utilities import rv2ic
     from shootingmodule import firstorder as shoot
     #  if no flight_time is given,
     #+ hopefully the user has switched on Minimize_Energy
     if flight_time == None: flight_time = 0.0
     #  create the initial conditions vector
     #  select an appropriate eom model that includes the
     #+ variational equations
     if len(target_position) == 2:
         ic = rv2ic(self.r[0:2], self.v[0:2], self.Mu, STM = True)
         temp_eom = 'P2BP_varEqns'
     elif len(target_position) == 3:
         ic = rv2ic(self.r, self.v, self.Mu, STM = True)
         temp_eom = 'S2BP_varEqns'
     #  Override 'flight_time' if Minimize_Energy = True
     if Minimize_Energy:
         from utilities import create_one_list
         from lambert   import prussing_conway
         from numpy     import array
         ic0 = create_one_list([self.r, self.v], 0, 1)
         icf = create_one_list([target_position, target_velocity], 0, 1)
         #  find the minimum time for a lambert solution
         lambert_solution = prussing_conway(ic0, icf, self.Mu, \
                                            FindMinEnergy = True, \
                                            NonDimUnits = False, \
                                            ScaleOutput = False)
         time_for_min_energy = lambert_solution['tm']
         #  use the minimum time as a flight time to compute
         #+ the lambert solution
         lambert_solution = prussing_conway(ic0, icf, self.Mu, \
                                            TransferTime = time_for_min_energy, \
                                            NonDimUnits = False, \
                                            ScaleOutput = False)
         #  calculate the delta-v
         dv1_guess = list(lambert_solution['v1'] - array(ic0[2:4]))
         dv1_guess.append(0.0)
         flight_time = lambert_solution['time']
     else:
         dv1_guess = None
     #  apply the first order shooting method
     trajectory = shoot(ic, target_position, [0, flight_time], temp_eom, \
                        tol = 1E-2, iLimit = 60, \
                        damping = 0.3, guess = dv1_guess, \
                        print_status = True)
     #  store the delta-v impulsive maneuvers needed
     #+ for the given transfer
     if len(target_position) == 2:
         self.dv1 = trajectory['y'][0][2:4] - self.v
         self.dv2 = target_velocity - trajectory['y'][-1][2:4]
     elif len(target_position) == 3:
         self.dv1 = trajectory['y'][0][3:6] - self.v
         self.dv2 = target_velocity - trajectory['y'][-1][3:6]
     #  save trajectory states if desired
     from utilities import extract_elements
     n = len(target_position)
     if r_history != None:
         extract_elements(trajectory['y'], 0, n - 1, r_history)
     else:
         extract_elements(trajectory['y'], 0, n - 1, self.r_history)
     if v_history != None:
         extract_elements(trajectory['y'], n, 2*n - 1, v_history)
     else:
         extract_elements(trajectory['y'], n, 2*n - 1, self.v_history)
     #  save the time history of the trajectory
     if t_history != None:
         t_history = trajectory['x']
     else:
         self.t_history = trajectory['x']
     #  if the user wants the entire flow history,
     #+ give it to them!
     if SaveFlow:
         self.theflow = trajectory
def opt_pterminal(x0, ic1, ic2, dt):
    """
        1.) flow #1, #2 based on x0
        2.) run prussing_conway
        3.) run shooting method w/ variational eqns
        4.) generate delta-vs
        5.) generate primer history
        6.) extract slopes
        7.) compute cost function
        """
    """
        FUTURE WORK
        1.) add in intelligent step sizing for flow3 = shoot()
        """
    # import statements
    from flow import P2B as flow
    from lambert import prussing_conway
    from shootingmodule import firstorder as shoot
    from numpy.linalg import norm
    from numpy import array, zeros, eye

    print ("Starting an iteration of opt_pterminal!")
    print ("x0 is: %e, %e " % (x0[0], x0[1]))

    # function parameters
    """ FW 1.) """
    step = 1e-2
    tol = 1e-3

    # extract mu
    mu = ic1[4]

    # flow #1, #2 based on x0[0]
    tspan = [0, x0[0]]
    ministep = abs(x0[0] / 2e2)
    print ("Midpoint-A of an iteration of opt_pterminal!")
    print ("tspan is: %e, %e " % (tspan[0], tspan[1]))
    print ("ministep is: %e " % ministep)
    flow1 = flow(ic1, tspan, ministep, trim_output=False, print_status=True, solver="rk4p")
    print ("Midpoint-B of an iteration of opt_pterminal!")
    flow2 = flow(ic2, tspan, ministep, trim_output=False)
    print ("Midpoint-C of an iteration of opt_pterminal!")
    # update ic1 and ic2
    ic1 = array(flow1["y"][-1])
    ic2 = array(flow2["y"][-1])
    # update of flow time based on x0
    tspan = [0, dt - x0[0] + x0[1]]
    # flow #2 based on x0[1]
    flow2 = flow(ic2, tspan, tstep=step, trim_output=False)
    # target position
    target = array(flow2["y"][-1][0:4])
    # solve for lambert arc
    print ("Midpoint-0 of an iteration of opt_pterminal!")
    arc = prussing_conway(ic1, target, mu, TransferTime=abs(tspan[1]))
    # setup initial conditions for flow of spacecraft with
    # variational equations
    ic = zeros(21)
    ic[0:2] = ic1[0:2]
    ic[2:4] = arc["v1"]
    ic[4] = mu
    ic[5:] = eye(4).reshape(16)
    # flow spacecraft using lambert arc
    print ("Midpoint-1 of an iteration of opt_pterminal!")
    flow3 = flow(ic, tspan, tstep=step, eom="P2BP_varEqns", trim_output=False)
    print ("Midpoint-2 of an iteration of opt_pterminal!")
    # position error
    poserror = flow3["y"][-1][0:2] - flow2["y"][-1][0:2]
    # apply shooting method to reduce error
    if norm(poserror) > tol:
        flow3 = shoot(
            ic, target[0:2], tspan, eom="P2BP_varEqns", tstep=step, damping=1 / 3.0, tol=tol, trim_output=False
        )
        poserror = flow3["y"][-1][0:2] - flow2["y"][-1][0:2]
    print ("Midpoint-3 of an iteration of opt_pterminal!")
    # delta-vs
    dv1 = array([flow3["y"][0][2], flow3["y"][0][3]]) - array([ic1[2], ic1[3]])
    dv2 = -array([flow3["y"][-1][2], flow3["y"][-1][3]]) + array([flow2["y"][-1][2], flow2["y"][-1][3]])
    # generate primer vector history
    p = phistory(dv1, dv2, flow3)
    # extract the slopes of the primer vector
    dp0 = pslope(flow3, p, 1, 0)
    dp1 = pslope(flow3, p, -1, -2)
    # scalar cost function
    J = dp0 ** 2 + dp1 ** 2

    print ("Finished an iteration of opt_pterminal!")

    return J, flow1, flow2, flow3