Example #1
0
def Solve_Py(XBINPUT, XBOPTS, VMOPTS, VMINPUT, AELAOPTS, **kwords):
    """@brief Nonlinear dynamic solver using Python to solve aeroelastic
    equation.
    @details Assembly of structural matrices is carried out with 
    Fortran subroutines. Aerodynamics solved using PyAero\.UVLM.
    @warning test outstanding: test for maintaining static deflections in
    same conditions.
    TODO: Maintain static deflections in same conditions.
    @param XBINPUT Beam inputs (for initialization in Python).
    @param XBOPTS Beam solver options (for Fortran).
    @param VMOPTS UVLM solver options (for C/C++).
    @param VMINPUT UVLM solver inputs (for initialization in Python).
    @param VMUNST Unsteady input information for aero solver.
    @param AELAOPTS Options relevant to coupled aeroelastic simulations.
    @param writeDict OrderedDict of 'name':tuple of outputs to write.
    """

    # Check correct solution code.
    assert XBOPTS.Solution.value == 312, ('NonlinearDynamic requested' +
                                          ' with wrong solution code')
    # Initialise static beam data.
    XBINPUT, XBOPTS, NumNodes_tot, XBELEM, PosIni, PsiIni, XBNODE, NumDof \
                = BeamInit.Static(XBINPUT,XBOPTS)

    # Calculate initial displacements.
    if AELAOPTS.ImpStart == False:
        XBOPTS.Solution.value = 112  # Modify options.
        VMOPTS.Steady = ct.c_bool(True)
        Rollup = VMOPTS.Rollup.value
        VMOPTS.Rollup.value = False
        # Solve Static Aeroelastic.
        PosDefor, PsiDefor, Zeta, ZetaStar, Gamma, GammaStar, Force = \
                    Static.Solve_Py(XBINPUT, XBOPTS, VMOPTS, VMINPUT, AELAOPTS)
        XBOPTS.Solution.value = 312  # Reset options.
        VMOPTS.Steady = ct.c_bool(False)
        VMOPTS.Rollup.value = Rollup
    elif AELAOPTS.ImpStart == True:
        PosDefor = PosIni.copy(order='F')
        PsiDefor = PsiIni.copy(order='F')
        Force = np.zeros((XBINPUT.NumNodesTot, 6), ct.c_double, 'F')

    # Write deformed configuration to file. TODO: tidy this away inside function.
    ofile = Settings.OutputDir + Settings.OutputFileRoot + '_SOL312_def.dat'
    if XBOPTS.PrintInfo == True:
        sys.stdout.write('Writing file %s ... ' % (ofile))
    fp = open(ofile, 'w')
    fp.write('TITLE="Non-linear static solution: deformed geometry"\n')
    fp.write('VARIABLES="iElem" "iNode" "Px" "Py" "Pz" "Rx" "Ry" "Rz"\n')
    fp.close()
    if XBOPTS.PrintInfo == True:
        sys.stdout.write('done\n')
    WriteMode = 'a'
    # Write
    BeamIO.OutputElems(XBINPUT.NumElems, NumNodes_tot.value, XBELEM, PosDefor,
                       PsiDefor, ofile, WriteMode)

    # Initialise structural variables for dynamic analysis.
    Time, NumSteps, ForceTime, ForcedVel, ForcedVelDot,\
    PosDotDef, PsiDotDef,\
    OutGrids, PosPsiTime, VelocTime, DynOut\
        = BeamInit.Dynamic(XBINPUT,XBOPTS)
    # Delete unused variables.
    del ForceTime, OutGrids, VelocTime

    # Write _force file
    #    ofile = Settings.OutputDir + Settings.OutputFileRoot + '_SOL312_force.dat'
    #    fp = open(ofile,'w')
    #    BeamIO.Write_force_File(fp, Time, ForceTime, ForcedVel, ForcedVelDot)
    #    fp.close()
    # Write _vel file
    #TODO: write _vel file
    # Write .mrb file.
    #TODO: write .mrb file

    if XBOPTS.PrintInfo.value == True:
        sys.stdout.write('Solve nonlinear dynamic case in Python ... \n')

    # Initialise structural system tensors.
    MglobalFull = np.zeros((NumDof.value, NumDof.value), ct.c_double, 'F')
    CglobalFull = np.zeros((NumDof.value, NumDof.value), ct.c_double, 'F')
    KglobalFull = np.zeros((NumDof.value, NumDof.value), ct.c_double, 'F')
    FglobalFull = np.zeros((NumDof.value, NumDof.value), ct.c_double, 'F')
    Asys = np.zeros((NumDof.value, NumDof.value), ct.c_double, 'F')

    ms = ct.c_int()
    cs = ct.c_int()
    ks = ct.c_int()
    fs = ct.c_int()

    Mvel = np.zeros((NumDof.value, 6), ct.c_double, 'F')
    Cvel = np.zeros((NumDof.value, 6), ct.c_double, 'F')

    #     X0    = np.zeros(NumDof.value, ct.c_double, 'F')
    X = np.zeros(NumDof.value, ct.c_double, 'F')
    DX = np.zeros(NumDof.value, ct.c_double, 'F')
    dXdt = np.zeros(NumDof.value, ct.c_double, 'F')
    dXddt = np.zeros(NumDof.value, ct.c_double, 'F')
    Force_Dof = np.zeros(NumDof.value, ct.c_double, 'F')

    Qglobal = np.zeros(NumDof.value, ct.c_double, 'F')

    # Initialise rotation operators.
    Unit = np.zeros((3, 3), ct.c_double, 'F')
    for i in range(3):
        Unit[i, i] = 1.0

    Unit4 = np.zeros((4, 4), ct.c_double, 'F')
    for i in range(4):
        Unit4[i, i] = 1.0

    Cao = Unit.copy('F')
    Temp = Unit4.copy('F')

    Quat = np.zeros(4, ct.c_double, 'F')
    Quat[0] = 1.0

    # Extract initial displacements and velocities.
    BeamLib.Cbeam_Solv_Disp2State(NumNodes_tot, NumDof, XBINPUT, XBNODE,
                                  PosDefor, PsiDefor, PosDotDef, PsiDotDef, X,
                                  dXdt)

    # Approximate initial accelerations.
    PosDotDotDef = np.zeros((NumNodes_tot.value, 3), ct.c_double, 'F')
    PsiDotDotDef = np.zeros((XBINPUT.NumElems, Settings.MaxElNod, 3),
                            ct.c_double, 'F')

    # Assemble matrices for dynamic analysis.
    BeamLib.Cbeam3_Asbly_Dynamic(XBINPUT, NumNodes_tot, XBELEM, XBNODE, PosIni,
                                 PsiIni, PosDefor, PsiDefor, PosDotDef,
                                 PsiDotDef, PosDotDotDef, PsiDotDotDef, Force,
                                 ForcedVel[0, :], ForcedVelDot[0, :], NumDof,
                                 Settings.DimMat, ms, MglobalFull, Mvel, cs,
                                 CglobalFull, Cvel, ks, KglobalFull, fs,
                                 FglobalFull, Qglobal, XBOPTS, Cao)

    # Get force vector for unconstrained nodes (Force_Dof).
    BeamLib.f_fem_m2v(ct.byref(NumNodes_tot), ct.byref(ct.c_int(6)),
                      Force.ctypes.data_as(ct.POINTER(ct.c_double)),
                      ct.byref(NumDof),
                      Force_Dof.ctypes.data_as(ct.POINTER(ct.c_double)),
                      XBNODE.Vdof.ctypes.data_as(ct.POINTER(ct.c_int)))

    # Get RHS at initial condition.
    Qglobal = Qglobal - np.dot(FglobalFull, Force_Dof)

    # Initial Accel.
    dXddt[:] = np.dot(np.linalg.inv(MglobalFull), -Qglobal)

    # Record position of all grid points in global FoR at initial time step.
    DynOut[0:NumNodes_tot.value, :] = PosDefor

    # Record state of the selected node in initial deformed configuration.
    PosPsiTime[0, :3] = PosDefor[-1, :]
    PosPsiTime[0, 3:] = PsiDefor[-1, XBELEM.NumNodes[-1] - 1, :]

    # Get gamma and beta for Newmark scheme.
    gamma = 0.5 + XBOPTS.NewmarkDamp.value
    beta = 0.25 * pow((gamma + 0.5), 2)

    # Initialize Aero
    Section = InitSection(VMOPTS, VMINPUT, AELAOPTS.ElasticAxis)

    # Declare memory for Aero variables.
    ZetaDot = np.zeros((Section.shape[0], PosDefor.shape[0], 3), ct.c_double,
                       'C')
    K = VMOPTS.M.value * VMOPTS.N.value
    AIC = np.zeros((K, K), ct.c_double, 'C')
    BIC = np.zeros((K, K), ct.c_double, 'C')
    AeroForces = np.zeros((VMOPTS.M.value + 1, VMOPTS.N.value + 1, 3),
                          ct.c_double, 'C')

    # Initialise A-frame location and orientation to be zero
    OriginA_G = np.zeros(3, ct.c_double, 'C')
    PsiA_G = np.zeros(3, ct.c_double, 'C')

    # Init external velocities.
    Ufree = InitSteadyExternalVels(VMOPTS, VMINPUT)

    # Init uninit vars if an impulsive start is specified.
    if AELAOPTS.ImpStart == True:
        Zeta = np.zeros((Section.shape[0], PosDefor.shape[0], 3), ct.c_double,
                        'C')
        Gamma = np.zeros((VMOPTS.M.value, VMOPTS.N.value), ct.c_double, 'C')
        # Generate surface, wake and gamma matrices.
        CoincidentGrid(PosDefor, PsiDefor, Section, ForcedVel[0, :3],
                       ForcedVel[0, 3:], PosDotDef, PsiDotDef, XBINPUT, Zeta,
                       ZetaDot, OriginA_G, PsiA_G, VMINPUT.ctrlSurf)
        # init wake grid and gamma matrix.
        ZetaStar, GammaStar = InitSteadyWake(VMOPTS, VMINPUT, Zeta,
                                             ForcedVel[0, :3])

    # Init GammaDot
    GammaDot = np.zeros_like(Gamma, ct.c_double, 'C')

    # Define tecplot stuff
    if Settings.PlotTec == True:
        FileName = Settings.OutputDir + Settings.OutputFileRoot + 'AeroGrid.dat'
        Variables = ['X', 'Y', 'Z', 'Gamma']
        FileObject = PostProcess.WriteAeroTecHeader(FileName, 'Default',
                                                    Variables)
        # Plot results of static analysis
        PostProcess.WriteUVLMtoTec(FileObject,
                                   Zeta,
                                   ZetaStar,
                                   Gamma,
                                   GammaStar,
                                   TimeStep=0,
                                   NumTimeSteps=XBOPTS.NumLoadSteps.value,
                                   Time=0.0,
                                   Text=True)

    # Open output file for writing
    if 'writeDict' in kwords and Settings.WriteOut == True:
        fp = OpenOutFile(writeDict, XBOPTS, Settings)

    # Write initial outputs to file.
    if 'writeDict' in kwords and Settings.WriteOut == True:
        WriteToOutFile(writeDict, fp, Time[0], PosDefor, PsiDefor, PosIni,
                       PsiIni, XBELEM, ctrlSurf)
    # END if write

    # Time loop.
    for iStep in range(NumSteps.value):

        if XBOPTS.PrintInfo.value == True:
            sys.stdout.write('Time: %-10.4e\n' % (Time[iStep + 1]))
            sys.stdout.write('   SubIter DeltaF     DeltaX     ResLog10\n')

        dt = Time[iStep + 1] - Time[iStep]

        # Set dt for aero force calcs.
        VMOPTS.DelTime = ct.c_double(dt)

        # Save Gamma at iStep.
        GammaSav = Gamma.copy(order='C')

        # Force at current time-step
        if iStep > 0 and AELAOPTS.Tight == False:

            # zero aero forces.
            AeroForces[:, :, :] = 0.0

            # Update CRV.
            PsiA_G = BeamLib.Cbeam3_quat2psi(Quat)  # CRV at iStep

            # Update origin.
            OriginA_G[:] = OriginA_G[:] + ForcedVel[iStep - 1, :3] * dt

            # Update control surface deflection.
            if VMINPUT.ctrlSurf != None:
                if 'mpcCont' in kwords:
                    uOpt = kwords['mpcCont'].getUopt(
                        getState(Gamma, GammaStar, GammaDot, X, dXdt))
                    VMINPUT.ctrlSurf.update(Time[iStep], uOpt[0, 0])
                else:
                    VMINPUT.ctrlSurf.update(Time[iStep])

            # Generate surface grid.
            CoincidentGrid(PosDefor, PsiDefor, Section, ForcedVel[iStep, :3],
                           ForcedVel[iStep, 3:], PosDotDef, PsiDotDef, XBINPUT,
                           Zeta, ZetaDot, OriginA_G, PsiA_G, VMINPUT.ctrlSurf)

            # Update wake geom
            #'roll' data.
            ZetaStar = np.roll(ZetaStar, 1, axis=0)
            GammaStar = np.roll(GammaStar, 1, axis=0)
            #overwrite grid points with TE.
            ZetaStar[0, :] = Zeta[VMOPTS.M.value, :]
            # overwrite Gamma with TE value from previous timestep.
            GammaStar[0, :] = Gamma[VMOPTS.M.value - 1, :]

            # Apply gust velocity.
            if VMINPUT.gust != None:
                Utot = Ufree + VMINPUT.gust.Vels(Zeta)
            else:
                Utot = Ufree

            # Solve for AeroForces.
            UVLMLib.Cpp_Solver_VLM(Zeta, ZetaDot, Utot, ZetaStar, VMOPTS,
                                   AeroForces, Gamma, GammaStar, AIC, BIC)

            # Get GammaDot.
            GammaDot[:] = Gamma[:] - GammaSav[:]

            # Apply density scaling.
            AeroForces[:, :, :] = AELAOPTS.AirDensity * AeroForces[:, :, :]

            if Settings.PlotTec == True:
                PostProcess.WriteUVLMtoTec(
                    FileObject,
                    Zeta,
                    ZetaStar,
                    Gamma,
                    GammaStar,
                    TimeStep=iStep,
                    NumTimeSteps=XBOPTS.NumLoadSteps.value,
                    Time=Time[iStep],
                    Text=True)

            # map AeroForces to beam.
            CoincidentGridForce(XBINPUT, PsiDefor, Section, AeroForces, Force)

            # Add gravity loads.
            AddGravityLoads(Force, XBINPUT, XBELEM, AELAOPTS, PsiDefor,
                            VMINPUT.c)
        #END if iStep > 0

        # Quaternion update for orientation.
        Temp = np.linalg.inv(Unit4 + 0.25 *
                             XbeamLib.QuadSkew(ForcedVel[iStep + 1, 3:]) * dt)
        Quat = np.dot(
            Temp,
            np.dot(Unit4 - 0.25 * XbeamLib.QuadSkew(ForcedVel[iStep, 3:]) * dt,
                   Quat))
        Quat = Quat / np.linalg.norm(Quat)
        Cao = XbeamLib.Rot(Quat)  # transformation matrix at iStep+1

        if AELAOPTS.Tight == True:
            # CRV at iStep+1
            PsiA_G = BeamLib.Cbeam3_quat2psi(Quat)
            # Origin at iStep+1
            OriginA_G[:] = OriginA_G[:] + ForcedVel[iStep, :3] * dt

        # Predictor step.
        X = X + dt * dXdt + (0.5 - beta) * dXddt * pow(dt, 2.0)
        dXdt = dXdt + (1.0 - gamma) * dXddt * dt
        dXddt[:] = 0.0

        # Reset convergence parameters.
        Iter = 0
        ResLog10 = 0.0

        # Newton-Raphson loop.
        while ((ResLog10 > np.log10(XBOPTS.MinDelta.value))
               & (Iter < XBOPTS.MaxIterations.value)):

            # set tensors to zero.
            Qglobal[:] = 0.0
            Mvel[:, :] = 0.0
            Cvel[:, :] = 0.0
            MglobalFull[:, :] = 0.0
            CglobalFull[:, :] = 0.0
            KglobalFull[:, :] = 0.0
            FglobalFull[:, :] = 0.0

            # Update counter.
            Iter += 1

            if XBOPTS.PrintInfo.value == True:
                sys.stdout.write('   %-7d ' % (Iter))

            # nodal diplacements and velocities from state vector.
            BeamLib.Cbeam3_Solv_State2Disp(XBINPUT, NumNodes_tot, XBELEM,
                                           XBNODE, PosIni, PsiIni, NumDof, X,
                                           dXdt, PosDefor, PsiDefor, PosDotDef,
                                           PsiDotDef)

            # if tightly coupled is on then get new aeroforces.
            if AELAOPTS.Tight == True:
                # zero aero forces.
                AeroForces[:, :, :] = 0.0

                # Set gamma at t-1 to saved solution.
                Gamma[:, :] = GammaSav[:, :]
                # get new grid.
                # The rigid-body DoFs (OriginA_G,PsiA_G,ForcedVel) at time step
                # i+1 are used to converge the aeroelastic equations.
                CoincidentGrid(PosDefor, PsiDefor, Section,
                               ForcedVel[iStep + 1, :3], ForcedVel[iStep + 1,
                                                                   3:],
                               PosDotDef, PsiDotDef, XBINPUT, Zeta, ZetaDot,
                               OriginA_G, PsiA_G, VMINPUT.ctrlSurf)

                # close wake.
                ZetaStar[0, :] = Zeta[VMOPTS.M.value, :]

                # save pereference and turn off rollup.
                Rollup = VMOPTS.Rollup.value
                VMOPTS.Rollup.value = False

                # Solve for AeroForces.
                UVLMLib.Cpp_Solver_VLM(Zeta, ZetaDot, Ufree, ZetaStar, VMOPTS,
                                       AeroForces, Gamma, GammaStar, AIC, BIC)

                # turn rollup back to original preference
                VMOPTS.Rollup.value = Rollup

                # apply density scaling.
                AeroForces[:, :, :] = AELAOPTS.AirDensity * AeroForces[:, :, :]

                # beam forces.
                CoincidentGridForce(XBINPUT, PsiDefor, Section, AeroForces,
                                    Force)

                # Add gravity loads.
                AddGravityLoads(Force, XBINPUT, XBELEM, AELAOPTS, PsiDefor,
                                VMINPUT.c)

            #END if Tight

            ForcedVelLoc = ForcedVel[iStep + 1, :].copy('F')
            ForcedVelDotLoc = ForcedVelDot[iStep + 1, :].copy('F')

            # Update matrices.
            BeamLib.Cbeam3_Asbly_Dynamic(
                XBINPUT, NumNodes_tot, XBELEM, XBNODE, PosIni, PsiIni,
                PosDefor, PsiDefor, PosDotDef, PsiDotDef, PosDotDotDef,
                PsiDotDotDef, Force, ForcedVelLoc, ForcedVelDotLoc, NumDof,
                Settings.DimMat, ms, MglobalFull, Mvel, cs, CglobalFull, Cvel,
                ks, KglobalFull, fs, FglobalFull, Qglobal, XBOPTS, Cao)

            # Get force vector for unconstrained nodes (Force_Dof).
            BeamLib.f_fem_m2v(
                ct.byref(NumNodes_tot), ct.byref(ct.c_int(6)),
                Force.ctypes.data_as(ct.POINTER(ct.c_double)),
                ct.byref(NumDof),
                Force_Dof.ctypes.data_as(ct.POINTER(ct.c_double)),
                XBNODE.Vdof.ctypes.data_as(ct.POINTER(ct.c_int)))

            # Solve for update vector.
            # Residual.
            Qglobal = Qglobal +  np.dot(MglobalFull, dXddt) \
                              + np.dot(Mvel,ForcedVelDotLoc) \
                              - np.dot(FglobalFull, Force_Dof)

            if XBOPTS.PrintInfo.value == True:
                sys.stdout.write('%-10.4e ' % (max(abs(Qglobal))))

            # Calculate system matrix for update calculation.
            Asys = KglobalFull \
                    + CglobalFull*gamma/(beta*dt) \
                    + MglobalFull/(beta*pow(dt,2.0))

            # Solve for update.
            DX[:] = np.dot(np.linalg.inv(Asys), -Qglobal)

            # Corrector step.
            X = X + DX
            dXdt = dXdt + DX * gamma / (beta * dt)
            dXddt = dXddt + DX / (beta * pow(dt, 2.0))

            # Residual at first iteration.
            if (Iter == 1):
                Res0_Qglobal = max(abs(Qglobal)) + 1.e-16
                Res0_DeltaX = max(abs(DX)) + 1.e-16

            # Update residual and compute log10.
            Res_Qglobal = max(abs(Qglobal)) + 1.e-16
            Res_DeltaX = max(abs(DX)) + 1.e-16
            ResLog10 = max([
                np.log10(Res_Qglobal / Res0_Qglobal),
                np.log10(Res_DeltaX / Res0_DeltaX)
            ])

            if XBOPTS.PrintInfo.value == True:
                sys.stdout.write('%-10.4e %8.4f\n' % (max(abs(DX)), ResLog10))

            if ResLog10 > 2.0:
                print("Residual growing! Exit Newton-Raphson...")
                break

        # END Netwon-Raphson.

        if ResLog10 > 2.0:
            print("Residual growing! Exit time-loop...")
            debug = 'here'
            del debug
            break

        # Update to converged nodal displacements and velocities.
        BeamLib.Cbeam3_Solv_State2Disp(XBINPUT, NumNodes_tot, XBELEM, XBNODE,
                                       PosIni, PsiIni, NumDof, X, dXdt,
                                       PosDefor, PsiDefor, PosDotDef,
                                       PsiDotDef)

        PosPsiTime[iStep + 1, :3] = PosDefor[-1, :]
        PosPsiTime[iStep + 1, 3:] = PsiDefor[-1, XBELEM.NumNodes[-1] - 1, :]

        # Position of all grid points in global FoR.
        i1 = (iStep + 1) * NumNodes_tot.value
        i2 = (iStep + 2) * NumNodes_tot.value
        DynOut[i1:i2, :] = PosDefor

        # Write selected outputs
        if 'writeDict' in kwords and Settings.WriteOut == True:
            WriteToOutFile(writeDict, fp, Time[iStep + 1], PosDefor, PsiDefor,
                           PosIni, PsiIni, XBELEM, ctrlSurf)
        # END if write.

        # 'Rollup' due to external velocities. TODO: Must add gusts here!
        ZetaStar[:, :] = ZetaStar[:, :] + VMINPUT.U_infty * dt
        if VMINPUT.gust != None:
            ZetaStar[:, :, :] = ZetaStar[:, :, :] + VMINPUT.gust.Vels(
                ZetaStar) * dt

    # END Time loop

    # Write _dyn file.
    ofile = Settings.OutputDir + Settings.OutputFileRoot + '_SOL312_dyn.dat'
    fp = open(ofile, 'w')
    BeamIO.Write_dyn_File(fp, Time, PosPsiTime)
    fp.close()

    #    "Write _shape file"
    #    ofile = Settings.OutputDir + Settings.OutputFileRoot + '_SOL312_shape.dat'
    #    fp = open(ofile,'w')
    #    BeamIO.Write_shape_File(fp, len(Time), NumNodes_tot.value, Time, DynOut)
    #    fp.close()

    # Close output file if it exists.
    if 'writeDict' in kwords and Settings.WriteOut == True:
        fp.close()

    # Close Tecplot ascii FileObject.
    if Settings.PlotTec == True:
        PostProcess.CloseAeroTecFile(FileObject)

    if XBOPTS.PrintInfo.value == True:
        sys.stdout.write(' ... done\n')

    # For interactive analysis at end of simulation set breakpoint.
    pass
def Solve_Py(XBINPUT,XBOPTS,VMOPTS,VMINPUT,AELAOPTS,**kwords):
    """@brief Nonlinear dynamic solver using Python to solve aeroelastic
    equation.
    @details Assembly of structural matrices is carried out with 
    Fortran subroutines. Aerodynamics solved using PyAero\.UVLM.
    @param XBINPUT Beam inputs (for initialization in Python).
    @param XBOPTS Beam solver options (for Fortran).
    @param VMOPTS UVLM solver options (for C/C++).
    @param VMINPUT UVLM solver inputs (for initialization in Python).
    @param VMUNST Unsteady input information for aero solver.
    @param AELAOPTS Options relevant to coupled aeroelastic simulations.
    @param writeDict OrderedDict of 'name':tuple of outputs to write.
    """
        
    # Check correct solution code.
    assert XBOPTS.Solution.value == 912, ('NonlinearFlightDynamic requested' +
                                          ' with wrong solution code')
    
    # I/O management
    XBOUT=DerivedTypes.Xboutput()  
    SaveDict=Settings.SaveDict
    if 'SaveDict' in kwords: SaveDict=kwords['SaveDict']
    if SaveDict['Format']=='h5':
        Settings.WriteOut=False
        Settings.PlotTec=False
        OutList=[AELAOPTS, VMINPUT, VMOPTS, XBOPTS, XBINPUT, XBOUT]
        #if VMINPUT.ctrlSurf!=None:
        #    for cc in range(len(VMINPUT.ctrlSurf)):
        #        OutList.append(VMINPUT.ctrlSurf[cc])
        if SaveDict['SaveWake'] is True:
            dirwake=SaveDict['OutputDir']+'wake'+SaveDict['OutputFileRoot']+'/'
            os.system('mkdir -p %s' %dirwake)
            XBOUT.dirwake=dirwake  
            
    XBOUT.cputime.append(time.clock()) # time.processor_time more appropriate but equivalent
    # for debugging
    XBOUT.ForceDofList=[]
    XBOUT.ForceRigidList=[]

    # Initialise static beam data.
    XBINPUT, XBOPTS, NumNodes_tot, XBELEM, PosIni, PsiIni, XBNODE, NumDof \
                = BeamInit.Static(XBINPUT,XBOPTS)
    
    # special BCs
    SphFlag=False
    if XBNODE.Sflag.any(): SphFlag=True
    
    # Debugging Flags

    if 'HardCodeAero' in kwords: HardCodeAero=kwords['HardCodeAero']
    SaveExtraVariables = False  

    
    #------------------------- Initial Displacement: ImpStart vs Static Solution
    
    # Calculate initial displacements.
    if AELAOPTS.ImpStart == False:
        XBOPTS.Solution.value = 112 # Modify options.
        VMOPTS.Steady = ct.c_bool(True)
        Rollup = VMOPTS.Rollup.value
        VMOPTS.Rollup.value = False
        # Solve Static Aeroelastic.
        PosDefor, PsiDefor, Zeta, ZetaStar, Gamma, GammaStar, Force = \
                    Static.Solve_Py(XBINPUT, XBOPTS, VMOPTS, VMINPUT, AELAOPTS)
        XBOPTS.Solution.value = 912 # Reset options.
        VMOPTS.Steady = ct.c_bool(False)
        VMOPTS.Rollup.value = Rollup 
    elif AELAOPTS.ImpStart == True:
        PosDefor = PosIni.copy(order='F')
        PsiDefor = PsiIni.copy(order='F')
        Force = np.zeros((XBINPUT.NumNodesTot,6),ct.c_double,'F')


    if SaveDict['Format']!='h5': 
        write_SOL912_def(XBOPTS,XBINPUT,XBELEM,NumNodes_tot,PosDefor,PsiDefor,SaveDict)
    else:
        XBOUT.PosDeforStatic=PosDefor.copy()
        XBOUT.PsiDeforStatic=PsiDefor.copy()
        XBOUT.ForceTotStatic = Force.copy()
        
    
    
    #------------------------------------------------ Initialise Dynamic Problem
    
    # Initialise structural variables for dynamic analysis.
    Time, NumSteps, ForceTime, Vrel, VrelDot,\
    PosDotDef, PsiDotDef,\
    OutGrids, PosPsiTime, VelocTime, DynOut\
        = BeamInit.Dynamic(XBINPUT,XBOPTS)
    # Delete unused variables.
    del OutGrids, VelocTime
    
    # sm I/O
    ### why forced velocity with Sol912 ???
    ### If forced velocities are prescribed, then is Sol312        
    XBOUT.PosDefor=PosDefor                # ...SOL912_def.dat
    XBOUT.PsiDefor=PsiDefor   
    XBOUT.ForceTime_force=ForceTime        # ...SOL912_force.dat
    XBOUT.Vrel_force=Vrel
    XBOUT.VrelDot_force=VrelDot   
    
    if XBOPTS.PrintInfo.value==True:
        sys.stdout.write('Solve nonlinear dynamic case in Python ... \n')
    
    
    
    #------------------------------------------------------ Initialise Variables
    #Initialise structural system tensors
    MssFull = np.zeros((NumDof.value,NumDof.value), ct.c_double, 'F')
    CssFull = np.zeros((NumDof.value,NumDof.value), ct.c_double, 'F') 
    KssFull = np.zeros((NumDof.value,NumDof.value), ct.c_double, 'F') 
    FstrucFull = np.zeros((NumDof.value,NumDof.value), ct.c_double, 'F')
    
    ms = ct.c_int()
    cs = ct.c_int()
    ks = ct.c_int()
    fs = ct.c_int()
    
    Msr = np.zeros((NumDof.value,6), ct.c_double, 'F')
    Csr = np.zeros((NumDof.value,6), ct.c_double, 'F')
    
    X     = np.zeros(NumDof.value, ct.c_double, 'F')
    dXdt  = np.zeros(NumDof.value, ct.c_double, 'F')
    Force_Dof = np.zeros(NumDof.value, ct.c_double, 'F')
    
    Qstruc = np.zeros(NumDof.value, ct.c_double, 'F')
    
    #Initialise rigid-body system tensors
    MrsFull = np.zeros((6,NumDof.value), ct.c_double, 'F')
    CrsFull = np.zeros((6,NumDof.value), ct.c_double, 'F') 
    KrsFull = np.zeros((6,NumDof.value), ct.c_double, 'F') 
    FrigidFull = np.zeros((6,NumDof.value+6), ct.c_double, 'F')
    
    mr = ct.c_int()
    cr = ct.c_int()
    kr = ct.c_int()
    fr = ct.c_int()
    
    Mrr = np.zeros((6,6), ct.c_double, 'F')
    Crr = np.zeros((6,6), ct.c_double, 'F')
        
    Qrigid = np.zeros(6, ct.c_double, 'F')
    
    #Initialise full system tensors
    Q     = np.zeros(NumDof.value+6+4, ct.c_double, 'F')
    DQ    = np.zeros(NumDof.value+6+4, ct.c_double, 'F')
    dQdt  = np.zeros(NumDof.value+6+4, ct.c_double, 'F')
    dQddt = np.zeros(NumDof.value+6+4, ct.c_double, 'F')
    Force_All = np.zeros(NumDof.value+6, ct.c_double, 'F')

    Msys = np.zeros((NumDof.value+6+4,NumDof.value+6+4), ct.c_double, 'F')
    Csys = np.zeros((NumDof.value+6+4,NumDof.value+6+4), ct.c_double, 'F') 
    Ksys = np.zeros((NumDof.value+6+4,NumDof.value+6+4), ct.c_double, 'F') 
    Asys = np.zeros((NumDof.value+6+4,NumDof.value+6+4), ct.c_double, 'F')
    
    Qsys = np.zeros(NumDof.value+6+4, ct.c_double, 'F')
    

    
    #---------------------------------------------------- Start Dynamic Solution
    
    #Initialise rotation operators. TODO: include initial AOA here
    currVrel=Vrel[0,:].copy('F')
    
    
    # Initialise attitude:
    Quat =  xbl.psi2quat(XBINPUT.PsiA_G)
    #Quat=   XBINPUT.quat0
    
    #### sm debug
    XBOUT.Quat0=Quat
    XBOUT.currVel0=currVrel
    
    Cao  = xbl.Rot(Quat)
    ACoa = np.zeros((6,6), ct.c_double, 'F')
    ACoa[:3,:3] = np.transpose(Cao)
    ACoa[3:,3:] = np.transpose(Cao)
    Cqr = np.zeros((4,6), ct.c_double, 'F')
    Cqq = np.zeros((4,4), ct.c_double, 'F')
        
    Unit4 = np.zeros((4,4), ct.c_double, 'F')
    for i in range(4):
        Unit4[i,i] = 1.0
    
    # Extract initial displacements and velocities.
    BeamLib.Cbeam_Solv_Disp2State(NumNodes_tot, NumDof, XBINPUT, XBNODE,
                                  PosDefor, PsiDefor, PosDotDef, PsiDotDef,
                                  X, dXdt)
    
    # Approximate initial accelerations.
    PosDotDotDef = np.zeros((NumNodes_tot.value,3),ct.c_double,'F')
    PsiDotDotDef = np.zeros((XBINPUT.NumElems,Settings.MaxElNod,3),
                             ct.c_double, 'F')
    
    #Populate state vector
    Q[:NumDof.value]=X.copy('F')
    dQdt[:NumDof.value]=dXdt.copy('F')
    dQdt[NumDof.value:NumDof.value+6] = Vrel[0,:].copy('F')
    dQdt[NumDof.value+6:]= Quat.copy('F')
    
    #Force at the first time-step
    #Force += (XBINPUT.ForceDyn*ForceTime[0]).copy('F')
    Force += (XBINPUT.ForceDyn[0,:,:]).copy('F')

    #Assemble matrices and loads for structural dynamic analysis
    currVrel=Vrel[0,:].copy('F')
    tmpQuat=Quat.copy('F')
    BeamLib.Cbeam3_Asbly_Dynamic(XBINPUT, NumNodes_tot, XBELEM, XBNODE,
                         PosIni, PsiIni, PosDefor, PsiDefor,
                         PosDotDef, PsiDotDef, PosDotDotDef, PsiDotDotDef,
                         Force, currVrel, 0*currVrel,
                         NumDof, Settings.DimMat,
                         ms, MssFull, Msr,
                         cs, CssFull, Csr,
                         ks, KssFull, fs, FstrucFull,
                         Qstruc, XBOPTS, Cao)
       
    BeamLib.f_fem_m2v(ct.byref(NumNodes_tot),
                              ct.byref(ct.c_int(6)),
                              Force.ctypes.data_as(ct.POINTER(ct.c_double)),
                              ct.byref(NumDof),
                              Force_Dof.ctypes.data_as(ct.POINTER(ct.c_double)),
                              XBNODE.Vdof.ctypes.data_as(ct.POINTER(ct.c_int)) )

    Qstruc -= np.dot(FstrucFull, Force_Dof)
    
    
    #Assemble matrices for rigid-body dynamic analysis
    BeamLib.Xbeam_Asbly_Dynamic(XBINPUT, NumNodes_tot, XBELEM, XBNODE,
                         PosIni, PsiIni, PosDefor, PsiDefor,
                         PosDotDef, PsiDotDef, PosDotDotDef, PsiDotDotDef,
                         currVrel, 0*currVrel, tmpQuat,
                         NumDof, Settings.DimMat,
                         mr, MrsFull, Mrr,
                         cr, CrsFull, Crr, Cqr, Cqq,
                         kr, KrsFull, fr, FrigidFull,
                         Qrigid, XBOPTS, Cao)
    
    BeamLib.f_fem_m2v_nofilter(ct.byref(NumNodes_tot),
                               ct.byref(ct.c_int(6)),
                               Force.ctypes.data_as(ct.POINTER(ct.c_double)),
                               ct.byref(ct.c_int(NumDof.value+6)),
                               Force_All.ctypes.data_as(ct.POINTER(ct.c_double)) )

    Qrigid -= np.dot(FrigidFull, Force_All)
         
#     #Separate assembly of follower and dead loads   
#     tmpForceTime=ForceTime[0].copy('F') 
#     tmpQforces,Dummy,tmpQrigid = xbl.LoadAssembly(XBINPUT, XBELEM, XBNODE, XBOPTS, NumDof, 
#                                  PosIni, PsiIni, PosDefor, PsiDefor, 
#                                  (XBINPUT.ForceStatic_foll + XBINPUT.ForceDyn_foll*tmpForceTime), 
#                                  (XBINPUT.ForceStatic_dead + XBINPUT.ForceDyn_dead*tmpForceTime), 
#                                   Cao,1)
#                            
#     Qstruc -= tmpQforces      
#     Qrigid -= tmpQrigid
    
    #Assemble system matrices
    Msys[:NumDof.value,:NumDof.value] = MssFull.copy('F')
    Msys[:NumDof.value,NumDof.value:NumDof.value+6] = Msr.copy('F')
    Msys[NumDof.value:NumDof.value+6,:NumDof.value] = MrsFull.copy('F')
    Msys[NumDof.value:NumDof.value+6,NumDof.value:NumDof.value+6] = Mrr.copy('F')
    Msys[NumDof.value+6:,NumDof.value+6:] = Unit4.copy('F')
       
    Qsys[:NumDof.value] = Qstruc
    Qsys[NumDof.value:NumDof.value+6] = Qrigid
    Qsys[NumDof.value+6:] = np.dot(Cqq,dQdt[NumDof.value+6:])
       
    # special BCs
    if SphFlag:
        # block translations
        iiblock = [ ii for ii in range(NumDof.value,NumDof.value+3) ]
        # block rotations
        iifree=[] # free rotational dof 
        for ii in range(3):
            if XBINPUT.EnforceAngVel_FoRA[ii] is True:
                iiblock.append(NumDof.value+3+ii)
            else:
                iifree.append(NumDof.value+3+ii)

        # block dof
        Msys[iiblock,:] = 0.0
        Msys[iiblock,iiblock] = 1.0
        Qsys[iiblock] = 0.0
        
        # add damp at the spherical joints
        if XBINPUT.sph_joint_damping is not None:
            Qsys[iifree]+= XBINPUT.sph_joint_damping*dQdt[iifree]
            
    # add structural damping term
    if XBINPUT.str_damping_model is not None:
        Cdamp = XBINPUT.str_damping_param['alpha'] * MssFull + \
                XBINPUT.str_damping_param['beta']  * KssFull
        Qsys[:NumDof.value] += np.dot( Cdamp, dQdt[:NumDof.value] )                  
        pass
        
    #store initial matrices for eigenvalues analysis
    XBOUT.MssFull0 = MssFull.copy()
    XBOUT.CssFull0 = CssFull.copy()
    XBOUT.KssFull0 = KssFull.copy()

    # Initial Accel.
    ###dQddt[:] = np.dot(np.linalg.inv(Msys), -Qsys)
    dQddt[:] = np.linalg.solve(Msys,-Qsys)
    
    XBOUT.dQddt0=dQddt.copy()
    
    #Record position of all grid points in global FoR at initial time step
    DynOut[0:NumNodes_tot.value,:] = PosDefor
    
    #Position/rotation of the selected node in initial deformed configuration
    PosPsiTime[0,:3] = PosDefor[-1,:]
    PosPsiTime[0,3:] = PsiDefor[-1,XBELEM.NumNodes[-1]-1,:]
    
    
    #Get gamma and beta for Newmark scheme
    gamma = 0.5 + XBOPTS.NewmarkDamp.value
    beta = 0.25*(gamma + 0.5)**2
    
    
    #---------------------------------------------- Initialise Aerodynamic Force
    # Initialise Aero       
    Section = InitSection(VMOPTS,VMINPUT,AELAOPTS.ElasticAxis)
    
    # Declare memory for Aero variables.
    ZetaDot = np.zeros((Section.shape[0],PosDefor.shape[0],3),ct.c_double,'C')
    K = VMOPTS.M.value*VMOPTS.N.value
    AIC = np.zeros((K,K),ct.c_double,'C')
    BIC = np.zeros((K,K),ct.c_double,'C')
    AeroForces = np.zeros((VMOPTS.M.value+1,VMOPTS.N.value+1,3),ct.c_double,'C')
    
    # Initialise A-frame location and orientation to be zero.
    OriginA_a = np.zeros(3,ct.c_double,'C')
    PsiA_G = XBINPUT.PsiA_G.copy() #xbl.quat2psi(Quat) # CRV at iStep    
    
    # Init external velocities.  
    Ufree = InitSteadyExternalVels(VMOPTS,VMINPUT)
    if AELAOPTS.ImpStart == True:
        Zeta = np.zeros((Section.shape[0],PosDefor.shape[0],3),ct.c_double,'C')             
        Gamma = np.zeros((VMOPTS.M.value,VMOPTS.N.value),ct.c_double,'C')
        # Generate surface, wake and gamma matrices.
        CoincidentGrid(PosDefor, PsiDefor, Section, currVrel[:3], 
                       currVrel[3:], PosDotDef, PsiDotDef, XBINPUT,
                       Zeta, ZetaDot, OriginA_a, PsiA_G,
                       VMINPUT.ctrlSurf)
        # init wake grid and gamma matrix.
        ZetaStar, GammaStar = InitSteadyWake(VMOPTS,VMINPUT,Zeta,currVrel[:3])
        
    # sm save
    #XBOUT.Zeta0 = Zeta.copy('C')
    #XBOUT.ZetaStar0 = ZetaStar.copy('C')
    #XBOUT.ZetaStarList.append(np.float32( ZetaStar.copy('C') ))
    
    # Define TecPlot stuff
    if Settings.PlotTec==True:
        FileObject=write_TecPlot(Zeta, ZetaStar, Gamma, GammaStar, NumSteps.value, 0, Time[0], SaveDict)
    
    if 'writeDict' in kwords and Settings.WriteOut == True:
        fp= write_SOL912_out(Time[0], PosDefor, PsiDefor, PosIni, PsiIni, XBELEM, kwords['writeDict'], SaveDict)
            
    
    # sm write class
    XBOUT.QuatList.append(Quat.copy())
    XBOUT.CRVList.append(PsiA_G)
    XBOUT.PosIni=PosIni
    XBOUT.PsiIni=PsiIni    
    
    XBOUT.AsysListStart=[]
    XBOUT.AsysListEnd=[]
    XBOUT.MsysList=[]
    XBOUT.CsysList=[]
    XBOUT.KsysList=[]
    

    #---------------------------------------------------------------- Time loop
    for iStep in range(NumSteps.value):
        
        if XBOPTS.PrintInfo.value==True:
            sys.stdout.write('Time: %-10.4e\n' %(Time[iStep+1]))
            sys.stdout.write('   SubIter DeltaF     DeltaX     ResLog10\n')
        
        #calculate dt
        dt = Time[iStep+1] - Time[iStep]
        
        # Set dt for aero force calcs.
        VMOPTS.DelTime = ct.c_double(dt)
        
        #Predictor step
        Q       += dt*dQdt + (0.5-beta)*dQddt*np.power(dt,2.0)
        dQdt    += (1.0-gamma)*dQddt*dt
        dQddt[:] = 0.0
        
        # Quaternion update for orientation.
        Quat = dQdt[NumDof.value+6:].copy('F')
        Quat = Quat/np.linalg.norm(Quat)
        Cao  = xbl.Rot(Quat)
        
        #nodal displacements and velocities from state vector
        X=Q[:NumDof.value].copy('F') 
        dXdt=dQdt[:NumDof.value].copy('F'); 
        BeamLib.Cbeam3_Solv_State2Disp(XBINPUT,NumNodes_tot,XBELEM,XBNODE,
                                       PosIni,PsiIni,NumDof,X,dXdt,
                                       PosDefor,PsiDefor,PosDotDef,PsiDotDef)
        
            
        #------------------------------------------------------- Aero Force Loop
        # Force at current time-step. TODO: Check communication flow. 
        if iStep > 0 and AELAOPTS.Tight == False:
            
            # zero aero forces.
            AeroForces[:,:,:] = 0.0
            
            # Update CRV.
            PsiA_G = xbl.quat2psi(Quat) # CRV at iStep
            
            # Update origin.
            # sm: origin position projected in FoR A
            currVrel=Vrel[iStep-1,:].copy('F')
            OriginA_a[:] = OriginA_a[:] + currVrel[:3]*dt #sm: OriginA_a initialised to zero
            
            # Update control surface deflection.
            if VMINPUT.ctrlSurf != None:
                # open-loop control
                for cc in range(len(VMINPUT.ctrlSurf)):
                    VMINPUT.ctrlSurf[cc].update(Time[iStep],iStep=iStep)
            
            # Generate surface grid.
            currVrel=Vrel[iStep,:].copy('F')
            CoincidentGrid(PosDefor, PsiDefor, Section, currVrel[:3], 
                           currVrel[3:], PosDotDef, PsiDotDef, XBINPUT,
                           Zeta, ZetaDot, OriginA_a, PsiA_G,
                           VMINPUT.ctrlSurf)
            
            # Update wake geom       
            #'roll' data.
            ZetaStar = np.roll(ZetaStar,1,axis = 0)
            GammaStar = np.roll(GammaStar,1,axis = 0)
            #overwrite grid points with TE.
            ZetaStar[0,:] = Zeta[VMOPTS.M.value,:]
            # overwrite Gamma with TE value from previous timestep.
            GammaStar[0,:] = Gamma[VMOPTS.M.value-1,:]
            
            # Apply gust velocity.
            if VMINPUT.gust != None:
                Utot = Ufree + VMINPUT.gust.Vels(Zeta)
            else:
                Utot = Ufree
            
            # Solve for AeroForces
            UVLMLib.Cpp_Solver_VLM(Zeta, ZetaDot, Utot, ZetaStar, VMOPTS, 
                           AeroForces, Gamma, GammaStar, AIC, BIC)
            
            # Apply density scaling
            AeroForces[:,:,:] = AELAOPTS.AirDensity*AeroForces[:,:,:]
            
            if Settings.PlotTec==True:
                FileObject=write_TecPlot(Zeta, ZetaStar, Gamma, GammaStar, NumSteps.value, iStep, 
                                         Time[iStep], SaveDict,FileObject=FileObject)

            # map AeroForces to beam.
            CoincidentGridForce(XBINPUT, PsiDefor, Section, AeroForces,
                                Force, PsiA_G)

            ForceAero = Force.copy('C')  
            
            # Add gravity loads.
            AddGravityLoads(Force, XBINPUT, XBELEM, AELAOPTS,
                            PsiDefor, VMINPUT.c)
            
            # Add thrust and other point loads
            #Force += (XBINPUT.ForceStatic + XBINPUT.ForceDyn*ForceTime[iStep+1]).copy('F')
            Force += (XBINPUT.ForceStatic + XBINPUT.ForceDyn[iStep+1,:,:]).copy('F')
                      
            # sm: here to avoid crash at first time-step
            XBOUT.ForceAeroList.append(ForceAero.copy('C')) 
         
        
        #END if iStep > 0        
            
        # sm: save aero data
        if ( SaveDict['SaveWake']==True and 
                 iStep%SaveDict['SaveWakeFreq'] == 0 ):
            nfile=iStep//SaveDict['SaveWakeFreq']
            hdwake=h5py.File(dirwake+'%.4d.h5'%nfile,'w')
            hdwake['iStep']=iStep
            hdwake['Zeta']= np.float32(Zeta.copy('C'))
            hdwake['ZetaStar']= np.float32(ZetaStar.copy('C'))            
            hdwake.close()
            #XBOUT.ZetaList.append( np.float32(Zeta.copy('C')) )
            #XBOUT.ZetaStarList.append( np.float32(ZetaStar.copy('C')) )
            #XBOUT.GammaStarList.append(GammaStar.copy('C'))
            #XBOUT.GammaList.append(Gamma.copy('C')) 
                      
          
        #Reset convergence parameters
        Iter = 0
        ResLog10 = 1.0
        
        
        #Newton-Raphson loop      
        while ( (ResLog10 > XBOPTS.MinDelta.value) & (Iter < XBOPTS.MaxIterations.value) ):
                                    
            #set tensors to zero 
            MssFull[:,:] = 0.0; CssFull[:,:] = 0.0
            KssFull[:,:] = 0.0; FstrucFull[:,:] = 0.0
            Msr[:,:] = 0.0; Csr[:,:] = 0.0
            Qstruc[:] = 0.0
            
            MrsFull[:,:] = 0.0; CrsFull[:,:] = 0.0
            KrsFull[:,:] = 0.0; FrigidFull[:,:] = 0.0
            Mrr[:,:] = 0.0; Crr[:,:] = 0.0
            Qrigid[:] = 0.0
    
            Msys[:,:] = 0.0; Csys[:,:] = 0.0
            Ksys[:,:] = 0.0; Asys[:,:] = 0.0
            Qsys[:] = 0.0
            
            # Update counter.
            Iter += 1
            if XBOPTS.PrintInfo.value==True: sys.stdout.write('   %-7d ' %(Iter))
                        
            # Nodal displacements and velocities from state vector
            X=Q[:NumDof.value].copy('F') 
            dXdt=dQdt[:NumDof.value].copy('F'); 
            BeamLib.Cbeam3_Solv_State2Disp(XBINPUT, NumNodes_tot,
                                           XBELEM, XBNODE,
                                           PosIni, PsiIni,
                                           NumDof, X, dXdt,
                                           PosDefor, PsiDefor,
                                           PosDotDef, PsiDotDef)


            #rigid-body velocities and orientation from state vector
            Vrel[iStep+1,:]    = dQdt[NumDof.value:NumDof.value+6].copy('F')
            VrelDot[iStep+1,:] = dQddt[NumDof.value:NumDof.value+6].copy('F')
            Quat = dQdt[NumDof.value+6:].copy('F')
            Quat = Quat/np.linalg.norm(Quat)
            Cao  = xbl.Rot(Quat)


            #Update matrices and loads for structural dynamic analysis
            tmpVrel=Vrel[iStep+1,:].copy('F')
            tmpQuat=Quat.copy('F')
            BeamLib.Cbeam3_Asbly_Dynamic(XBINPUT, NumNodes_tot, XBELEM, XBNODE,
                                 PosIni, PsiIni, PosDefor, PsiDefor,
                                 PosDotDef, PsiDotDef, PosDotDotDef, PsiDotDotDef,
                                 Force, tmpVrel, 0*tmpVrel,
                                 NumDof, Settings.DimMat,
                                 ms, MssFull, Msr,
                                 cs, CssFull, Csr,
                                 ks, KssFull, fs, FstrucFull,
                                 Qstruc, XBOPTS, Cao)
            
            BeamLib.f_fem_m2v(ct.byref(NumNodes_tot),
                              ct.byref(ct.c_int(6)),
                              Force.ctypes.data_as(ct.POINTER(ct.c_double)),
                              ct.byref(NumDof),
                              Force_Dof.ctypes.data_as(ct.POINTER(ct.c_double)),
                              XBNODE.Vdof.ctypes.data_as(ct.POINTER(ct.c_int)) )
                    
            
            #Update matrices for rigid-body dynamic analysis
            BeamLib.Xbeam_Asbly_Dynamic(XBINPUT, NumNodes_tot, XBELEM, XBNODE,
                                 PosIni, PsiIni, PosDefor, PsiDefor,
                                 PosDotDef, PsiDotDef, PosDotDotDef, PsiDotDotDef,
                                 tmpVrel, 0*tmpVrel, tmpQuat,
                                 NumDof, Settings.DimMat,
                                 mr, MrsFull, Mrr,
                                 cr, CrsFull, Crr, Cqr, Cqq,
                                 kr, KrsFull, fs, FrigidFull,
                                 Qrigid, XBOPTS, Cao)
    
            BeamLib.f_fem_m2v_nofilter(ct.byref(NumNodes_tot),
                                       ct.byref(ct.c_int(6)),
                                       Force.ctypes.data_as(ct.POINTER(ct.c_double)),
                                       ct.byref(ct.c_int(NumDof.value+6)),
                                       Force_All.ctypes.data_as(ct.POINTER(ct.c_double)) )
        
        
            #Residual at first iteration
            if(Iter == 1):
                Res0_Qglobal = max(max(abs(Qsys)),1)
                Res0_DeltaX  = max(max(abs(DQ)),1)
              
            
            #Assemble discrete system matrices with linearised quaternion equations          
            Msys[:NumDof.value,:NumDof.value] = MssFull.copy('F')
            Msys[:NumDof.value,NumDof.value:NumDof.value+6] = Msr.copy('F')
            Msys[NumDof.value:NumDof.value+6,:NumDof.value] = MrsFull.copy('F')
            Msys[NumDof.value:NumDof.value+6,NumDof.value:NumDof.value+6] = Mrr.copy('F')
            Msys[NumDof.value+6:,NumDof.value+6:] = Unit4.copy('F')
            
            Csys[:NumDof.value,:NumDof.value] = CssFull.copy('F')
            Csys[:NumDof.value,NumDof.value:NumDof.value+6] = Csr.copy('F')
            Csys[NumDof.value:NumDof.value+6,:NumDof.value] = CrsFull.copy('F')
            Csys[NumDof.value:NumDof.value+6,NumDof.value:NumDof.value+6] = Crr.copy('F')
            
            Csys[NumDof.value+6:,NumDof.value:NumDof.value+6] = Cqr.copy('F')
            Csys[NumDof.value+6:,NumDof.value+6:] = Cqq.copy('F')
            
            Ksys[:NumDof.value,:NumDof.value] = KssFull.copy('F')
            Ksys[NumDof.value:NumDof.value+6,:NumDof.value] = KrsFull.copy('F')
                     
#             #Separate assembly of follower and dead loads   
#             tmpForceTime=ForceTime[iStep+1].copy('F') 
#             tmpQforces,Dummy,tmpQrigid = xbl.LoadAssembly(XBINPUT, XBELEM, XBNODE, XBOPTS, NumDof, \
#                                             PosIni, PsiIni, PosDefor, PsiDefor, \
#                                             (XBINPUT.ForceStatic_foll + XBINPUT.ForceDyn_foll*tmpForceTime), \
#                                             (XBINPUT.ForceStatic_dead + XBINPUT.ForceDyn_dead*tmpForceTime), \
#                                             Cao,1)
#                                    
#             Qstruc -= tmpQforces      
#             Qrigid -= tmpQrigid

            #Compute residual to solve update vector
            Qstruc += -np.dot(FstrucFull, Force_Dof)
            Qrigid += -np.dot(FrigidFull, Force_All)
            
            Qsys[:NumDof.value] = Qstruc
            Qsys[NumDof.value:NumDof.value+6] = Qrigid
            Qsys[NumDof.value+6:] = np.dot(Cqq,dQdt[NumDof.value+6:])

            Qsys += np.dot(Msys,dQddt)
            
            # include damping
            if XBINPUT.str_damping_model == 'prop':
                Cdamp = XBINPUT.str_damping_param['alpha'] * MssFull + \
                        XBINPUT.str_damping_param['beta']  * KssFull
                Csys[:NumDof.value,:NumDof.value] += Cdamp
                Qsys[:NumDof.value] += np.dot(Cdamp, dQdt[:NumDof.value])
                                

            # special BCs
            if SphFlag:
                Msys[iiblock,:] = 0.0
                Msys[iiblock,iiblock] = 1.0
                Csys[iiblock,:] = 0.0
                Ksys[iiblock,:] = 0.0
                Qsys[iiblock]   = 0.0
                if XBINPUT.sph_joint_damping is not None:
                    Csys[iifree,iifree] += XBINPUT.sph_joint_damping
                    Qsys[iifree] += XBINPUT.sph_joint_damping*dQdt[iifree]
          
            #Calculate system matrix for update calculation
            Asys = Ksys + Csys*gamma/(beta*dt) + Msys/(beta*dt**2)
            
            #Compute correction
            
            ###DQ[:] = np.dot(np.linalg.inv(Asys), -Qsys)
            DQ[:] = np.linalg.solve(Asys,-Qsys)

            Q += DQ
            dQdt += DQ*gamma/(beta*dt)
            dQddt += DQ/(beta*dt**2)
            
            
            #Update convergence criteria
            if XBOPTS.PrintInfo.value==True:                 
                sys.stdout.write('%-10.4e ' %(max(abs(Qsys))))
            
            Res_Qglobal = max(abs(Qsys))
            Res_DeltaX  = max(abs(DQ))
            
            ResLog10 = max(Res_Qglobal/Res0_Qglobal,Res_DeltaX/Res0_DeltaX)
            
            if XBOPTS.PrintInfo.value==True:
                sys.stdout.write('%-10.4e %8.4f\n' %(max(abs(DQ)),ResLog10))

            if SaveExtraVariables is True:
                if Iter == 1:
                    XBOUT.AsysListStart.append(Asys.copy())
                if ( (ResLog10 < XBOPTS.MinDelta.value) or (Iter >= XBOPTS.MaxIterations.value) ):
                    XBOUT.AsysListEnd.append(Asys.copy())
                    XBOUT.MsysList.append(Msys.copy())
                    XBOUT.CsysList.append(Csys.copy())
                    XBOUT.KsysList.append(Ksys.copy())
        # END Netwon-Raphson.
        
        
        # sm debug:
        # save forcing terms:
        XBOUT.ForceDofList.append( np.dot(FstrucFull, Force_Dof).copy() )
        XBOUT.ForceRigidList.append( np.dot(FrigidFull, Force_All).copy() )
              
        #update to converged nodal displacements and velocities
        X=Q[:NumDof.value].copy('F') 
        dXdt=dQdt[:NumDof.value].copy('F'); 
        BeamLib.Cbeam3_Solv_State2Disp(XBINPUT, NumNodes_tot, XBELEM, XBNODE,
                           PosIni, PsiIni, NumDof, X, dXdt,\
                           PosDefor, PsiDefor, PosDotDef, PsiDotDef)
        
        PosPsiTime[iStep+1,:3] = PosDefor[-1,:]
        PosPsiTime[iStep+1,3:] = PsiDefor[-1,XBELEM.NumNodes[-1]-1,:]
        
        #Position of all grid points in global FoR
        i1 = (iStep+1)*NumNodes_tot.value
        i2 = (iStep+2)*NumNodes_tot.value
        DynOut[i1:i2,:] = PosDefor
        
        #Export rigid-body velocities/accelerations
        if XBOPTS.OutInaframe.value==True:
            Vrel[iStep,:] = dQdt[NumDof.value:NumDof.value+6].copy('F')
            VrelDot[iStep,:] = dQddt[NumDof.value:NumDof.value+6].copy('F')
        else:
            Quat = dQdt[NumDof.value+6:].copy('F')
            Quat = Quat/np.linalg.norm(Quat)
            Cao  = xbl.Rot(Quat)
            ACoa[:3,:3] = np.transpose(Cao)
            ACoa[3:,3:] = np.transpose(Cao)
            
            Vrel[iStep,:] = np.dot(ACoa,dQdt[NumDof.value:NumDof.value+6].copy('F'))
            VrelDot[iStep,:] = np.dot(ACoa,dQddt[NumDof.value:NumDof.value+6].copy('F'))
        
        
        if 'writeDict' in kwords and Settings.WriteOut == True:
            fp= write_SOL912_out(Time[iStep+1], PosDefor, PsiDefor, PosIni, PsiIni, XBELEM, 
                                 kwords['writeDict'], SaveDict, FileObject=fp)
                    
        # 'Rollup' due to external velocities. TODO: Must add gusts here!
        ZetaStar[:,:] = ZetaStar[:,:] + VMINPUT.U_infty*dt
        
        # sm: append outputs
        XBOUT.QuatList.append(Quat.copy())
        XBOUT.CRVList.append(PsiA_G.copy())
 
        # sm I/O: FoR A velocities/accelerations
        XBOUT.Time=Time                     # ...dyn.dat
        #XBOUT.PosPsiTime = PosPsiTime       
        
        XBOUT.DynOut=DynOut                 # ...shape.dat
        
        XBOUT.Vrel=Vrel                     # ...rigid.dat
        XBOUT.VrelDot=VrelDot
        #XBOUT.PosPsiTime=PosPsiTime          
        
        XBOUT.PsiList.append(PsiDefor.copy())   
        
        XBOUT.cputime.append( time.clock() - XBOUT.cputime[0] )
        
        if SaveDict['SaveProgress']:
            iisave=np.arange(1,NumSteps.value,np.ceil(NumSteps.value/SaveDict['NumSavePoints']))
            if any(iisave==iStep):
                PyLibs.io.save.h5file(SaveDict['OutputDir'], SaveDict['OutputFileRoot']+'.h5', 
                                      *OutList)
        
    # END Time loop
    
    
    if SaveDict['Format'] != 'h5': 
        write_SOL912_final(Time, PosPsiTime, NumNodes_tot, DynOut, Vrel, VrelDot, SaveDict) 
        
    # Close output file if it exists.
    if 'writeDict' in kwords and Settings.WriteOut == True:
        fp.close()
        
    # Close TecPlot ASCII FileObject.
    if Settings.PlotTec==True:
        PostProcess.CloseAeroTecFile(FileObject)
        
    if XBOPTS.PrintInfo.value==True:
        sys.stdout.write(' ... done\n')
        
    # For interactive analysis at end of simulation set breakpoint.
    pass

    # sm I/O: FoR A velocities/accelerations

    XBOUT.Time=Time                     # ...dyn.dat
    XBOUT.PosPsiTime = PosPsiTime       
    
    XBOUT.DynOut=DynOut                 # ...sgape.dat
    
    XBOUT.Vrel=Vrel                     # ...rigid.dat
    XBOUT.VrelDot=VrelDot
    XBOUT.PosPsiTime=PosPsiTime   
    
    if  SaveDict['SaveWake'] is True:
        #XBOUT.dirwake=dirwake  
        XBOUT.NTwake=NumSteps.value//SaveDict['SaveWakeFreq']
    
    #saveh5(SaveDict, AELAOPTS, VMINPUT, VMOPTS, XBOPTS, XBINPUT, XBOUT )
    PyLibs.io.save.h5file(SaveDict['OutputDir'], SaveDict['OutputFileRoot']+'.h5', *OutList)
    
    return XBOUT
def Solve_Py(XBINPUT,XBOPTS,VMOPTS,VMINPUT,AELAOPTS,**kwords):
    """@brief Nonlinear dynamic solver using Python to solve aeroelastic
    equation.
    @details Assembly of structural matrices is carried out with 
    Fortran subroutines. Aerodynamics solved using PyAero\.UVLM.
    @param XBINPUT Beam inputs (for initialization in Python).
    @param XBOPTS Beam solver options (for Fortran).
    @param VMOPTS UVLM solver options (for C/C++).
    @param VMINPUT UVLM solver inputs (for initialization in Python).
    @param VMUNST Unsteady input information for aero solver.
    @param AELAOPTS Options relevant to coupled aeroelastic simulations.
    @param writeDict OrderedDict of 'name':tuple of outputs to write.
    """
        
    # Check correct solution code.
    assert XBOPTS.Solution.value == 912, ('NonlinearFlightDynamic requested' +
                                          ' with wrong solution code')
    # Initialise static beam data.
    XBINPUT, XBOPTS, NumNodes_tot, XBELEM, PosIni, PsiIni, XBNODE, NumDof \
                = BeamInit.Static(XBINPUT,XBOPTS)
                
    # Calculate initial displacements.
    if AELAOPTS.ImpStart == False:
        XBOPTS.Solution.value = 112 # Modify options.
        VMOPTS.Steady = ct.c_bool(True)
        Rollup = VMOPTS.Rollup.value
        VMOPTS.Rollup.value = False
        # Solve Static Aeroelastic.
        PosDefor, PsiDefor, Zeta, ZetaStar, Gamma, GammaStar, Force = \
                    Static.Solve_Py(XBINPUT, XBOPTS, VMOPTS, VMINPUT, AELAOPTS)
        XBOPTS.Solution.value = 912 # Reset options.
        VMOPTS.Steady = ct.c_bool(False)
        VMOPTS.Rollup.value = Rollup
    elif AELAOPTS.ImpStart == True:
        PosDefor = PosIni.copy(order='F')
        PsiDefor = PsiIni.copy(order='F')
        Force = np.zeros((XBINPUT.NumNodesTot,6),ct.c_double,'F')
        
    # Write deformed configuration to file. TODO: tidy this away inside function.
    ofile = Settings.OutputDir + Settings.OutputFileRoot + '_SOL912_def.dat'
    if XBOPTS.PrintInfo==True:
        sys.stdout.write('Writing file %s ... ' %(ofile))
    fp = open(ofile,'w')
    fp.write('TITLE="Non-linear static solution: deformed geometry"\n')
    fp.write('VARIABLES="iElem" "iNode" "Px" "Py" "Pz" "Rx" "Ry" "Rz"\n')
    fp.close()
    if XBOPTS.PrintInfo==True:
        sys.stdout.write('done\n')
    WriteMode = 'a'
    # Write
    BeamIO.OutputElems(XBINPUT.NumElems, NumNodes_tot.value, XBELEM,
                       PosDefor, PsiDefor, ofile, WriteMode)
    
    # Initialise structural variables for dynamic analysis.
    Time, NumSteps, ForceTime, Vrel, VrelDot,\
    PosDotDef, PsiDotDef,\
    OutGrids, PosPsiTime, VelocTime, DynOut\
        = BeamInit.Dynamic(XBINPUT,XBOPTS)
    # Delete unused variables.
    del OutGrids, VelocTime
    
    if XBOPTS.PrintInfo.value==True:
        sys.stdout.write('Solve nonlinear dynamic case in Python ... \n')
    
    
    #Initialise structural system tensors
    MssFull = np.zeros((NumDof.value,NumDof.value), ct.c_double, 'F')
    CssFull = np.zeros((NumDof.value,NumDof.value), ct.c_double, 'F') 
    KssFull = np.zeros((NumDof.value,NumDof.value), ct.c_double, 'F') 
    FstrucFull = np.zeros((NumDof.value,NumDof.value), ct.c_double, 'F')
    
    ms = ct.c_int()
    cs = ct.c_int()
    ks = ct.c_int()
    fs = ct.c_int()
    
    Msr = np.zeros((NumDof.value,6), ct.c_double, 'F')
    Csr = np.zeros((NumDof.value,6), ct.c_double, 'F')
    
    X     = np.zeros(NumDof.value, ct.c_double, 'F')
    dXdt  = np.zeros(NumDof.value, ct.c_double, 'F')
    Force_Dof = np.zeros(NumDof.value, ct.c_double, 'F')
    
    Qstruc = np.zeros(NumDof.value, ct.c_double, 'F')
    
    #Initialise rigid-body system tensors
    MrsFull = np.zeros((6,NumDof.value), ct.c_double, 'F')
    CrsFull = np.zeros((6,NumDof.value), ct.c_double, 'F') 
    KrsFull = np.zeros((6,NumDof.value), ct.c_double, 'F') 
    FrigidFull = np.zeros((6,NumDof.value+6), ct.c_double, 'F')
    
    mr = ct.c_int()
    cr = ct.c_int()
    kr = ct.c_int()
    fr = ct.c_int()
    
    Mrr = np.zeros((6,6), ct.c_double, 'F')
    Crr = np.zeros((6,6), ct.c_double, 'F')
        
    Qrigid = np.zeros(6, ct.c_double, 'F')
    
    #Initialise full system tensors
    Q     = np.zeros(NumDof.value+6+4, ct.c_double, 'F')
    DQ    = np.zeros(NumDof.value+6+4, ct.c_double, 'F')
    dQdt  = np.zeros(NumDof.value+6+4, ct.c_double, 'F')
    dQddt = np.zeros(NumDof.value+6+4, ct.c_double, 'F')
    Force_All = np.zeros(NumDof.value+6, ct.c_double, 'F')

    Msys = np.zeros((NumDof.value+6+4,NumDof.value+6+4), ct.c_double, 'F')
    Csys = np.zeros((NumDof.value+6+4,NumDof.value+6+4), ct.c_double, 'F') 
    Ksys = np.zeros((NumDof.value+6+4,NumDof.value+6+4), ct.c_double, 'F') 
    Asys = np.zeros((NumDof.value+6+4,NumDof.value+6+4), ct.c_double, 'F')
    
    Qsys = np.zeros(NumDof.value+6+4, ct.c_double, 'F')
    
    #Initialise rotation operators. TODO: include initial AOA here
    currVrel=Vrel[0,:].copy('F')
    AOA  = np.arctan(currVrel[2]/-currVrel[1])
    Quat = xbl.Euler2Quat(AOA,0,0)
    Cao  = xbl.Rot(Quat)
    ACoa = np.zeros((6,6), ct.c_double, 'F')
    ACoa[:3,:3] = np.transpose(Cao)
    ACoa[3:,3:] = np.transpose(Cao)
    Cqr = np.zeros((4,6), ct.c_double, 'F')
    Cqq = np.zeros((4,4), ct.c_double, 'F')
        
    Unit4 = np.zeros((4,4), ct.c_double, 'F')
    for i in range(4):
        Unit4[i,i] = 1.0
    
    # Extract initial displacements and velocities.
    BeamLib.Cbeam_Solv_Disp2State(NumNodes_tot, NumDof, XBINPUT, XBNODE,
                                  PosDefor, PsiDefor, PosDotDef, PsiDotDef,
                                  X, dXdt)
    
    # Approximate initial accelerations.
    PosDotDotDef = np.zeros((NumNodes_tot.value,3),ct.c_double,'F')
    PsiDotDotDef = np.zeros((XBINPUT.NumElems,Settings.MaxElNod,3),
                             ct.c_double, 'F')
    
    #Populate state vector
    Q[:NumDof.value]=X.copy('F')
    dQdt[:NumDof.value]=dXdt.copy('F')
    dQdt[NumDof.value:NumDof.value+6] = Vrel[0,:].copy('F')
    dQdt[NumDof.value+6:]= Quat.copy('F')
    
    #Force at the first time-step
    Force += (XBINPUT.ForceDyn*ForceTime[0]).copy('F')
    

    #Assemble matrices and loads for structural dynamic analysis
    currVrel=Vrel[0,:].copy('F')
    tmpQuat=Quat.copy('F')
    BeamLib.Cbeam3_Asbly_Dynamic(XBINPUT, NumNodes_tot, XBELEM, XBNODE,\
                         PosIni, PsiIni, PosDefor, PsiDefor,\
                         PosDotDef, PsiDotDef, PosDotDotDef, PsiDotDotDef,\
                         Force, currVrel, 0*currVrel,\
                         NumDof, Settings.DimMat,\
                         ms, MssFull, Msr,\
                         cs, CssFull, Csr,\
                         ks, KssFull, fs, FstrucFull,\
                         Qstruc, XBOPTS, Cao)
       
    BeamLib.f_fem_m2v(ct.byref(NumNodes_tot),\
                              ct.byref(ct.c_int(6)),\
                              Force.ctypes.data_as(ct.POINTER(ct.c_double)),\
                              ct.byref(NumDof),\
                              Force_Dof.ctypes.data_as(ct.POINTER(ct.c_double)),\
                              XBNODE.Vdof.ctypes.data_as(ct.POINTER(ct.c_int)) )

    Qstruc -= np.dot(FstrucFull, Force_Dof)
    
    
    #Assemble matrices for rigid-body dynamic analysis
    BeamLib.Xbeam_Asbly_Dynamic(XBINPUT, NumNodes_tot, XBELEM, XBNODE,\
                         PosIni, PsiIni, PosDefor, PsiDefor,\
                         PosDotDef, PsiDotDef, PosDotDotDef, PsiDotDotDef,\
                         currVrel, 0*currVrel, tmpQuat,\
                         NumDof, Settings.DimMat,\
                         mr, MrsFull, Mrr,\
                         cr, CrsFull, Crr, Cqr, Cqq,\
                         kr, KrsFull, fr, FrigidFull,\
                         Qrigid, XBOPTS, Cao)
    
    BeamLib.f_fem_m2v_nofilter(ct.byref(NumNodes_tot),\
                               ct.byref(ct.c_int(6)),\
                               Force.ctypes.data_as(ct.POINTER(ct.c_double)),\
                               ct.byref(ct.c_int(NumDof.value+6)),\
                               Force_All.ctypes.data_as(ct.POINTER(ct.c_double)) )

    Qrigid -= np.dot(FrigidFull, Force_All)
        
          
#     #Separate assembly of follower and dead loads   
#     tmpForceTime=ForceTime[0].copy('F') 
#     tmpQforces,Dummy,tmpQrigid = xbl.LoadAssembly(XBINPUT, XBELEM, XBNODE, XBOPTS, NumDof, \
#                                     PosIni, PsiIni, PosDefor, PsiDefor, \
#                                     (XBINPUT.ForceStatic_foll + XBINPUT.ForceDyn_foll*tmpForceTime), \
#                                     (XBINPUT.ForceStatic_dead + XBINPUT.ForceDyn_dead*tmpForceTime), \
#                                     Cao,1)
#                            
#     Qstruc -= tmpQforces      
#     Qrigid -= tmpQrigid
    
    
    #Assemble system matrices
    Msys[:NumDof.value,:NumDof.value] = MssFull.copy('F')
    Msys[:NumDof.value,NumDof.value:NumDof.value+6] = Msr.copy('F')
    Msys[NumDof.value:NumDof.value+6,:NumDof.value] = MrsFull.copy('F')
    Msys[NumDof.value:NumDof.value+6,NumDof.value:NumDof.value+6] = Mrr.copy('F')
    Msys[NumDof.value+6:,NumDof.value+6:] = Unit4.copy('F')
       
    Qsys[:NumDof.value] = Qstruc
    Qsys[NumDof.value:NumDof.value+6] = Qrigid
    Qsys[NumDof.value+6:] = np.dot(Cqq,dQdt[NumDof.value+6:])
       

    # Initial Accel.
    dQddt[:] = np.dot(np.linalg.inv(Msys), -Qsys)
    
    
    #Record position of all grid points in global FoR at initial time step
    DynOut[0:NumNodes_tot.value,:] = PosDefor
    
    #Position/rotation of the selected node in initial deformed configuration
    PosPsiTime[0,:3] = PosDefor[-1,:]
    PosPsiTime[0,3:] = PsiDefor[-1,XBELEM.NumNodes[-1]-1,:]
    
    
    #Get gamma and beta for Newmark scheme
    gamma = 0.5 + XBOPTS.NewmarkDamp.value
    beta = 0.25*(gamma + 0.5)**2
    
    
    # Initialise Aero       
    Section = InitSection(VMOPTS,VMINPUT,AELAOPTS.ElasticAxis)
    
    # Declare memory for Aero variables.
    ZetaDot = np.zeros((Section.shape[0],PosDefor.shape[0],3),ct.c_double,'C')
    K = VMOPTS.M.value*VMOPTS.N.value
    AIC = np.zeros((K,K),ct.c_double,'C')
    BIC = np.zeros((K,K),ct.c_double,'C')
    AeroForces = np.zeros((VMOPTS.M.value+1,VMOPTS.N.value+1,3),ct.c_double,'C')
    
    # Initialise A-frame location and orientation to be zero.
    OriginA_G = np.zeros(3,ct.c_double,'C')
    PsiA_G = xbl.quat2psi(Quat) # CRV at iStep
    
    # Init external velocities.  
    Ufree = InitSteadyExternalVels(VMOPTS,VMINPUT)
    if AELAOPTS.ImpStart == True:
        Zeta = np.zeros((Section.shape[0],PosDefor.shape[0],3),ct.c_double,'C')             
        Gamma = np.zeros((VMOPTS.M.value,VMOPTS.N.value),ct.c_double,'C')
        # Generate surface, wake and gamma matrices.
        CoincidentGrid(PosDefor, PsiDefor, Section, currVrel[:3], 
                       currVrel[3:], PosDotDef, PsiDotDef, XBINPUT,
                       Zeta, ZetaDot, OriginA_G, PsiA_G,
                       VMINPUT.ctrlSurf)
        # init wake grid and gamma matrix.
        ZetaStar, GammaStar = InitSteadyWake(VMOPTS,VMINPUT,Zeta,currVrel[:3])
          
    # Define tecplot stuff
    if Settings.PlotTec==True:
        FileName = Settings.OutputDir + Settings.OutputFileRoot + 'AeroGrid.dat'
        Variables = ['X', 'Y', 'Z','Gamma']        
        FileObject = PostProcess.WriteAeroTecHeader(FileName, 
                                                    'Default',
                                                    Variables)
        # Plot results of static analysis
        PostProcess.WriteUVLMtoTec(FileObject,
                                   Zeta,
                                   ZetaStar,
                                   Gamma,
                                   GammaStar,
                                   TimeStep = 0,
                                   NumTimeSteps = XBOPTS.NumLoadSteps.value,
                                   Time = 0.0,
                                   Text = True)
    
    # Open output file for writing
    if 'writeDict' in kwords and Settings.WriteOut == True:
        writeDict = kwords['writeDict']
        ofile = Settings.OutputDir + \
                Settings.OutputFileRoot + \
                '_SOL912_out.dat'
        fp = open(ofile,'w')
        fp.write("{:<14}".format("Time"))
        for output in writeDict.keys():
            fp.write("{:<14}".format(output))
        fp.write("\n")
        fp.flush()
        
    # Write initial outputs to file.
    if 'writeDict' in kwords and Settings.WriteOut == True:
        locForces = None # Stops recalculation of forces
        fp.write("{:<14,e}".format(Time[0]))
        for myStr in writeDict.keys():
            if re.search(r'^R_.',myStr):
                if re.search(r'^R_._.', myStr):
                    index = int(myStr[4])
                elif re.search(r'root', myStr):
                    index = 0
                elif re.search(r'tip', myStr):
                    index = -1
                else:
                    raise IOError("Node index not recognised.")
                
                if myStr[2] == 'x':
                    component = 0
                elif myStr[2] == 'y':
                    component = 1
                elif myStr[2] == 'z':
                    component = 2
                else:
                    raise IOError("Displacement component not recognised.")
                
                fp.write("{:<14,e}".format(PosDefor[index,component]))
                
            elif re.search(r'^M_.',myStr):
                if re.search(r'^M_._.', myStr):
                    index = int(myStr[4])
                elif re.search(r'root', myStr):
                    index = 0
                elif re.search(r'tip', myStr):
                    index = -1
                else:
                    raise IOError("Node index not recognised.")
                
                if myStr[2] == 'x':
                    component = 0
                elif myStr[2] == 'y':
                    component = 1
                elif myStr[2] == 'z':
                    component = 2
                else:
                    raise IOError("Moment component not recognised.")
                
                if locForces == None:
                    locForces = BeamIO.localElasticForces(PosDefor,
                                                          PsiDefor,
                                                          PosIni,
                                                          PsiIni,
                                                          XBELEM,
                                                          [index])
                
                fp.write("{:<14,e}".format(locForces[0,3+component]))
            else:
                raise IOError("writeDict key not recognised.")
        # END for myStr
        fp.write("\n")
        fp.flush()
    # END if write

    # Time loop.
    for iStep in range(NumSteps.value):
        
        if XBOPTS.PrintInfo.value==True:
            sys.stdout.write('Time: %-10.4e\n' %(Time[iStep+1]))
            sys.stdout.write('   SubIter DeltaF     DeltaX     ResLog10\n')
        
        #calculate dt
        dt = Time[iStep+1] - Time[iStep]
        
        # Set dt for aero force calcs.
        VMOPTS.DelTime = ct.c_double(dt)
        
        #Predictor step
        Q       += dt*dQdt + (0.5-beta)*dQddt*np.power(dt,2.0)
        dQdt    += (1.0-gamma)*dQddt*dt
        dQddt[:] = 0.0
        
        # Quaternion update for orientation.
        Quat = dQdt[NumDof.value+6:].copy('F')
        Quat = Quat/np.linalg.norm(Quat)
        Cao  = xbl.Rot(Quat)
        
        #nodal diplacements and velocities from state vector
        X=Q[:NumDof.value].copy('F') 
        dXdt=dQdt[:NumDof.value].copy('F'); 
        BeamLib.Cbeam3_Solv_State2Disp(XBINPUT,NumNodes_tot,XBELEM,XBNODE,
                                       PosIni,PsiIni,NumDof,X,dXdt,
                                       PosDefor,PsiDefor,PosDotDef,PsiDotDef)
            
        # Force at current time-step. TODO: Check communication flow. 
        if iStep > 0 and AELAOPTS.Tight == False:
            
            # zero aero forces.
            AeroForces[:,:,:] = 0.0
            
            # Update CRV.
            PsiA_G = xbl.quat2psi(Quat) # CRV at iStep
            
            # Update origin.
            currVrel=Vrel[iStep-1,:].copy('F')
            OriginA_G[:] = OriginA_G[:] + currVrel[:3]*dt
            
            # Update control surface deflection.
            if VMINPUT.ctrlSurf != None:
                VMINPUT.ctrlSurf.update(Time[iStep])
            
            # Generate surface grid.
            currVrel=Vrel[iStep,:].copy('F')
            CoincidentGrid(PosDefor, PsiDefor, Section, currVrel[:3], 
                           currVrel[3:], PosDotDef, PsiDotDef, XBINPUT,
                           Zeta, ZetaDot, OriginA_G, PsiA_G,
                           VMINPUT.ctrlSurf)
            
            # Update wake geom       
            #'roll' data.
            ZetaStar = np.roll(ZetaStar,1,axis = 0)
            GammaStar = np.roll(GammaStar,1,axis = 0)
            #overwrite grid points with TE.
            ZetaStar[0,:] = Zeta[VMOPTS.M.value,:]
            # overwrite Gamma with TE value from previous timestep.
            GammaStar[0,:] = Gamma[VMOPTS.M.value-1,:]
            
            # Apply gust velocity.
            if VMINPUT.gust != None:
                Utot = Ufree + VMINPUT.gust.Vels(Zeta)
            else:
                Utot = Ufree
            
            # Solve for AeroForces
            UVLMLib.Cpp_Solver_VLM(Zeta, ZetaDot, Utot, ZetaStar, VMOPTS, 
                           AeroForces, Gamma, GammaStar, AIC, BIC)
            
            # Apply density scaling
            AeroForces[:,:,:] = AELAOPTS.AirDensity*AeroForces[:,:,:]
            
            if Settings.PlotTec==True:
                PostProcess.WriteUVLMtoTec(FileObject,
                                           Zeta,
                                           ZetaStar,
                                           Gamma,
                                           GammaStar,
                                           TimeStep = iStep,
                                           NumTimeSteps = XBOPTS.NumLoadSteps.value,
                                           Time = Time[iStep],
                                           Text = True)

            # map AeroForces to beam.
            CoincidentGridForce(XBINPUT, PsiDefor, Section, AeroForces,
                                Force)
            
            # Add gravity loads.
            AddGravityLoads(Force, XBINPUT, XBELEM, AELAOPTS,
                            PsiDefor, VMINPUT.c)
            
            # Add thrust and other point loads
            Force += (XBINPUT.ForceStatic + 
                      XBINPUT.ForceDyn*ForceTime[iStep+1]).copy('F')
        #END if iStep > 0
            
            
        #Reset convergence parameters
        Iter = 0
        ResLog10 = 1.0
        
        
        #Newton-Raphson loop      
        while ( (ResLog10 > XBOPTS.MinDelta.value) \
                & (Iter < XBOPTS.MaxIterations.value) ):
                                    
            #set tensors to zero 
            MssFull[:,:] = 0.0; CssFull[:,:] = 0.0
            KssFull[:,:] = 0.0; FstrucFull[:,:] = 0.0
            Msr[:,:] = 0.0; Csr[:,:] = 0.0
            Qstruc[:] = 0.0
            
            MrsFull[:,:] = 0.0; CrsFull[:,:] = 0.0
            KrsFull[:,:] = 0.0; FrigidFull[:,:] = 0.0
            Mrr[:,:] = 0.0; Crr[:,:] = 0.0
            Qrigid[:] = 0.0
    
            Msys[:,:] = 0.0; Csys[:,:] = 0.0
            Ksys[:,:] = 0.0; Asys[:,:] = 0.0;
            Qsys[:] = 0.0
            
            # Update counter.
            Iter += 1
            
            if XBOPTS.PrintInfo.value==True:
                sys.stdout.write('   %-7d ' %(Iter))
                        
            #nodal diplacements and velocities from state vector
            X=Q[:NumDof.value].copy('F') 
            dXdt=dQdt[:NumDof.value].copy('F'); 
            BeamLib.Cbeam3_Solv_State2Disp(XBINPUT,
                                           NumNodes_tot,
                                           XBELEM,
                                           XBNODE,
                                           PosIni,
                                           PsiIni,
                                           NumDof,
                                           X,
                                           dXdt,
                                           PosDefor,
                                           PsiDefor,
                                           PosDotDef,
                                           PsiDotDef)


            #rigid-body velocities and orientation from state vector
            Vrel[iStep+1,:]    = dQdt[NumDof.value:NumDof.value+6].copy('F')
            VrelDot[iStep+1,:] = dQddt[NumDof.value:NumDof.value+6].copy('F')
            Quat = dQdt[NumDof.value+6:].copy('F')
            Quat = Quat/np.linalg.norm(Quat)
            Cao  = xbl.Rot(Quat)


            #Update matrices and loads for structural dynamic analysis
            tmpVrel=Vrel[iStep+1,:].copy('F')
            tmpQuat=Quat.copy('F')
            BeamLib.Cbeam3_Asbly_Dynamic(XBINPUT, NumNodes_tot, XBELEM, XBNODE,\
                                 PosIni, PsiIni, PosDefor, PsiDefor,\
                                 PosDotDef, PsiDotDef, PosDotDotDef, PsiDotDotDef,\
                                 Force, tmpVrel, 0*tmpVrel,\
                                 NumDof, Settings.DimMat,\
                                 ms, MssFull, Msr,\
                                 cs, CssFull, Csr,\
                                 ks, KssFull, fs, FstrucFull,\
                                 Qstruc, XBOPTS, Cao)
            
            BeamLib.f_fem_m2v(ct.byref(NumNodes_tot),\
                              ct.byref(ct.c_int(6)),\
                              Force.ctypes.data_as(ct.POINTER(ct.c_double)),\
                              ct.byref(NumDof),\
                              Force_Dof.ctypes.data_as(ct.POINTER(ct.c_double)),\
                              XBNODE.Vdof.ctypes.data_as(ct.POINTER(ct.c_int)) )
                    
            
            #Update matrices for rigid-body dynamic analysis
            BeamLib.Xbeam_Asbly_Dynamic(XBINPUT, NumNodes_tot, XBELEM, XBNODE,\
                                 PosIni, PsiIni, PosDefor, PsiDefor,\
                                 PosDotDef, PsiDotDef, PosDotDotDef, PsiDotDotDef,\
                                 tmpVrel, 0*tmpVrel, tmpQuat,\
                                 NumDof, Settings.DimMat,\
                                 mr, MrsFull, Mrr,\
                                 cr, CrsFull, Crr, Cqr, Cqq,\
                                 kr, KrsFull, fs, FrigidFull,\
                                 Qrigid, XBOPTS, Cao)
    
            BeamLib.f_fem_m2v_nofilter(ct.byref(NumNodes_tot),\
                                       ct.byref(ct.c_int(6)),\
                                       Force.ctypes.data_as(ct.POINTER(ct.c_double)),\
                                       ct.byref(ct.c_int(NumDof.value+6)),\
                                       Force_All.ctypes.data_as(ct.POINTER(ct.c_double)) )
        
        
            #Residual at first iteration
            if(Iter == 1):
                Res0_Qglobal = max(max(abs(Qsys)),1)
                Res0_DeltaX  = max(max(abs(DQ)),1)
              
            
            #Assemble discrete system matrices with linearised quaternion equations          
            Msys[:NumDof.value,:NumDof.value] = MssFull.copy('F')
            Msys[:NumDof.value,NumDof.value:NumDof.value+6] = Msr.copy('F')
            Msys[NumDof.value:NumDof.value+6,:NumDof.value] = MrsFull.copy('F')
            Msys[NumDof.value:NumDof.value+6,NumDof.value:NumDof.value+6] = Mrr.copy('F')
            Msys[NumDof.value+6:,NumDof.value+6:] = Unit4.copy('F')
            
            Csys[:NumDof.value,:NumDof.value] = CssFull.copy('F')
            Csys[:NumDof.value,NumDof.value:NumDof.value+6] = Csr.copy('F')
            Csys[NumDof.value:NumDof.value+6,:NumDof.value] = CrsFull.copy('F')
            Csys[NumDof.value:NumDof.value+6,NumDof.value:NumDof.value+6] = Crr.copy('F')
            
            Csys[NumDof.value+6:,NumDof.value:NumDof.value+6] = Cqr.copy('F')
            Csys[NumDof.value+6:,NumDof.value+6:] = Cqq.copy('F')
            
            Ksys[:NumDof.value,:NumDof.value] = KssFull.copy('F')
            Ksys[NumDof.value:NumDof.value+6,:NumDof.value] = KrsFull.copy('F')
            
          
#             #Separate assembly of follower and dead loads   
#             tmpForceTime=ForceTime[iStep+1].copy('F') 
#             tmpQforces,Dummy,tmpQrigid = xbl.LoadAssembly(XBINPUT, XBELEM, XBNODE, XBOPTS, NumDof, \
#                                             PosIni, PsiIni, PosDefor, PsiDefor, \
#                                             (XBINPUT.ForceStatic_foll + XBINPUT.ForceDyn_foll*tmpForceTime), \
#                                             (XBINPUT.ForceStatic_dead + XBINPUT.ForceDyn_dead*tmpForceTime), \
#                                             Cao,1)
#                                    
#             Qstruc -= tmpQforces      
#             Qrigid -= tmpQrigid
    
            
            #Compute residual to solve update vector
            Qstruc += -np.dot(FstrucFull, Force_Dof)
            Qrigid += -np.dot(FrigidFull, Force_All)
            
            Qsys[:NumDof.value] = Qstruc
            Qsys[NumDof.value:NumDof.value+6] = Qrigid
            Qsys[NumDof.value+6:] = np.dot(Cqq,dQdt[NumDof.value+6:])
            
            Qsys += np.dot(Msys,dQddt)

                
            #Calculate system matrix for update calculation
            Asys = Ksys + \
                      Csys*gamma/(beta*dt) + \
                      Msys/(beta*dt**2)
                      
            
            #Compute correction
            DQ[:] = np.dot(np.linalg.inv(Asys), -Qsys)

            Q += DQ
            dQdt += DQ*gamma/(beta*dt)
            dQddt += DQ/(beta*dt**2)
            
            
            #Update convergence criteria
            if XBOPTS.PrintInfo.value==True:                 
                sys.stdout.write('%-10.4e ' %(max(abs(Qsys))))
            
            Res_Qglobal = max(abs(Qsys))
            Res_DeltaX  = max(abs(DQ))
            
            ResLog10 = max(Res_Qglobal/Res0_Qglobal,Res_DeltaX/Res0_DeltaX)
            
            if XBOPTS.PrintInfo.value==True:
                sys.stdout.write('%-10.4e %8.4f\n' %(max(abs(DQ)),ResLog10))

        # END Netwon-Raphson.
                
                
        #update to converged nodal displacements and velocities
        X=Q[:NumDof.value].copy('F') 
        dXdt=dQdt[:NumDof.value].copy('F'); 
        BeamLib.Cbeam3_Solv_State2Disp(XBINPUT, NumNodes_tot, XBELEM, XBNODE,
                           PosIni, PsiIni, NumDof, X, dXdt,\
                           PosDefor, PsiDefor, PosDotDef, PsiDotDef)
        
        PosPsiTime[iStep+1,:3] = PosDefor[-1,:]
        PosPsiTime[iStep+1,3:] = PsiDefor[-1,XBELEM.NumNodes[-1]-1,:]
        
        #Position of all grid points in global FoR
        i1 = (iStep+1)*NumNodes_tot.value
        i2 = (iStep+2)*NumNodes_tot.value
        DynOut[i1:i2,:] = PosDefor
        
        #Export rigid-body velocities/accelerations
        if XBOPTS.OutInaframe.value==True:
            Vrel[iStep,:] = dQdt[NumDof.value:NumDof.value+6].copy('F')
            VrelDot[iStep,:] = dQddt[NumDof.value:NumDof.value+6].copy('F')
        else:
            Quat = dQdt[NumDof.value+6:].copy('F')
            Quat = Quat/np.linalg.norm(Quat)
            Cao  = xbl.Rot(Quat)
            ACoa[:3,:3] = np.transpose(Cao)
            ACoa[3:,3:] = np.transpose(Cao)
            
            Vrel[iStep,:] = np.dot(ACoa,dQdt[NumDof.value:NumDof.value+6].copy('F'))
            VrelDot[iStep,:] = np.dot(ACoa,dQddt[NumDof.value:NumDof.value+6].copy('F'))
            
        # Write selected outputs
        # tidy this away using function.
        if 'writeDict' in kwords and Settings.WriteOut == True:
            locForces = None # Stops recalculation of forces
            fp.write("{:<14,e}".format(Time[iStep+1]))
            for myStr in writeDict.keys():
                if re.search(r'^R_.',myStr):
                    if re.search(r'^R_._.', myStr):
                        index = int(myStr[4])
                    elif re.search(r'root', myStr):
                        index = 0
                    elif re.search(r'tip', myStr):
                        index = -1
                    else:
                        raise IOError("Node index not recognised.")
                    
                    if myStr[2] == 'x':
                        component = 0
                    elif myStr[2] == 'y':
                        component = 1
                    elif myStr[2] == 'z':
                        component = 2
                    else:
                        raise IOError("Displacement component not recognised.")
                    
                    fp.write("{:<14,e}".format(PosDefor[index,component]))
                    
                elif re.search(r'^M_.',myStr):
                    if re.search(r'^M_._.', myStr):
                        index = int(myStr[4])
                    elif re.search(r'root', myStr):
                        index = 0
                    elif re.search(r'tip', myStr):
                        index = -1
                    else:
                        raise IOError("Node index not recognised.")
                    
                    if myStr[2] == 'x':
                        component = 0
                    elif myStr[2] == 'y':
                        component = 1
                    elif myStr[2] == 'z':
                        component = 2
                    else:
                        raise IOError("Moment component not recognised.")
                    
                    if locForces == None:
                        locForces = BeamIO.localElasticForces(PosDefor,
                                                              PsiDefor,
                                                              PosIni,
                                                              PsiIni,
                                                              XBELEM,
                                                              [index])
                    
                    fp.write("{:<14,e}".format(locForces[0,3+component]))
                else:
                    raise IOError("writeDict key not recognised.")
            # END for myStr
            fp.write("\n")
            fp.flush()
                    
        # 'Rollup' due to external velocities. TODO: Must add gusts here!
        ZetaStar[:,:] = ZetaStar[:,:] + VMINPUT.U_infty*dt
    
    # END Time loop
    
    # Write _dyn file.
    ofile = Settings.OutputDir + Settings.OutputFileRoot + '_SOL912_dyn.dat'
    fp = open(ofile,'w')
    BeamIO.Write_dyn_File(fp, Time, PosPsiTime)
    fp.close()
    
    #Write _shape file
    ofile = Settings.OutputDir + Settings.OutputFileRoot + '_SOL912_shape.dat'
    fp = open(ofile,'w')
    BeamIO.Write_shape_File(fp, len(Time), NumNodes_tot.value, Time, DynOut)
    fp.close()
    
    #Write rigid-body velocities
    ofile = Settings.OutputDir + Settings.OutputFileRoot + '_SOL912_rigid.dat'
    fp = open(ofile,'w')
    BeamIO.Write_rigid_File(fp, Time, Vrel, VrelDot)
    fp.close()
    
    # Close output file if it exists.
    if 'writeDict' in kwords and Settings.WriteOut == True:
        fp.close()
    
    # Close Tecplot ascii FileObject.
    if Settings.PlotTec==True:
        PostProcess.CloseAeroTecFile(FileObject)
    
    if XBOPTS.PrintInfo.value==True:
        sys.stdout.write(' ... done\n')
        
    # For interactive analysis at end of simulation set breakpoint.
    pass
Example #4
0
def zetaDotSubMat(r, psi, rDot, psiDot, v_a, omega_a, psi_G2a, xi, xiDot, iLin, jLin, nBeam, nSection):
    """@brief create submatrix(3,N_states) for the linearized velocity at any
    point on the surface, zeta, defined by beam DoFs R and Psi, and the
    cross-sectional coordinate xi.
    @param r Position vector on beam, defined in the a-frame.
    @param psi CRV of orientation of beam cross-section.
    @param rDot Velocity of beam at r, defined in the a-frame.
    @param psiDot Time rate of change of psi.
    @param v_a Velocity of a-frame, defined in the a-frame.
    @param omega_a Angular vel of a-frame, defined in a-frame.
    @param psi_G2a CRV of orientation of a-frame relative to earth.
    @param xi Cross-sectional coordinate, defined in B-frame.
    @param xiDot Time rate of change of xi, defined in B-frame.
    @param iLin index of xi on the sectional DoF used for the linear analysis.
    @param jLin index of R on the beam DoF used for the linear analysis.
    @param nBeam Number of points on the beam axis.
    @param nSection Number of points in the section definition.
    
    @returns subMat Linear transformation from FBD + sectional DoFs to surface
              velocity (G-frame) at point r_0 + C_Ga*(R + C(Psi)*xi).
    @details This routine calculates the matrix corresponding to any point
              on the beam axis and corresponding cross-section.
              Interpolation of the primary beam and section DoFs may pre-
              or post- multiply this matrix in an aeroelastic analysis.
    """
    # Total states on RHS of matrix
    nStates = 2 * 6 * nBeam + 9 + 2 * 3 * nSection
    # Initialise subMat
    subMat = np.zeros((3, nStates))

    # term 1: \delta(C^{Ga}v_a)
    cGa = xbl.Psi2TransMat(psi_G2a)
    skewVa = xbl.Skew(v_a)
    tangGa = xbl.Tangential(psi_G2a)
    # term for \delta \psi_{Ga}
    subMat[:, 2 * 6 * nBeam + 6 : 2 * 6 * nBeam + 9 : 1] += -np.dot(cGa, np.dot(skewVa, tangGa))
    # term for \delta v_a
    subMat[:, 2 * 6 * nBeam : 2 * 6 * nBeam + 3 : 1] += cGa

    # term 2: \delta(C^{Ga} \tilde{\Omega_a_a} R_a)
    skewOmAa = xbl.Skew(omega_a)
    skewRa = xbl.Skew(r)
    # term for \delta \psi_{Ga}
    subMat[:, 2 * 6 * nBeam + 6 : 2 * 6 * nBeam + 9 : 1] += -np.dot(cGa, np.dot(xbl.Skew(np.dot(skewOmAa, r)), tangGa))
    # term for \delta \Omega_a_a
    subMat[:, 2 * 6 * nBeam + 3 : 2 * 6 * nBeam + 6 : 1] += -np.dot(cGa, skewRa)
    # term for \delta R_a
    subMat[:, 6 * jLin : 6 * jLin + 3 : 1] += np.dot(cGa, skewOmAa)

    # term 3: \delta(C^{Ga}\tilde{\Omega_a_a}C^{aB}\xi_B)
    cAb = xbl.Psi2TransMat(psi)
    skewXi = xbl.Skew(xi)
    tangAb = xbl.Tangential(psi)
    # term for \delta \psi_{Ga}
    subMat[:, 2 * 6 * nBeam + 6 : 2 * 6 * nBeam + 9 : 1] += -np.dot(
        cGa, np.dot(xbl.Skew(np.dot(skewOmAa, np.dot(cAb, xi))), tangGa)
    )
    # term for \delta \Omega_a_a
    subMat[:, 2 * 6 * nBeam + 3 : 2 * 6 * nBeam + 6 : 1] += -np.dot(cGa, xbl.Skew(np.dot(cAb, xi)))
    # term for \delta \psi_{aB}
    subMat[:, 6 * jLin + 3 : 6 * jLin + 6 : 1] += -np.dot(cGa, np.dot(skewOmAa, np.dot(cAb, np.dot(skewXi, tangAb))))
    # term for \delta xi_B
    subMat[:, 2 * 6 * nBeam + 9 + 3 * iLin : 2 * 6 * nBeam + 9 + 3 * iLin + 3 : 1] += np.dot(cGa, np.dot(skewOmAa, cAb))

    # term 4: \delta(C^{Ga}\dot{R}_a)
    skewRdot = xbl.Skew(rDot)
    # term for \delta \psi_{Ga}
    subMat[:, 2 * 6 * nBeam + 6 : 2 * 6 * nBeam + 9 : 1] += -np.dot(cGa, np.dot(skewRdot, tangGa))
    # term for \delta \dot{R}_a
    subMat[:, 6 * nBeam + 6 * jLin : 6 * nBeam + 6 * jLin + 3 : 1] += cGa

    # term 5: \delta(C^{Ga}C^{aB}\dot{\xi}_B)
    skewXiDot = xbl.Skew(xiDot)
    # term for \delta \psi_{Ga}
    subMat[:, 2 * 6 * nBeam + 6 : 2 * 6 * nBeam + 9 : 1] += -np.dot(cGa, np.dot(xbl.Skew(np.dot(cAb, xiDot)), tangGa))
    # term for \delta \psi_{aB}
    subMat[:, 6 * jLin + 3 : 6 * jLin + 6 : 1] += -np.dot(cGa, np.dot(cAb, np.dot(skewXiDot, tangAb)))
    # term for \delta \dot{\xi}_B
    subMat[:, 2 * 6 * nBeam + 9 + 3 * nSection + 3 * iLin : 2 * 6 * nBeam + 9 + 3 * nSection + 3 * iLin + 3] += np.dot(
        cGa, cAb
    )

    # term 6: \delta(C^{Ga}C^{aB}\tilde{T(\psi_{aB})\dot{\psi}_{aB}\xi_B)
    # Note: eqn. rearranged to \delta(-C^{Ga}C^{aB}\tilde{\xi}T(\psi_{aB})\dot{\psi}_{aB})
    # term for  \delta \psi_{Ga}
    subMat[:, 2 * 6 * nBeam + 6 : 2 * 6 * nBeam + 9 : 1] += np.dot(
        cGa, np.dot(xbl.Skew(np.dot(cAb, np.dot(skewXi, np.dot(tangAb, psiDot)))), tangGa)
    )
    # term for \delta \psi_{aB}
    subMat[:, 6 * jLin + 3 : 6 * jLin + 6 : 1] += np.dot(
        cGa, np.dot(cAb, np.dot(xbl.Skew(np.dot(skewXi, np.dot(tangAb, psiDot))), tangAb))
    )
    # term for \delta \xi_B
    subMat[:, 2 * 6 * nBeam + 9 + 3 * iLin : 2 * 6 * nBeam + 9 + 3 * iLin + 3 : 1] += np.dot(
        cGa, np.dot(cAb, xbl.Skew(np.dot(tangAb, psiDot)))
    )
    # term for \delta T(\psi_{aB})\dot{\psi_{aB}}
    subMat[:, 6 * jLin + 3 : 6 * jLin + 6 : 1] += -np.dot(cGa, np.dot(cAb, np.dot(skewXi, xbl.a1(psi, psiDot))))
    # term for \delta \dot{\psi}_{aB}
    subMat[:, 6 * nBeam + 6 * jLin + 3 : 6 * nBeam + 6 * jLin + 6 : 1] += -np.dot(
        cGa, np.dot(cAb, np.dot(skewXi, tangAb))
    )
    return subMat
def Solve_Py(XBINPUT,XBOPTS,VMOPTS,VMINPUT,AELAOPTS,**kwords):
    """@brief Nonlinear dynamic solver using Python to solve aeroelastic
    equation.
    @details Assembly of structural matrices is carried out with 
    Fortran subroutines. Aerodynamics solved using PyAero\.UVLM.
    @warning test outstanding: test for maintaining static deflections in
    same conditions.
    TODO: Maintain static deflections in same conditions.
    @param XBINPUT Beam inputs (for initialization in Python).
    @param XBOPTS Beam solver options (for Fortran).
    @param VMOPTS UVLM solver options (for C/C++).
    @param VMINPUT UVLM solver inputs (for initialization in Python).
    @param VMUNST Unsteady input information for aero solver.
    @param AELAOPTS Options relevant to coupled aeroelastic simulations.
    
    kwords:
    @param writeDict OrderedDict of of outputs to write.
    @param mpcCont Instance of PyMPC.MPC class.
    """
        
    # Check correct solution code.
    assert XBOPTS.Solution.value == 312, ('NonlinearDynamic requested' +
                                          ' with wrong solution code')
    # Initialise static beam data.
    XBINPUT, XBOPTS, NumNodes_tot, XBELEM, PosIni, PsiIni, XBNODE, NumDof \
                = BeamInit.Static(XBINPUT,XBOPTS)
                
    # Calculate initial displacements.
    if AELAOPTS.ImpStart == False:
        XBOPTS.Solution.value = 112 # Modify options.
        VMOPTS.Steady = ct.c_bool(True)
        Rollup = VMOPTS.Rollup.value
        VMOPTS.Rollup.value = False
        # Solve Static Aeroelastic.
        PosDefor, PsiDefor, Zeta, ZetaStar, Gamma, GammaStar, Force = \
                    Static.Solve_Py(XBINPUT, XBOPTS, VMOPTS, VMINPUT, AELAOPTS)
        XBOPTS.Solution.value = 312 # Reset options.
        VMOPTS.Steady = ct.c_bool(False)
        VMOPTS.Rollup.value = Rollup
    elif AELAOPTS.ImpStart == True:
        PosDefor = PosIni.copy(order='F')
        PsiDefor = PsiIni.copy(order='F')
        Force = np.zeros((XBINPUT.NumNodesTot,6),ct.c_double,'F')
        
    # Write deformed configuration to file. TODO: tidy this away inside function.
    ofile = Settings.OutputDir + Settings.OutputFileRoot + '_SOL312_def.dat'
    if XBOPTS.PrintInfo==True:
        sys.stdout.write('Writing file %s ... ' %(ofile))
    fp = open(ofile,'w')
    fp.write('TITLE="Non-linear static solution: deformed geometry"\n')
    fp.write('VARIABLES="iElem" "iNode" "Px" "Py" "Pz" "Rx" "Ry" "Rz"\n')
    fp.close()
    if XBOPTS.PrintInfo==True:
        sys.stdout.write('done\n')
    WriteMode = 'a'
    # Write
    BeamIO.OutputElems(XBINPUT.NumElems, NumNodes_tot.value, XBELEM,
                       PosDefor, PsiDefor, ofile, WriteMode)
    
    # Initialise structural variables for dynamic analysis.
    Time, NumSteps, ForceTime, ForcedVel, ForcedVelDot,\
    PosDotDef, PsiDotDef,\
    OutGrids, PosPsiTime, VelocTime, DynOut\
        = BeamInit.Dynamic(XBINPUT,XBOPTS)
    # Delete unused variables.
    del ForceTime, OutGrids, VelocTime
        
    # Write _force file
#    ofile = Settings.OutputDir + Settings.OutputFileRoot + '_SOL312_force.dat'
#    fp = open(ofile,'w')
#    BeamIO.Write_force_File(fp, Time, ForceTime, ForcedVel, ForcedVelDot)
#    fp.close() 
    # Write _vel file   
    #TODO: write _vel file
    # Write .mrb file.
    #TODO: write .mrb file
    
    if XBOPTS.PrintInfo.value==True:
        sys.stdout.write('Solve nonlinear dynamic case in Python ... \n')
    
    # Initialise structural system tensors.
    MglobalFull = np.zeros((NumDof.value,NumDof.value), ct.c_double, 'F')
    CglobalFull = np.zeros((NumDof.value,NumDof.value), ct.c_double, 'F') 
    KglobalFull = np.zeros((NumDof.value,NumDof.value), ct.c_double, 'F') 
    FglobalFull = np.zeros((NumDof.value,NumDof.value), ct.c_double, 'F')
    Asys = np.zeros((NumDof.value,NumDof.value), ct.c_double, 'F')
    
    ms = ct.c_int()
    cs = ct.c_int()
    ks = ct.c_int()
    fs = ct.c_int()
    
    Mvel = np.zeros((NumDof.value,6), ct.c_double, 'F')
    Cvel = np.zeros((NumDof.value,6), ct.c_double, 'F')
    
#     X0    = np.zeros(NumDof.value, ct.c_double, 'F')
    X     = np.zeros(NumDof.value, ct.c_double, 'F')
    DX    = np.zeros(NumDof.value, ct.c_double, 'F')
    dXdt  = np.zeros(NumDof.value, ct.c_double, 'F')
    dXddt = np.zeros(NumDof.value, ct.c_double, 'F')
    Force_Dof = np.zeros(NumDof.value, ct.c_double, 'F')
    
    Qglobal = np.zeros(NumDof.value, ct.c_double, 'F')
    
    # Initialise rotation operators.
    Unit = np.zeros((3,3), ct.c_double, 'F')
    for i in range(3):
        Unit[i,i] = 1.0
    
    Unit4 = np.zeros((4,4), ct.c_double, 'F')
    for i in range(4):
        Unit4[i,i] = 1.0
        
    Cao = Unit.copy('F')
    Temp = Unit4.copy('F')
    
    Quat = np.zeros(4, ct.c_double, 'F')
    Quat[0] = 1.0
    
    # Extract initial displacements and velocities.
    BeamLib.Cbeam_Solv_Disp2State(NumNodes_tot, NumDof, XBINPUT, XBNODE,
                                  PosDefor, PsiDefor, PosDotDef, PsiDotDef,
                                  X, dXdt)
    
    # Approximate initial accelerations.
    PosDotDotDef = np.zeros((NumNodes_tot.value,3),ct.c_double,'F')
    PsiDotDotDef = np.zeros((XBINPUT.NumElems,Settings.MaxElNod,3),
                             ct.c_double, 'F')
    
    # Assemble matrices for dynamic analysis.
    BeamLib.Cbeam3_Asbly_Dynamic(XBINPUT, NumNodes_tot, XBELEM, XBNODE,
                                 PosIni, PsiIni, PosDefor, PsiDefor,
                                 PosDotDef, PsiDotDef, PosDotDotDef, PsiDotDotDef,
                                 Force, ForcedVel[0,:], ForcedVelDot[0,:],
                                 NumDof, Settings.DimMat,
                                 ms, MglobalFull, Mvel,
                                 cs, CglobalFull, Cvel,
                                 ks, KglobalFull, fs, FglobalFull,
                                 Qglobal, XBOPTS, Cao)
    
    # Get force vector for unconstrained nodes (Force_Dof).
    BeamLib.f_fem_m2v(ct.byref(NumNodes_tot),
                      ct.byref(ct.c_int(6)),
                      Force.ctypes.data_as(ct.POINTER(ct.c_double)),
                      ct.byref(NumDof),
                      Force_Dof.ctypes.data_as(ct.POINTER(ct.c_double)),
                      XBNODE.Vdof.ctypes.data_as(ct.POINTER(ct.c_int)) )
    
    # Get RHS at initial condition.
    Qglobal = Qglobal - np.dot(FglobalFull, Force_Dof)
    
    # Initial Accel.
    dXddt[:] = np.dot(np.linalg.inv(MglobalFull), -Qglobal)
    
    # Record position of all grid points in global FoR at initial time step.
    DynOut[0:NumNodes_tot.value,:] = PosDefor
    
    # Record state of the selected node in initial deformed configuration.
    PosPsiTime[0,:3] = PosDefor[-1,:]
    PosPsiTime[0,3:] = PsiDefor[-1,XBELEM.NumNodes[-1]-1,:]
    
    # Get gamma and beta for Newmark scheme.
    gamma = 0.5 + XBOPTS.NewmarkDamp.value
    beta = 0.25*pow((gamma + 0.5),2)
    
    # Initialize Aero       
    Section = InitSection(VMOPTS,VMINPUT,AELAOPTS.ElasticAxis)
    
    # Declare memory for Aero variables.
    ZetaDot = np.zeros((Section.shape[0],PosDefor.shape[0],3),ct.c_double,'C')
    K = VMOPTS.M.value*VMOPTS.N.value
    AIC = np.zeros((K,K),ct.c_double,'C')
    BIC = np.zeros((K,K),ct.c_double,'C')
    AeroForces = np.zeros((VMOPTS.M.value+1,VMOPTS.N.value+1,3),ct.c_double,'C')
    
    # Initialise A-frame location and orientation to be zero
    OriginA_G = np.zeros(3,ct.c_double,'C')
    PsiA_G = np.zeros(3,ct.c_double,'C')
    
    # Init external velocities.  
    Ufree = InitSteadyExternalVels(VMOPTS,VMINPUT)
    
    # Init uninit vars if an impulsive start is specified.
    if AELAOPTS.ImpStart == True:
        Zeta = np.zeros((Section.shape[0],PosDefor.shape[0],3),ct.c_double,'C')             
        Gamma = np.zeros((VMOPTS.M.value,VMOPTS.N.value),ct.c_double,'C')
        # Generate surface, wake and gamma matrices.
        CoincidentGrid(PosDefor, PsiDefor, Section, ForcedVel[0,:3], 
                       ForcedVel[0,3:], PosDotDef, PsiDotDef, XBINPUT,
                       Zeta, ZetaDot, OriginA_G, PsiA_G,
                       VMINPUT.ctrlSurf)
        # init wake grid and gamma matrix.
        ZetaStar, GammaStar = InitSteadyWake(VMOPTS,VMINPUT,Zeta,ForcedVel[0,:3])
        
    # Init GammaDot
    GammaDot = np.zeros_like(Gamma, ct.c_double, 'C')
          
    # Define tecplot stuff
    if Settings.PlotTec==True:
        FileName = Settings.OutputDir + Settings.OutputFileRoot + 'AeroGrid.dat'
        Variables = ['X', 'Y', 'Z','Gamma']        
        FileObject = PostProcess.WriteAeroTecHeader(FileName, 
                                                    'Default',
                                                    Variables)
        # Plot results of static analysis
        PostProcess.WriteUVLMtoTec(FileObject,
                                   Zeta,
                                   ZetaStar,
                                   Gamma,
                                   GammaStar,
                                   TimeStep = 0,
                                   NumTimeSteps = XBOPTS.NumLoadSteps.value,
                                   Time = 0.0,
                                   Text = False)
    
    # Open output file for writing
    if 'writeDict' in kwords and Settings.WriteOut == True:
        fp = OpenOutFile(kwords['writeDict'], XBOPTS, Settings)
        
    # Write initial outputs to file.
    if 'writeDict' in kwords and Settings.WriteOut == True:
        WriteToOutFile(kwords['writeDict'],
                       fp,
                       Time[0],
                       PosDefor,
                       PsiDefor,
                       PosIni,
                       PsiIni,
                       XBELEM,
                       ctrlSurf,
                       kwords['mpcCont'])
    # END if write

    # Time loop.
    for iStep in range(NumSteps.value):
        
        if XBOPTS.PrintInfo.value==True:
            sys.stdout.write('Time: %-10.4e\n' %(Time[iStep+1]))
            sys.stdout.write('   SubIter DeltaF     DeltaX     ResLog10\n')
        
        dt = Time[iStep+1] - Time[iStep]
        
        # Set dt for aero force calcs.
        VMOPTS.DelTime = ct.c_double(dt)
        
        # Save Gamma at iStep.
        GammaSav = Gamma.copy(order = 'C')
        # Force at current time-step
        if iStep > 0 and AELAOPTS.Tight == False:
            
            # zero aero forces.
            AeroForces[:,:,:] = 0.0
            
            # Update CRV.
            PsiA_G = xbl.quat2psi(Quat) # CRV at iStep
            
            # Update origin.
            OriginA_G[:] = OriginA_G[:] + ForcedVel[iStep-1,:3]*dt
            
            # Update control surface deflection.
            if VMINPUT.ctrlSurf != None:
                if 'mpcCont' in kwords and kwords['mpcCont'] != None:
                    uOpt = kwords['mpcCont'].getUopt(
                            getState(Gamma,GammaStar,GammaDot,X,dXdt) )
                    VMINPUT.ctrlSurf.update(Time[iStep],uOpt[0,0])
                else:
                    VMINPUT.ctrlSurf.update(Time[iStep])
            
            # Generate surface grid.
            CoincidentGrid(PosDefor, PsiDefor, Section, ForcedVel[iStep,:3], 
                           ForcedVel[iStep,3:], PosDotDef, PsiDotDef, XBINPUT,
                           Zeta, ZetaDot, OriginA_G, PsiA_G,
                           VMINPUT.ctrlSurf)
            
            # Update wake geom       
            #'roll' data.
            ZetaStar = np.roll(ZetaStar,1,axis = 0)
            GammaStar = np.roll(GammaStar,1,axis = 0)
            #overwrite grid points with TE.
            ZetaStar[0,:] = Zeta[VMOPTS.M.value,:]
            # overwrite Gamma with TE value from previous timestep.
            GammaStar[0,:] = Gamma[VMOPTS.M.value-1,:]
            
            # Apply gust velocity.
            if VMINPUT.gust != None:
                Utot = Ufree + VMINPUT.gust.Vels(Zeta)
            else:
                Utot = Ufree
            
            # Solve for AeroForces.
            UVLMLib.Cpp_Solver_VLM(Zeta, ZetaDot, Utot, ZetaStar, VMOPTS, 
                           AeroForces, Gamma, GammaStar, AIC, BIC)
            
            # Get GammaDot.
            GammaDot[:] = Gamma[:] - GammaSav[:]
            
            # Apply density scaling.
            AeroForces[:,:,:] = AELAOPTS.AirDensity*AeroForces[:,:,:]
            
            if Settings.PlotTec==True:
                PostProcess.WriteUVLMtoTec(FileObject,
                                           Zeta - OriginA_G[:],
                                           ZetaStar - OriginA_G[:],
                                           Gamma,
                                           GammaStar,
                                           TimeStep = iStep,
                                           NumTimeSteps = XBOPTS.NumLoadSteps.value,
                                           Time = Time[iStep],
                                           Text = False)

            # map AeroForces to beam.
            CoincidentGridForce(XBINPUT, PsiDefor, Section, AeroForces,
                                Force)
            
            # Add gravity loads.
            AddGravityLoads(Force, XBINPUT, XBELEM, AELAOPTS,
                            PsiDefor, VMINPUT.c)
        #END if iStep > 0
        
        # Quaternion update for orientation.
        Temp = np.linalg.inv(Unit4 + 0.25*xbl.QuadSkew(ForcedVel[iStep+1,3:])*dt)
        Quat = np.dot(Temp, np.dot(Unit4 - 0.25*xbl.QuadSkew(ForcedVel[iStep,3:])*dt, Quat))
        Quat = Quat/np.linalg.norm(Quat)
        Cao  = xbl.Rot(Quat) # transformation matrix at iStep+1
        
        if AELAOPTS.Tight == True:
            # CRV at iStep+1
            PsiA_G = xbl.quat2psi(Quat)
            # Origin at iStep+1
            OriginA_G[:] = OriginA_G[:] + ForcedVel[iStep,:3]*dt
            
            GammaSav = Gamma.copy(order = 'C')
            
        # Predictor step.
        X        = X + dt*dXdt + (0.5-beta)*dXddt*pow(dt,2.0)
        dXdt     = dXdt + (1.0-gamma)*dXddt*dt
        dXddt[:] = 0.0
        
        # Reset convergence parameters.
        Iter = 0
        ResLog10 = 0.0
        
        # Newton-Raphson loop.        
        while ( (ResLog10 > np.log10(XBOPTS.MinDelta.value)) 
                & (Iter < XBOPTS.MaxIterations.value) ):
            
            # set tensors to zero.
            Qglobal[:] = 0.0 
            Mvel[:,:] = 0.0
            Cvel[:,:] = 0.0
            MglobalFull[:,:] = 0.0
            CglobalFull[:,:] = 0.0
            KglobalFull[:,:] = 0.0
            FglobalFull[:,:] = 0.0
            
            # Update counter.
            Iter += 1
            
            if XBOPTS.PrintInfo.value==True:
                sys.stdout.write('   %-7d ' %(Iter))
            
            # nodal diplacements and velocities from state vector.
            BeamLib.Cbeam3_Solv_State2Disp(XBINPUT,
                                           NumNodes_tot,
                                           XBELEM,
                                           XBNODE,
                                           PosIni,
                                           PsiIni,
                                           NumDof,
                                           X,
                                           dXdt,
                                           PosDefor,
                                           PsiDefor,
                                           PosDotDef,
                                           PsiDotDef)
            
            # if tightly coupled is on then get new aeroforces.
            if AELAOPTS.Tight == True:
                # zero aero forces.
                AeroForces[:,:,:] = 0.0
                
                # Set gamma at t-1 to saved solution.
                Gamma[:,:] = GammaSav[:,:]
                # get new grid.
                # The rigid-body DoFs (OriginA_G,PsiA_G,ForcedVel) at time step
                # i+1 are used to converge the aeroelastic equations.
                CoincidentGrid(PosDefor, PsiDefor, Section, ForcedVel[iStep+1,:3], 
                               ForcedVel[iStep+1,3:], PosDotDef, PsiDotDef, XBINPUT,
                               Zeta, ZetaDot, OriginA_G, PsiA_G,
                               VMINPUT.ctrlSurf)
                
                # close wake.
                ZetaStar[0,:] = Zeta[VMOPTS.M.value,:]
                
                # save pereference and turn off rollup.
                Rollup = VMOPTS.Rollup.value
                VMOPTS.Rollup.value = False
                
                # Solve for AeroForces.
                UVLMLib.Cpp_Solver_VLM(Zeta, ZetaDot, Ufree, ZetaStar, VMOPTS, 
                                       AeroForces, Gamma, GammaStar, AIC, BIC)
                
                # turn rollup back to original preference
                VMOPTS.Rollup.value = Rollup
                
                # apply density scaling.
                AeroForces[:,:,:] = AELAOPTS.AirDensity*AeroForces[:,:,:]
                
                # beam forces.
                CoincidentGridForce(XBINPUT, PsiDefor, Section, AeroForces,
                                    Force)
                
                # Add gravity loads.
                AddGravityLoads(Force,XBINPUT,XBELEM,AELAOPTS,
                                PsiDefor,VMINPUT.c)
            
            #END if Tight
            
            ForcedVelLoc = ForcedVel[iStep+1,:].copy('F')
            ForcedVelDotLoc = ForcedVelDot[iStep+1,:].copy('F')
            
            # Update matrices.
            BeamLib.Cbeam3_Asbly_Dynamic(XBINPUT, NumNodes_tot, XBELEM, XBNODE,
                         PosIni, PsiIni, PosDefor, PsiDefor,
                         PosDotDef, PsiDotDef, PosDotDotDef, PsiDotDotDef,
                         Force, ForcedVelLoc, ForcedVelDotLoc,
                         NumDof, Settings.DimMat,
                         ms, MglobalFull, Mvel,
                         cs, CglobalFull, Cvel,
                         ks, KglobalFull, fs, FglobalFull,
                         Qglobal, XBOPTS, Cao)
            
            
            # Get force vector for unconstrained nodes (Force_Dof).
            BeamLib.f_fem_m2v(ct.byref(NumNodes_tot),
                              ct.byref(ct.c_int(6)),
                              Force.ctypes.data_as(ct.POINTER(ct.c_double)),
                              ct.byref(NumDof),
                              Force_Dof.ctypes.data_as(ct.POINTER(ct.c_double)),
                              XBNODE.Vdof.ctypes.data_as(ct.POINTER(ct.c_int)) )
            
            # Solve for update vector.
            # Residual.
            Qglobal = Qglobal +  np.dot(MglobalFull, dXddt) \
                              + np.dot(Mvel,ForcedVelDotLoc) \
                              - np.dot(FglobalFull, Force_Dof)
                              
            if XBOPTS.PrintInfo.value==True:                 
                sys.stdout.write('%-10.4e ' %(max(abs(Qglobal))))

            
            # Calculate system matrix for update calculation.
            Asys = KglobalFull \
                    + CglobalFull*gamma/(beta*dt) \
                    + MglobalFull/(beta*pow(dt,2.0))
            
            # Solve for update.
            DX[:] = np.dot(np.linalg.inv(Asys), -Qglobal)
            
            # Corrector step.
            X = X + DX
            dXdt = dXdt + DX*gamma/(beta*dt)
            dXddt = dXddt + DX/(beta*pow(dt,2.0))
            
            # Residual at first iteration.
            if(Iter == 1):
                Res0_Qglobal = max(abs(Qglobal)) + 1.e-16
                Res0_DeltaX  = max(abs(DX)) + 1.e-16
            
            # Update residual and compute log10.
            Res_Qglobal = max(abs(Qglobal)) + 1.e-16
            Res_DeltaX  = max(abs(DX)) + 1.e-16
            ResLog10 = max([np.log10(Res_Qglobal/Res0_Qglobal),
                            np.log10(Res_DeltaX/Res0_DeltaX)])
            
            if XBOPTS.PrintInfo.value==True:
                sys.stdout.write('%-10.4e %8.4f\n' %(max(abs(DX)),ResLog10))
            
            if ResLog10 > 2.0:
                print("Residual growing! Exit Newton-Raphson...")
                break
                
        # END Netwon-Raphson.
        
        if ResLog10 > 2.0:
                print("Residual growing! Exit time-loop...")
                debug = 'here'
                del debug
                break
        
        # Update to converged nodal displacements and velocities.
        BeamLib.Cbeam3_Solv_State2Disp(XBINPUT, NumNodes_tot, XBELEM, XBNODE,
                           PosIni, PsiIni, NumDof, X, dXdt,
                           PosDefor, PsiDefor, PosDotDef, PsiDotDef)
        
        PosPsiTime[iStep+1,:3] = PosDefor[-1,:]
        PosPsiTime[iStep+1,3:] = PsiDefor[-1,XBELEM.NumNodes[-1]-1,:]
        
        # Position of all grid points in global FoR.
        i1 = (iStep+1)*NumNodes_tot.value
        i2 = (iStep+2)*NumNodes_tot.value
        DynOut[i1:i2,:] = PosDefor
        
        # Write selected outputs
        if 'writeDict' in kwords and Settings.WriteOut == True:
            WriteToOutFile(writeDict,
                           fp,
                           Time[iStep+1],
                           PosDefor,
                           PsiDefor,
                           PosIni,
                           PsiIni,
                           XBELEM,
                           ctrlSurf,
                           kwords['mpcCont'])
        # END if write.
        
        ZetaStar[:,:] = ZetaStar[:,:] + VMINPUT.U_infty*dt
        if VMINPUT.gust != None:
            ZetaStar[:,:,:] = ZetaStar[:,:,:] + VMINPUT.gust.Vels(ZetaStar)*dt
    
    # END Time loop
    
    # Write _dyn file.
    ofile = Settings.OutputDir + Settings.OutputFileRoot + '_SOL312_dyn.dat'
    fp = open(ofile,'w')
    BeamIO.Write_dyn_File(fp, Time, PosPsiTime)
    fp.close()
    
#    "Write _shape file"
#    ofile = Settings.OutputDir + Settings.OutputFileRoot + '_SOL312_shape.dat'
#    fp = open(ofile,'w')
#    BeamIO.Write_shape_File(fp, len(Time), NumNodes_tot.value, Time, DynOut)
#    fp.close()
    
    # Close output file if it exists.
    if 'writeDict' in kwords and Settings.WriteOut == True:
        fp.close()
    
    # Close Tecplot ascii FileObject.
    if Settings.PlotTec==True:
        PostProcess.CloseAeroTecFile(FileObject)
    
    if XBOPTS.PrintInfo.value==True:
        sys.stdout.write(' ... done\n')
        
    # For interactive analysis at end of simulation set breakpoint.
    pass
def Solve_Py(XBINPUT,XBOPTS,VMOPTS,VMINPUT,AELAOPTS,**kwords):
    """
    @brief Nonlinear dynamic solver using Python to solve aeroelastic equation.
    @details Assembly of structural matrices is carried out with  Fortran subroutines. Aerodynamics 
             solved using PyAero\.UVLM.
    @param XBINPUT Beam inputs (for initialization in Python).
    @param XBOPTS Beam solver options (for Fortran).
    @param VMOPTS UVLM solver options (for C/C++).
    @param VMINPUT UVLM solver inputs (for initialization in Python).
    @param VMUNST Unsteady input information for aero solver.
    @param AELAOPTS Options relevant to coupled aeroelastic simulations.
    @param writeDict OrderedDict of 'name':tuple of outputs to write.
    
    @todo: Add list of variables with description
    
    """

    # Check correct solution code.
    assert XBOPTS.Solution.value == 912, ('NonlinearFlightDynamic requested' +
                                          ' with wrong solution code')
    # Check loads options
    assert XBOPTS.FollowerForceRig.value == True, ('For NonlinearFlightDynamic '
        'solution XBOPTS.FollowerForceRig = ct.c_bool(True) is required. \n'
        'Note that gravity is treated as a dead load by default.')


    # I/O options
    XBOUT=DerivedTypes.Xboutput()  
    SaveDict=Settings.SaveDict
    if 'SaveDict' in kwords: SaveDict=kwords['SaveDict']
    if SaveDict['Format']=='h5':
        Settings.WriteOut=False
        Settings.PlotTec=False
        OutList=[AELAOPTS, VMINPUT, VMOPTS, XBOPTS, XBINPUT, XBOUT]
        #if VMINPUT.ctrlSurf!=None:
        #    for cc in range(len(VMINPUT.ctrlSurf)):
        #        OutList.append(VMINPUT.ctrlSurf[cc])
        if SaveDict['SaveWake'] is True:
            dirwake=SaveDict['OutputDir']+'wake'+SaveDict['OutputFileRoot']+'/'
            os.system('mkdir -p %s' %dirwake)
            XBOUT.dirwake=dirwake  
            
    XBOUT.cputime.append(time.clock()) # time.processor_time more appropriate 
    XBOUT.ForceDofList=[]
    XBOUT.ForceRigidList=[]


    #------------------ Initial Displacement/Forces: ImpStart vs Static Solution
    
    # Initialise static beam data.
    XBINPUT, XBOPTS, NumNodes_tot, XBELEM, PosIni, PsiIni, XBNODE, NumDof \
                = BeamInit.Static(XBINPUT,XBOPTS)
    
    
    # Calculate initial displacements.
    if AELAOPTS.ImpStart == True:

        PosDefor = PosIni.copy(order='F')
        PsiDefor = PsiIni.copy(order='F')
        # Extract forces
        XBOUT.ForceStaticTotal = XBINPUT.ForceStatic.copy('F')
        AddGravityLoads(XBOUT.ForceStaticTotal,XBINPUT,XBELEM,
                        AELAOPTS=None, # allows defining inertial/elastic axis
                        PsiDefor=PsiDefor,
                        chord = 0.0, # used to define inertial/elastic axis
                        PsiA_G=XBINPUT.PsiA_G,
                        FollowerForceRig=True)

        ForceAero = 0.0*XBOUT.ForceStaticTotal.copy('C')

    else:
        XBOPTS.Solution.value = 112 # Modify options.
        VMOPTS.Steady = ct.c_bool(True)
        Rollup = VMOPTS.Rollup.value
        VMOPTS.Rollup.value = False

        if VMINPUT.ctrlSurf != None:
            # open-loop control
            for cc in range(len(VMINPUT.ctrlSurf)):
                VMINPUT.ctrlSurf[cc].update(0.0,iStep=0)
        # Solve Static Aeroelastic.
        # Note: output force includes gravity loads
        #PosDefor, PsiDefor, Zeta, ZetaStar, Gamma, GammaStar, Force = \
        #    Static.Solve_Py(XBINPUT, XBOPTS, VMOPTS, VMINPUT, AELAOPTS)
        XBSTA=Static.Solve_Py(XBINPUT, XBOPTS, VMOPTS, VMINPUT, AELAOPTS)
        # Extract forces
        XBOUT.ForceStaticTotal=XBSTA.ForceStaticTotal.copy(order='F') # gravity/aero/applied
        ForceAero = XBSTA.ForceAeroStatic.copy(order='C')
        # Define Pos/Psi as fortran array (crash otherwise)
        PosDefor=XBSTA.PosDeforStatic.copy(order='F')
        PsiDefor=XBSTA.PsiDeforStatic.copy(order='F')
        XBOUT.PosDeforStatic = XBSTA.PosDeforStatic
        XBOUT.PsiDeforStatic = XBSTA.PsiDeforStatic
        # Wake variables
        Zeta=XBSTA.ZetaStatic
        ZetaStar=XBSTA.ZetaStarStatic
        Gamma=XBSTA.GammaStatic
        GammaStar=XBSTA.GammaStarStatic
        del XBSTA

        XBOPTS.Solution.value = 912 # Reset options.
        VMOPTS.Steady = ct.c_bool(False)
        VMOPTS.Rollup.value = Rollup


    # Save output   
    if SaveDict['Format']=='dat': 
        write_SOL912_def(XBOPTS,XBINPUT,XBELEM,NumNodes_tot,PosDefor,PsiDefor,SaveDict)
    else: # HDF5
        XBOUT.drop( PosIni=PosIni, PsiIni=PsiIni,PosDeforStatic=PosDefor.copy(), 
                    PsiDeforStatic=PsiDefor.copy() )
        XBOUT.ForceAeroList.append( ForceAero )
        PyLibs.io.save.h5file(SaveDict['OutputDir'], 
                                     SaveDict['OutputFileRoot']+'.h5', *OutList)
    

    #------------------------------------------- Initialise Structural Variables
    
    # Build arrays for dynamic analysis
    #   1. initial accelerations  {Pos/Psi}DotDotDef approximated to zero
    #   2. {Pos/Psi}DotDotDef remain zero
    ( Time, NumSteps, ForceTime, Vrel, VrelDot, PosDotDef, PsiDotDef, 
      OutGrids, PosPsiTime, VelocTime, DynOut ) = BeamInit.Dynamic( XBINPUT, XBOPTS)
    del OutGrids, VelocTime
    PosDotDotDef = np.zeros((NumNodes_tot.value,3),ct.c_double,'F')
    PsiDotDotDef = np.zeros((XBINPUT.NumElems,Settings.MaxElNod,3),ct.c_double, 'F')
    

    # Allocate memory for GEBM tensors:
    #   1. all set to zero
    #   2. all arrays as ct_c_double with Fortran ordering
    ( MssFull, CssFull, KssFull, FstrucFull, Qstruc,
      MrsFull, CrsFull, KrsFull, FrigidFull, Qrigid,
      Mrr, Crr, Msr, Csr, 
      Force_Dof, Force_All,
      X, dXdt, Q, DQ, dQdt, dQddt,
      Msys, Csys, Ksys, Asys, Qsys ) = init_GEBM_tensors(NumDof)
    
    
    # Extract initial displacements (and velocities ?).
    #   1. also for non impulsive start, this step only populates X...
    BeamLib.Cbeam_Solv_Disp2State(NumNodes_tot, NumDof, XBINPUT, XBNODE,
                                 PosDefor, PsiDefor, PosDotDef, PsiDotDef,
                                  X, dXdt)
    

    # Assemble Tensors for Dynamic Solution
    #  1. extract variables used twice or more (Quat, currVel)
    #  2. update Force
    #  3. Populate state vectors (Q, dQdt)
    if XBINPUT.PsiA_G_dyn != None: PsiA_G = XBINPUT.PsiA_G_dyn.copy()
    else: PsiA_G = XBINPUT.PsiA_G.copy()
    Quat=xbl.psi2quat(PsiA_G)
    currVrel=Vrel[0,:].copy('F')   # also shared by aero initialisation
    
    # ForceStatic will include aero only in the non-impulsive case
    Force = XBOUT.ForceStaticTotal.copy('F') + XBINPUT.ForceDyn[0,:,:].copy('F')  

    Q[:NumDof.value]=X.copy('F')
    dQdt[:NumDof.value]=dXdt.copy('F')
    dQdt[NumDof.value:NumDof.value+6] = currVrel.copy('F')
    dQdt[NumDof.value+6:]= Quat.copy('F')
    
    (Msys, Csys, Ksys, Qsys) = assemble_GEBM_tensors( 
                                       0, # iStep
                                       NumDof, NumNodes_tot,
                                       XBELEM, XBNODE,
                                       XBINPUT, VMINPUT, XBOPTS, 
                                       AELAOPTS, VMOPTS,  
                                       currVrel,     
                                       PosIni, PsiIni,          
                                       PosDefor, PsiDefor,  
                                       PosDotDef, PsiDotDef,
                                       PosDotDotDef, PsiDotDotDef,
                                       Force,
                                       MssFull, CssFull, KssFull, FstrucFull,
                                       Msr, Csr,
                                       Force_Dof, Qstruc,
                                       MrsFull, CrsFull, KrsFull, FrigidFull,
                                       Mrr, Crr, Qrigid,
                                       Q, dQdt, dQddt, 
                                       Force_All,
                                       Msys, Csys, Ksys, Asys, Qsys )


    # I/O
    DynOut[0:NumNodes_tot.value,:] = PosDefor                # nodal position
    PosPsiTime[0,:3] = PosDefor[-1,:]                        # nodal position
    PosPsiTime[0,3:] = PsiDefor[-1,XBELEM.NumNodes[-1]-1,:]  # nodal CRV
    XBOUT.drop( Vrel=Vrel, VrelDot=VrelDot, ForceTime=ForceTime, 
                AsysListStart =[], AsysListEnd=[], 
                MsysList=[], CsysList=[], KsysList=[])
    XBOUT.QuatList.append(Quat.copy())
    XBOUT.CRVList.append( PsiA_G.copy())
    XBOUT.drop( Msys0 = Msys.copy(), Csys0 = Csys.copy(), 
                                       Ksys0 = Ksys.copy(), Qsys0 = Qsys.copy())
    

    #--------------------------------------------------------------------- Initialise UVLM variables
    
    # 1. Section properties
    Section = InitSection(VMOPTS,VMINPUT,AELAOPTS.ElasticAxis)
    
    # Declare memory for Aero variables.
    K = VMOPTS.M.value*VMOPTS.N.value
    ZetaDot = np.zeros((Section.shape[0],PosDefor.shape[0],3),ct.c_double,'C')
    AIC = np.zeros((K,K),ct.c_double,'C')
    BIC = np.zeros((K,K),ct.c_double,'C')
    AeroForces = np.zeros((VMOPTS.M.value+1,VMOPTS.N.value+1,3),ct.c_double,'C')
    
    # Initialise A-frame location and orientation to be zero.
    OriginA_a = np.zeros(3,ct.c_double,'C')
    #PsiA_G=XBINPUT.PsiA_G.copy()
    
    # Init external velocities.  
    Ufree = InitSteadyExternalVels(VMOPTS,VMINPUT)
    if AELAOPTS.ImpStart == True:
        Zeta = np.zeros((Section.shape[0],PosDefor.shape[0],3),ct.c_double,'C')             
        Gamma = np.zeros((VMOPTS.M.value,VMOPTS.N.value),ct.c_double,'C')
        # Generate surface, wake and gamma matrices.
        CoincidentGrid(PosDefor, PsiDefor, Section, currVrel[:3], 
                       currVrel[3:], PosDotDef, PsiDotDef, XBINPUT,
                       Zeta, ZetaDot, OriginA_a, PsiA_G,
                       VMINPUT.ctrlSurf)
        # init wake grid and gamma matrix.
        ZetaStar, GammaStar = InitSteadyWake(VMOPTS,VMINPUT,Zeta,currVrel[:3])
    
    # Define TecPlot stuff
    if Settings.PlotTec==True:
        FileObject=write_TecPlot( Zeta, ZetaStar, Gamma, GammaStar, NumSteps.value, 0, Time[0], 
                                  SaveDict)
    if 'writeDict' in kwords and Settings.WriteOut == True:
        fp= write_SOL912_out( Time[0], PosDefor, PsiDefor, PosIni, PsiIni, XBELEM, 
                              kwords['writeDict'], SaveDict)
            
    
    
    #------------------------------------------------------------------------------- Time-Loop Start
    
    if XBOPTS.PrintInfo.value==True:
        sys.stdout.write('Solve nonlinear dynamic case in Python ... \n')
    
    
    #Get gamma and beta for Newmark scheme
    gamma = 0.5 + XBOPTS.NewmarkDamp.value
    beta = 0.25*(gamma + 0.5)**2 
    
    
    # Initial Accelerations:
    #   1. These can be non-zero (e.g Impulsive start or unbalanced start: in these cases, the 
    # structure has an initially undeformed/statically-deformed configuration, with zero velocities
    # but accelerations are non-zero).
    #   2. lagged solution compensates for Msys mal-conditioned
    if XBOPTS.RigidDynamics is False and AELAOPTS.ImpStart is False:
        lagsol=True
        if lagsol==True: dQddt[:] = lagsolver(Msys,-Qsys,MaxIter=1)
        else: dQddt[:] = np.linalg.solve(Msys,-Qsys)
    else:
        # solve only rigid body dynamics
        dQddt[:NumDof.value]=0.0
        dQddt[NumDof.value:]=np.linalg.solve( Msys[NumDof.value:,NumDof.value:],
                                             -Qsys[NumDof.value:] ) 
    XBOUT.dQddt0=dQddt.copy()


    for iStep in range(1,NumSteps.value+1): 
        # Note len(Time)=NumSteps+1
        
        if XBOPTS.PrintInfo.value==True:
            sys.stdout.write('Time: %-10.4e\n' %(Time[iStep]))
            sys.stdout.write('   SubIter DeltaF     DeltaX     ResLog10\n')
        
        #calculate dt
        dt = Time[iStep]-Time[iStep-1]
        VMOPTS.DelTime = ct.c_double(dt)
        
        #Predictor step
        Q    += dt*dQdt + (0.5-beta)*dQddt*np.power(dt,2.0)
        dQdt += (1.0-gamma)*dQddt*dt
        
        ### Corrector
        #  1. assume initial guess for the acceleration dQddt(iStep) = dQddt(iStep-1)
        Q += beta*dQddt*dt**2
        dQdt += gamma*dQddt*dt
        
        #nodal displacements and velocities from state vector
        X=Q[:NumDof.value].copy('F') 
        dXdt=dQdt[:NumDof.value].copy('F'); 
        BeamLib.Cbeam3_Solv_State2Disp(XBINPUT,NumNodes_tot,XBELEM,XBNODE,
                                       PosIni,PsiIni,NumDof,X,dXdt,
                                       PosDefor,PsiDefor,PosDotDef,PsiDotDef)
        
        #Reset convergence parameters
        Iter = 0
        ResLog10 = 1.0
        AELAMinRes=0.0
        
        # Residual at previous time-step
        # warning: the convergence check is not well implemented: these variables are going to be 
        #  equal to 1 always.
        if XBOPTS.RigidDynamics==False: iicheck=[ii for ii in range(NumDof.value+10)]
        else: iicheck=[ii for ii in range(NumDof.value, NumDof.value+10)]
        Res0_Qglobal = max(max(abs(Qsys[iicheck])),1)
        Res0_DeltaX  = max(max(abs(DQ[iicheck])),1)
        
        #---------------------------------------------------------------------------- Newton-Raphson 
        while ( (ResLog10 > XBOPTS.MinDelta.value) & (Iter < XBOPTS.MaxIterations.value) ):
 
            # Unpack state variables
            Vrel[iStep,:]    = dQdt[NumDof.value:NumDof.value+6].copy('F')
            VrelDot[iStep,:] = dQddt[NumDof.value:NumDof.value+6].copy('F')
            Quat = dQdt[NumDof.value+6:].copy('F')
            Quat = Quat/np.linalg.norm(Quat)
            X=Q[:NumDof.value].copy('F') 
            dXdt=dQdt[:NumDof.value].copy('F')
            Cao  = xbl.Rot(Quat)
            PsiA_G = xbl.quat2psi(Quat)

            # Aero Force 
            #   The aerodynamic force is update until the residual does not fall below a prescribed 
            # factor, AELAMinRes. This is evaluated after Iter=0 to access the value of the residual 
            # ResLog10. According to the setting AELAOPTS.MinRes, AELAOPTS.MaxRes, AELAOPTS.LimRes 
            # the threshold can then be lowered or raised.
            
            if Iter==1: AELAMinRes = set_res_tight( ResLog10, AELAOPTS.MinRes, 
                                                    AELAOPTS.MaxRes,AELAOPTS.LimRes)
            
            if (AELAOPTS.Tight == False)  and (ResLog10>AELAMinRes or Iter==0):
                Force, Zeta, ZetaStar, Gamma, GammaStar = update_UVLMsol( iStep, dt, Time, Force,
                                            XBINPUT, VMINPUT, XBELEM, AELAOPTS, VMOPTS, 
                                            PsiA_G, Vrel[iStep,:].copy('F'), OriginA_a, Section,
                                            PosDefor, PsiDefor, PosDotDef, PsiDotDef, 
                                            Ufree, AIC, BIC,
                                            AeroForces, Zeta, ZetaDot, ZetaStar, Gamma, GammaStar )
                ForceAero = Force.copy()        
            else: Force[:,:] = ForceAero 
             
            # Add gravity loads.
            AddGravityLoads(Force, XBINPUT, XBELEM, AELAOPTS, PsiDefor, 
                                       VMINPUT.c, PsiA_G, FollowerForceRig=True)

            # Add prescribed loads
            Force += (XBINPUT.ForceStatic + XBINPUT.ForceDyn[iStep,:,:]).copy('F')
            # Dead forces are defined in FoR G, follower in FoR A.
            # None follows the structure as it deflects
                             
            #Update matrices and loads for structural dynamic analysis
            BeamLib.Cbeam3_Solv_State2Disp(XBINPUT, NumNodes_tot,
                                           XBELEM, XBNODE,
                                           PosIni, PsiIni,
                                           NumDof, X, dXdt,
                                           PosDefor, PsiDefor,
                                           PosDotDef, PsiDotDef)           
            
            (Msys, Csys, Ksys, Qsys) =  assemble_GEBM_tensors(
                                             iStep, 
                                             NumDof, NumNodes_tot,
                                             XBELEM, XBNODE,
                                             XBINPUT, VMINPUT, XBOPTS, 
                                             AELAOPTS, VMOPTS,  
                                             Vrel[iStep,:].copy('F')  ,                   
                                             PosIni, PsiIni,
                                             PosDefor, PsiDefor,
                                             PosDotDef, PsiDotDef,
                                             PosDotDotDef, PsiDotDotDef,
                                             Force,
                                             MssFull, CssFull, KssFull, FstrucFull,
                                             Msr, Csr,
                                             Force_Dof, Qstruc,
                                             MrsFull, CrsFull, KrsFull, FrigidFull,
                                             Mrr, Crr, Qrigid,
                                             Q, dQdt, dQddt,
                                             Force_All,
                                             Msys, Csys, Ksys, Asys, Qsys )  
            
            #Calculate system matrix for update calculation
            Asys = Ksys + Csys*gamma/(beta*dt) + Msys/(beta*dt**2)
       
            #Compute correction
            if XBOPTS.RigidDynamics==False:
                DQ[:]  = np.linalg.solve(Asys,-Qsys)
            else:    
                DQ[:NumDof.value]=0.0
                ### correction is zero, no need of this
                #Qsys[NumDof.value:NumDof.value+6]+=np.dot(
                #                        Ksys[NumDof.value:NumDof.value+6,:NumDof.value],
                #                        DQ[:NumDof.value] )
                DQ[NumDof.value:]=np.linalg.solve( Asys[NumDof.value:,NumDof.value:],
                                                  -Qsys[NumDof.value:])  
            Q     += DQ
            dQdt  += DQ*gamma/(beta*dt)
            dQddt += DQ/(beta*dt**2)

            #Update convergence criteria
            Res_Qglobal = max(abs(Qsys[iicheck]))
            Res_DeltaX  = max(abs(DQ[iicheck]))
            ResLog10 = max(Res_Qglobal/Res0_Qglobal, Res_DeltaX/Res0_DeltaX)
            
            # Update counter.
            Iter += 1
            if XBOPTS.PrintInfo.value==True: 
                sys.stdout.write( '   %-7d %-10.4e %-10.4e %8.4f\n' 
                                  %(Iter, max(abs(Qsys)), max(abs(DQ)), ResLog10) )             
        # END Netwon-Raphson.
        

        
        #----------------------------------------------------------------------- Terminate time-Loop
        
        # Unpack state variables
        Vrel[iStep,:]    = dQdt[NumDof.value:NumDof.value+6].copy('F')
        VrelDot[iStep,:] = dQddt[NumDof.value:NumDof.value+6].copy('F')
        Quat = dQdt[NumDof.value+6:].copy('F')
        Quat = Quat/np.linalg.norm(Quat)
        X=Q[:NumDof.value].copy('F') 
        dXdt=dQdt[:NumDof.value].copy('F')
        Cao  = xbl.Rot(Quat)
        PsiA_G = xbl.quat2psi(Quat)
        

        # sm: here to avoid crash at first time-step
        if AELAOPTS.Tight == False:
            XBOUT.ForceAeroList.append(ForceAero.copy('C'))

        # sm: save aero data
        if ( SaveDict['SaveWake']==True           and 
             iStep%SaveDict['SaveWakeFreq'] == 0  ):
            nfile=iStep//SaveDict['SaveWakeFreq']
            hdwake=h5py.File(dirwake+'%.4d.h5'%nfile,'w')
            hdwake['iStep']=iStep
            hdwake['Zeta']= np.float32(Zeta.copy('C'))
            hdwake['ZetaStar']= np.float32(ZetaStar.copy('C'))            
            hdwake.close()

        # sm debug:
        #XBOUT.ForceDofList.append( np.dot(FstrucFull, Force_Dof).copy() )
        XBOUT.ForceRigidList.append( np.dot(FrigidFull, Force_All).copy() )
              
        #update to converged nodal displacements and velocities
        X=Q[:NumDof.value].copy('F') 
        dXdt=dQdt[:NumDof.value].copy('F'); 
        BeamLib.Cbeam3_Solv_State2Disp(XBINPUT, NumNodes_tot, XBELEM, XBNODE,
                           PosIni, PsiIni, NumDof, X, dXdt,\
                           PosDefor, PsiDefor, PosDotDef, PsiDotDef)
        
        PosPsiTime[iStep,:3] = PosDefor[-1,:]
        PosPsiTime[iStep,3:] = PsiDefor[-1,XBELEM.NumNodes[-1]-1,:]
        
        #Position of all grid points in global FoR
        i1 = (iStep)*NumNodes_tot.value
        i2 = (iStep+1)*NumNodes_tot.value
        DynOut[i1:i2,:] = PosDefor
        
        #Export rigid-body velocities/accelerations
        if XBOPTS.OutInaframe.value==False:
            ACoa = np.zeros((6,6), ct.c_double, 'F')
            ACoa[:3,:3] = np.transpose(Cao)
            ACoa[3:,3:] = np.transpose(Cao)
            Vrel[iStep,:] = np.dot(ACoa,dQdt[NumDof.value:NumDof.value+6].copy('F'))
            VrelDot[iStep,:] = np.dot(ACoa,dQddt[NumDof.value:NumDof.value+6].copy('F'))
        
        
        if 'writeDict' in kwords and Settings.WriteOut == True:
            fp= write_SOL912_out(Time[iStep], PosDefor, PsiDefor, PosIni, PsiIni, XBELEM, 
                                 kwords['writeDict'], SaveDict, FileObject=fp)
                    
        # 'Rollup' due to external velocities. TODO: Must add gusts here!
        ZetaStar[:,:] = ZetaStar[:,:] + VMINPUT.U_infty*dt
        
        # sm: append outputs

 
        # sm I/O: FoR A velocities/accelerations
        XBOUT.drop( Time=Time, DynOut=DynOut, Vrel=Vrel, VrelDot=VrelDot  )
        XBOUT.PsiList.append(PsiDefor.copy())   
        XBOUT.QuatList.append(Quat.copy())
        XBOUT.CRVList.append(PsiA_G.copy())
        
        XBOUT.cputime.append( time.clock() - XBOUT.cputime[0] )
        
        if SaveDict['SaveProgress']:
            iisave=np.arange(1,NumSteps.value,np.ceil(NumSteps.value/SaveDict['NumSavePoints']))
            if any(iisave==iStep):
                PyLibs.io.save.h5file(SaveDict['OutputDir'], SaveDict['OutputFileRoot']+'.h5', 
                                      *OutList)
        
    # END Time loop
    
    
    if SaveDict['Format'] == 'dat': 
        write_SOL912_final(Time, PosPsiTime, NumNodes_tot, DynOut, Vrel, VrelDot, SaveDict) 
        
    # Close output file if it exists.
    if 'writeDict' in kwords and Settings.WriteOut == True:
        fp.close()
        
    # Close TecPlot ASCII FileObject.
    if Settings.PlotTec==True:
        PostProcess.CloseAeroTecFile(FileObject)   
    if XBOPTS.PrintInfo.value==True:
        sys.stdout.write(' ... done\n')
    
    # sm I/O: FoR A velocities/accelerations
    XBOUT.drop( Time=Time, PosPsiTime=PosPsiTime, DynOut=DynOut, Vrel=Vrel, VrelDot=VrelDot  )
    if  SaveDict['SaveWake'] is True:
        XBOUT.NTwake=NumSteps.value//SaveDict['SaveWakeFreq']
    PyLibs.io.save.h5file(SaveDict['OutputDir'], SaveDict['OutputFileRoot']+'.h5', *OutList)
    
    return XBOUT
Example #7
0
def zetaDotSubMat(r, psi, rDot, psiDot, v_a, omega_a, psi_G2a, xi, xiDot, iLin,
                  jLin, nBeam, nSection):
    """@brief create submatrix(3,N_states) for the linearized velocity at any
    point on the surface, zeta, defined by beam DoFs R and Psi, and the
    cross-sectional coordinate xi.
    @param r Position vector on beam, defined in the a-frame.
    @param psi CRV of orientation of beam cross-section.
    @param rDot Velocity of beam at r, defined in the a-frame.
    @param psiDot Time rate of change of psi.
    @param v_a Velocity of a-frame, defined in the a-frame.
    @param omega_a Angular vel of a-frame, defined in a-frame.
    @param psi_G2a CRV of orientation of a-frame relative to earth.
    @param xi Cross-sectional coordinate, defined in B-frame.
    @param xiDot Time rate of change of xi, defined in B-frame.
    @param iLin index of xi on the sectional DoF used for the linear analysis.
    @param jLin index of R on the beam DoF used for the linear analysis.
    @param nBeam Number of points on the beam axis.
    @param nSection Number of points in the section definition.
    
    @returns subMat Linear transformation from FBD + sectional DoFs to surface
              velocity (G-frame) at point r_0 + C_Ga*(R + C(Psi)*xi).
    @details This routine calculates the matrix corresponding to any point
              on the beam axis and corresponding cross-section.
              Interpolation of the primary beam and section DoFs may pre-
              or post- multiply this matrix in an aeroelastic analysis.
    """
    # Total states on RHS of matrix
    nStates = 2 * 6 * nBeam + 9 + 2 * 3 * nSection
    # Initialise subMat
    subMat = np.zeros((3, nStates))

    # term 1: \delta(C^{Ga}v_a)
    cGa = xbl.Psi2TransMat(psi_G2a)
    skewVa = xbl.Skew(v_a)
    tangGa = xbl.Tangential(psi_G2a)
    # term for \delta \psi_{Ga}
    subMat[:, 2 * 6 * nBeam + 6:2 * 6 * nBeam +
           9:1] += -np.dot(cGa, np.dot(skewVa, tangGa))
    # term for \delta v_a
    subMat[:, 2 * 6 * nBeam:2 * 6 * nBeam + 3:1] += cGa

    # term 2: \delta(C^{Ga} \tilde{\Omega_a_a} R_a)
    skewOmAa = xbl.Skew(omega_a)
    skewRa = xbl.Skew(r)
    # term for \delta \psi_{Ga}
    subMat[:, 2 * 6 * nBeam + 6:2 * 6 * nBeam +
           9:1] += -np.dot(cGa, np.dot(xbl.Skew(np.dot(skewOmAa, r)), tangGa))
    # term for \delta \Omega_a_a
    subMat[:, 2 * 6 * nBeam + 3:2 * 6 * nBeam + 6:1] += -np.dot(cGa, skewRa)
    # term for \delta R_a
    subMat[:, 6 * jLin:6 * jLin + 3:1] += np.dot(cGa, skewOmAa)

    # term 3: \delta(C^{Ga}\tilde{\Omega_a_a}C^{aB}\xi_B)
    cAb = xbl.Psi2TransMat(psi)
    skewXi = xbl.Skew(xi)
    tangAb = xbl.Tangential(psi)
    # term for \delta \psi_{Ga}
    subMat[:, 2 * 6 * nBeam + 6:2 * 6 * nBeam + 9:1] += -np.dot(
        cGa, np.dot(xbl.Skew(np.dot(skewOmAa, np.dot(cAb, xi))), tangGa))
    # term for \delta \Omega_a_a
    subMat[:, 2 * 6 * nBeam + 3:2 * 6 * nBeam +
           6:1] += -np.dot(cGa, xbl.Skew(np.dot(cAb, xi)))
    # term for \delta \psi_{aB}
    subMat[:, 6 * jLin + 3:6 * jLin + 6:1] += -np.dot(
        cGa, np.dot(skewOmAa, np.dot(cAb, np.dot(skewXi, tangAb))))
    # term for \delta xi_B
    subMat[:, 2 * 6 * nBeam + 9 + 3 * iLin:2 * 6 * nBeam + 9 + 3 * iLin +
           3:1] += np.dot(cGa, np.dot(skewOmAa, cAb))

    # term 4: \delta(C^{Ga}\dot{R}_a)
    skewRdot = xbl.Skew(rDot)
    # term for \delta \psi_{Ga}
    subMat[:, 2 * 6 * nBeam + 6:2 * 6 * nBeam +
           9:1] += -np.dot(cGa, np.dot(skewRdot, tangGa))
    # term for \delta \dot{R}_a
    subMat[:, 6 * nBeam + 6 * jLin:6 * nBeam + 6 * jLin + 3:1] += cGa

    # term 5: \delta(C^{Ga}C^{aB}\dot{\xi}_B)
    skewXiDot = xbl.Skew(xiDot)
    # term for \delta \psi_{Ga}
    subMat[:, 2 * 6 * nBeam + 6:2 * 6 * nBeam +
           9:1] += -np.dot(cGa, np.dot(xbl.Skew(np.dot(cAb, xiDot)), tangGa))
    # term for \delta \psi_{aB}
    subMat[:, 6 * jLin + 3:6 * jLin +
           6:1] += -np.dot(cGa, np.dot(cAb, np.dot(skewXiDot, tangAb)))
    # term for \delta \dot{\xi}_B
    subMat[:, 2 * 6 * nBeam + 9 + 3 * nSection + 3 * iLin:2 * 6 * nBeam + 9 +
           3 * nSection + 3 * iLin + 3] += np.dot(cGa, cAb)

    # term 6: \delta(C^{Ga}C^{aB}\tilde{T(\psi_{aB})\dot{\psi}_{aB}\xi_B)
    # Note: eqn. rearranged to \delta(-C^{Ga}C^{aB}\tilde{\xi}T(\psi_{aB})\dot{\psi}_{aB})
    # term for  \delta \psi_{Ga}
    subMat[:, 2 * 6 * nBeam + 6:2 * 6 * nBeam + 9:1] += np.dot(
        cGa,
        np.dot(xbl.Skew(np.dot(cAb, np.dot(skewXi, np.dot(tangAb, psiDot)))),
               tangGa))
    # term for \delta \psi_{aB}
    subMat[:, 6 * jLin + 3:6 * jLin + 6:1] += np.dot(
        cGa,
        np.dot(
            cAb,
            np.dot(xbl.Skew(np.dot(skewXi, np.dot(tangAb, psiDot))), tangAb)))
    # term for \delta \xi_B
    subMat[:, 2 * 6 * nBeam + 9 + 3 * iLin:2 * 6 * nBeam + 9 + 3 * iLin +
           3:1] += np.dot(cGa, np.dot(cAb, xbl.Skew(np.dot(tangAb, psiDot))))
    # term for \delta T(\psi_{aB})\dot{\psi_{aB}}
    subMat[:, 6 * jLin + 3:6 * jLin + 6:1] += -np.dot(
        cGa, np.dot(cAb, np.dot(skewXi, xbl.a1(psi, psiDot))))
    # term for \delta \dot{\psi}_{aB}
    subMat[:, 6 * nBeam + 6 * jLin + 3:6 * nBeam + 6 * jLin +
           6:1] += -np.dot(cGa, np.dot(cAb, np.dot(skewXi, tangAb)))
    return subMat
def Solve_Py(XBINPUT,XBOPTS,SaveDict=Settings.SaveDict):

    """Nonlinear dynamic structural solver in Python. Assembly of matrices 
    is carried out with Fortran subroutines."""
    
    #Check correct solution code
    assert XBOPTS.Solution.value == 912, ('NonlinearDynamic requested' +\
                                              ' with wrong solution code')
    
    # I/O management
    Settings.SaveDict=SaveDict # overwrite for compatibility with 'dat' format output
    XBOUT=DerivedTypes.Xboutput()  
    XBOUT.cputime.append(time.clock())
    if SaveDict['Format']=='h5':
        Settings.WriteOut=False
        Settings.PlotTec=False
        OutList=[XBOPTS, XBINPUT, XBOUT]
            
    #Initialise beam
    XBINPUT, XBOPTS, NumNodes_tot, XBELEM, PosIni, PsiIni, XBNODE, NumDof \
                = BeamInit.Static(XBINPUT,XBOPTS)
     
    # Solve static
    # Note: gravity loads added according to XBINPUT.PsiA_G into Solve_Py_Static
    if XBOPTS.ImpStart==True:
        PosDefor = PosIni.copy(order='F')
        PsiDefor = PsiIni.copy(order='F')
        XBOUT.ForceStaticTotal = XBINPUT.ForceStatic.copy('F')
        AddGravityLoads(XBOUT.ForceStaticTotal,XBINPUT,XBELEM,
                        AELAOPTS=None, # allows defining inertial/elastic axis
                        PsiDefor=PsiDefor,
                        chord=0.0, # used to define inertial/elastic axis
                        PsiA_G=XBINPUT.PsiA_G,
                        FollowerForceRig=XBOPTS.FollowerForceRig.value)
    else:
        if XBNODE.Sflag.any():
            raise NameError('Static solution with spherical joint not '
                                                 'implemented and/or possible!')
        XBOPTS.Solution.value = 112
        XBSTA = Solve_Py_Static(XBINPUT, XBOPTS, SaveDict=SaveDict)
        XBOPTS.Solution.value = 912
        PosDefor=XBSTA.PosDeforStatic.copy(order='F')
        PsiDefor=XBSTA.PsiDeforStatic.copy(order='F')
        XBOUT.PosDeforStatic  =XBSTA.PosDeforStatic
        XBOUT.PsiDeforStatic  =XBSTA.PsiDeforStatic
        XBOUT.ForceStaticTotal=XBSTA.ForceStaticTotal # includes gravity
        del XBSTA


    if SaveDict['Format']=='dat': 
        PyLibs.io.dat.write_SOL912_def(XBOPTS,XBINPUT,XBELEM,
                                NumNodes_tot,PosDefor,PsiDefor,SaveDict)
    
    ## sm I/O
    #XBOUT.PosDefor=PosDefor
    #XBOUT.PsiDefor=PsiDefor

    #Initialise variables for dynamic analysis
    Time, NumSteps, ForceTime, Vrel, VrelDot,\
    PosDotDef, PsiDotDef,\
    OutGrids, PosPsiTime, VelocTime, DynOut = BeamInit.Dynamic(XBINPUT,XBOPTS)
    # Delete unused variables.
    del OutGrids, VelocTime
    
    #Write _force file
    if SaveDict['Format']=='dat': 
        PyLibs.io.dat.write_SOL912_force(XBOPTS,XBINPUT,XBELEM,
                                         Time, ForceTime, Vrel, VrelDot)
    
    # sm I/O
    ### why forced velocity with Sol912 ???
    ### If forced velocities are prescribed, then is Sol312
    XBOUT.Time_force=Time               # ...SOL912_force.dat
    XBOUT.ForceTime_force=ForceTime
    XBOUT.Vrel_force=Vrel
    XBOUT.VrelDot_force=VrelDot  
    # debugging 
    XBOUT.KsysList=[]
    XBOUT.MsysList=[]
    
    #---------------------------------------------------- Start Dynamic Solution

    if XBOPTS.PrintInfo.value==True:
        sys.stdout.write('Solve nonlinear dynamic case in Python ... \n')
    
    #Initialise structural system tensors
    MssFull = np.zeros((NumDof.value,NumDof.value), ct.c_double, 'F')
    CssFull = np.zeros((NumDof.value,NumDof.value), ct.c_double, 'F') 
    KssFull = np.zeros((NumDof.value,NumDof.value), ct.c_double, 'F') 
    FstrucFull = np.zeros((NumDof.value,NumDof.value), ct.c_double, 'F')
    
    ms = ct.c_int()
    cs = ct.c_int()
    ks = ct.c_int()
    fs = ct.c_int()
    
    Msr = np.zeros((NumDof.value,6), ct.c_double, 'F')
    Csr = np.zeros((NumDof.value,6), ct.c_double, 'F')
    
    X     = np.zeros(NumDof.value, ct.c_double, 'F')
    dXdt  = np.zeros(NumDof.value, ct.c_double, 'F')
    Force_Dof = np.zeros(NumDof.value, ct.c_double, 'F')
    
    Qstruc = np.zeros(NumDof.value, ct.c_double, 'F')
    
    #Initialise rigid-body system tensors
    MrsFull = np.zeros((6,NumDof.value), ct.c_double, 'F')
    CrsFull = np.zeros((6,NumDof.value), ct.c_double, 'F') 
    KrsFull = np.zeros((6,NumDof.value), ct.c_double, 'F') 
    FrigidFull = np.zeros((6,NumDof.value+6), ct.c_double, 'F')
    
    mr = ct.c_int()
    cr = ct.c_int()
    kr = ct.c_int()
    fr = ct.c_int()
    
    Mrr = np.zeros((6,6), ct.c_double, 'F')
    Crr = np.zeros((6,6), ct.c_double, 'F')
        
    Qrigid = np.zeros(6, ct.c_double, 'F')
    
    #Initialise full system tensors
    Q     = np.zeros(NumDof.value+6+4, ct.c_double, 'F')
    DQ    = np.zeros(NumDof.value+6+4, ct.c_double, 'F')
    dQdt  = np.zeros(NumDof.value+6+4, ct.c_double, 'F')
    dQddt = np.zeros(NumDof.value+6+4, ct.c_double, 'F')
    Force_All = np.zeros(NumDof.value+6, ct.c_double, 'F')

    Msys = np.zeros((NumDof.value+6+4,NumDof.value+6+4), ct.c_double, 'F')
    Csys = np.zeros((NumDof.value+6+4,NumDof.value+6+4), ct.c_double, 'F') 
    Ksys = np.zeros((NumDof.value+6+4,NumDof.value+6+4), ct.c_double, 'F') 
    Asys = np.zeros((NumDof.value+6+4,NumDof.value+6+4), ct.c_double, 'F')
    
    Qsys = np.zeros(NumDof.value+6+4, ct.c_double, 'F')
    
    
    #Initialise rotation operators 
    Quat =  xbl.psi2quat(XBINPUT.PsiA_G)
    Cao  = XbeamLib.Rot(Quat)
    ACoa = np.zeros((6,6), ct.c_double, 'F')
    Cqr = np.zeros((4,6), ct.c_double, 'F')
    Cqq = np.zeros((4,4), ct.c_double, 'F')
        
    Unit4 = np.zeros((4,4), ct.c_double, 'F')
    for i in range(4): Unit4[i,i] = 1.0
        
    #Extract initial displacements and velocities
    BeamLib.Cbeam_Solv_Disp2State(NumNodes_tot, NumDof, XBINPUT, XBNODE,
                          PosDefor, PsiDefor, PosDotDef, PsiDotDef,
                          X, dXdt)
          
    #Initialise accelerations as zero arrays
    PosDotDotDef = np.zeros((NumNodes_tot.value,3),ct.c_double,'F')
    PsiDotDotDef = np.zeros((XBINPUT.NumElems,Settings.MaxElNod,3),
                            ct.c_double, 'F')
    
    #Populate state vector
    Q[:NumDof.value]=X.copy('F')
    dQdt[:NumDof.value]=dXdt.copy('F')
    dQdt[NumDof.value:NumDof.value+6] = Vrel[0,:].copy('F')
    dQdt[NumDof.value+6:]= Quat.copy('F')
    
    # Force at the first time-step
    # Gravity needs including for correctly computing the accelerations at the 
    # 1st time-step
    # Note: rank of Force increased after removing ForceTime
    #Force = (XBINPUT.ForceStatic + XBINPUT.ForceDyn*ForceTime[0]).copy('F')
    #Force = (XBINPUT.ForceStatic + XBINPUT.ForceDyn[0,:,:]).copy('F')
    Force = (XBOUT.ForceStaticTotal + XBINPUT.ForceDyn[0,:,:]).copy('F')


    #Assemble matrices and loads for structural dynamic analysis
    tmpVrel=Vrel[0,:].copy('F')
    tmpQuat=Quat.copy('F')
    BeamLib.Cbeam3_Asbly_Dynamic(XBINPUT, NumNodes_tot, XBELEM, XBNODE,
                         PosIni, PsiIni, PosDefor, PsiDefor,
                         PosDotDef, PsiDotDef, PosDotDotDef, PsiDotDotDef,
                         Force, tmpVrel, 0*tmpVrel,
                         NumDof, Settings.DimMat,
                         ms, MssFull, Msr,
                         cs, CssFull, Csr,
                         ks, KssFull, fs, FstrucFull,
                         Qstruc, XBOPTS, Cao)
    
    BeamLib.f_fem_m2v(ct.byref(NumNodes_tot),\
                              ct.byref(ct.c_int(6)),\
                              Force.ctypes.data_as(ct.POINTER(ct.c_double)),\
                              ct.byref(NumDof),\
                              Force_Dof.ctypes.data_as(ct.POINTER(ct.c_double)),\
                              XBNODE.Vdof.ctypes.data_as(ct.POINTER(ct.c_int)) )

    Qstruc -= np.dot(FstrucFull, Force_Dof)
    
    
    #Assemble matrices for rigid-body dynamic analysis
    BeamLib.Xbeam_Asbly_Dynamic(XBINPUT, NumNodes_tot, XBELEM, XBNODE,\
                         PosIni, PsiIni, PosDefor, PsiDefor,\
                         PosDotDef, PsiDotDef, PosDotDotDef, PsiDotDotDef,\
                         tmpVrel, 0*tmpVrel, tmpQuat,\
                         NumDof, Settings.DimMat,\
                         mr, MrsFull, Mrr,\
                         cr, CrsFull, Crr, Cqr, Cqq,\
                         kr, KrsFull, fr, FrigidFull,\
                         Qrigid, XBOPTS, Cao)
    
    BeamLib.f_fem_m2v_nofilter(ct.byref(NumNodes_tot),\
                               ct.byref(ct.c_int(6)),\
                               Force.ctypes.data_as(ct.POINTER(ct.c_double)),\
                               ct.byref(ct.c_int(NumDof.value+6)),\
                               Force_All.ctypes.data_as(ct.POINTER(ct.c_double)) )

    Qrigid -= np.dot(FrigidFull, Force_All)
    
          
    #Separate assembly of follower and dead loads   
    tmpForceTime=ForceTime[0].copy('F') 
    tmpQforces,Dummy,tmpQrigid = XbeamLib.LoadAssembly(XBINPUT, XBELEM, XBNODE, XBOPTS, NumDof, \
                                    PosIni, PsiIni, PosDefor, PsiDefor, \
                                    ### sm increased rank of ForceDyn_*
                                    #(XBINPUT.ForceStatic_foll + XBINPUT.ForceDyn_foll*tmpForceTime), \
                                    #(XBINPUT.ForceStatic_dead + XBINPUT.ForceDyn_dead*tmpForceTime), \
                                    (XBINPUT.ForceStatic_foll + XBINPUT.ForceDyn_foll[0,:,:]), \
                                    (XBINPUT.ForceStatic_dead + XBINPUT.ForceDyn_dead[0,:,:]), \
                                    Cao,1)
                           
    Qstruc -= tmpQforces      
    Qrigid -= tmpQrigid
    
    #Assemble system matrices
    Msys[:NumDof.value,:NumDof.value] = MssFull.copy('F')
    Msys[:NumDof.value,NumDof.value:NumDof.value+6] = Msr.copy('F')
    Msys[NumDof.value:NumDof.value+6,:NumDof.value] = MrsFull.copy('F')
    Msys[NumDof.value:NumDof.value+6,NumDof.value:NumDof.value+6] = Mrr.copy('F')
    Msys[NumDof.value+6:,NumDof.value+6:] = Unit4.copy('F')
       
    Qsys[:NumDof.value] = Qstruc
    Qsys[NumDof.value:NumDof.value+6] = Qrigid
    Qsys[NumDof.value+6:] = np.dot(Cqq,dQdt[NumDof.value+6:])
    
    # -------------------------------------------------------------------   
    # special BCs
    iiblock=[]
    if XBNODE.Sflag.any():
        # block translations (redundant:)
        for ii in range(3):
            if XBINPUT.EnforceTraVel_FoRA[ii] == True: iiblock.append(NumDof.value+ii)
        # block rotations
        iirotfree=[] # free rotational dof 
        for ii in range(3):
            if XBINPUT.EnforceAngVel_FoRA[ii] is True: iiblock.append(NumDof.value+3+ii)
            else: iirotfree.append(NumDof.value+3+ii)

    if len(iiblock)>0:
        Msys[iiblock,:] = 0.0
        Msys[:,iiblock] = 0.0 
        Msys[iiblock,iiblock] = 1.0
        Csys[iiblock,:] = 0.0
        Ksys[iiblock,:] = 0.0
        Qsys[iiblock]   = 0.0
        if XBINPUT.sph_joint_damping is not None:
            Csys[iirotfree,iirotfree] += XBINPUT.sph_joint_damping
            Qsys[iirotfree] += XBINPUT.sph_joint_damping*dQdt[iirotfree]
    #from IPython import embed; embed()     
    # add structural damping term
    if XBINPUT.str_damping_model is not None:
        Cdamp = XBINPUT.str_damping_param['alpha'] * MssFull + \
                XBINPUT.str_damping_param['beta']  * KssFull
        Qsys[:NumDof.value] += np.dot( Cdamp, dQdt[:NumDof.value] )                  
        pass
     
    # -------------------------------------------------------------------  
    
    #store initial matrices for eigenvalues analysis
    XBOUT.Msys0 = Msys.copy()
    XBOUT.Csys0 = Csys.copy()
    XBOUT.Ksys0 = Ksys.copy()
    XBOUT.Qsys0 = Qsys.copy()

    #Initial Accel
    #dQddt[:] = np.dot(np.linalg.inv(Msys), -Qsys)
    #dQddt[:] = np.linalg.solve(Msys,-Qsys) # most correct but inaccurate / Msys ill-conditioned
    # solve only rigid body dynamics
    dQddt[:NumDof.value]=0.0
    dQddt[NumDof.value:]=np.linalg.solve( Msys[NumDof.value:,NumDof.value:],
                                                          -Qsys[NumDof.value:] ) 


    XBOUT.dQddt0=dQddt.copy()
    
    #Record position of all grid points in global FoR at initial time step
    DynOut[0:NumNodes_tot.value,:] = PosDefor
    
    #Position/rotation of the selected node in initial deformed configuration
    PosPsiTime[0,:3] = PosDefor[-1,:]
    PosPsiTime[0,3:] = PsiDefor[-1,XBELEM.NumNodes[-1]-1,:]
    
    
    #Get gamma and beta for Newmark scheme
    gamma = 0.5 + XBOPTS.NewmarkDamp.value
    beta = 0.25*(gamma + 0.5)**2
    
    # sm write class
    XBOUT.QuatList.append(Quat)
    XBOUT.PosIni=PosIni
    XBOUT.PsiIni=PsiIni 
   
    XBOUT.AsysListStart=[]
    XBOUT.AsysListEnd=[]
    XBOUT.MsysList=[]
    XBOUT.CsysList=[]
    XBOUT.KsysList=[]
    
    #Time loop
    for iStep in range(NumSteps.value):
        
        if XBOPTS.PrintInfo.value==True:
            sys.stdout.write('  Time: %-10.4e\n' %(Time[iStep+1]))
            sys.stdout.write('   SubIter DeltaF     DeltaX     ResLog10\n')
        
        
        #calculate dt
        dt = Time[iStep+1] - Time[iStep]
        
        ###Predictor step
        Q += dt*dQdt + (0.5-beta)*dQddt*dt**2
        dQdt += (1.0-gamma)*dQddt*dt
        ### Corrector
        #dQddt[:] = 0.0 # initial guess for acceleration at next time-step is zero
        Q += beta*dQddt*dt**2
        dQdt += gamma*dQddt*dt

        #Reset convergence parameters
        Iter = 0
        ResLog10 = 1.0

        #Newton-Raphson loop      
        while ( (ResLog10 > XBOPTS.MinDelta.value) \
                & (Iter < XBOPTS.MaxIterations.value) ):
            
            #set tensors to zero 
            MssFull[:,:] = 0.0; CssFull[:,:] = 0.0
            KssFull[:,:] = 0.0; FstrucFull[:,:] = 0.0
            Msr[:,:] = 0.0; Csr[:,:] = 0.0
            Qstruc[:] = 0.0
            
            MrsFull[:,:] = 0.0; CrsFull[:,:] = 0.0
            KrsFull[:,:] = 0.0; FrigidFull[:,:] = 0.0
            Mrr[:,:] = 0.0; Crr[:,:] = 0.0
            Qrigid[:] = 0.0
    
            Msys[:,:] = 0.0; Csys[:,:] = 0.0
            Ksys[:,:] = 0.0; Asys[:,:] = 0.0;
            Qsys[:] = 0.0
            
            
            #Update counter
            Iter += 1
            
            if XBOPTS.PrintInfo.value==True:
                sys.stdout.write('   %-7d ' %(Iter))

            #nodal displacements and velocities from state vector
            X=Q[:NumDof.value].copy('F') 
            dXdt=dQdt[:NumDof.value].copy('F'); 
            BeamLib.Cbeam3_Solv_State2Disp(XBINPUT, NumNodes_tot, XBELEM, XBNODE,
                           PosIni, PsiIni, NumDof, X, dXdt,\
                           PosDefor, PsiDefor, PosDotDef, PsiDotDef)
            
            
            #rigid-body velocities and orientation from state vector
            Vrel[iStep+1,:] = dQdt[NumDof.value:NumDof.value+6].copy('F')
            VrelDot[iStep+1,:] = dQddt[NumDof.value:NumDof.value+6].copy('F')
            Quat = dQdt[NumDof.value+6:].copy('F')
            Quat = Quat/np.linalg.norm(Quat)
            Cao  = XbeamLib.Rot(Quat)


            ### sm: removed ForceTime and increased rank of ForceDyn 
            # Note: do not add gravity load here!
            #Force at current time-step
            #Force = (XBINPUT.ForceStatic+XBINPUT.ForceDyn*ForceTime[iStep+1]).copy('F')
            Force = (XBINPUT.ForceStatic+XBINPUT.ForceDyn[iStep+1,:,:]).copy('F')

            # Add gravity loads (accounting for new orientation)
            AddGravityLoads(Force, XBINPUT, XBELEM,
                        AELAOPTS=None, # allows defining inertial/elastic axis
                        PsiDefor=PsiDefor,
                        chord = 0.0, # used to define inertial/elastic axis
                        PsiA_G=xbl.quat2psi(Quat),
                        FollowerForceRig=XBOPTS.FollowerForceRig.value)

            #Assemble matrices and loads for structural dynamic analysis
            tmpVrel=Vrel[iStep+1,:].copy('F')
            tmpQuat=Quat.copy('F')
            BeamLib.Cbeam3_Asbly_Dynamic(XBINPUT, NumNodes_tot, XBELEM, XBNODE,\
                                 PosIni, PsiIni, PosDefor, PsiDefor,\
                                 PosDotDef, PsiDotDef, PosDotDotDef, PsiDotDotDef,\
                                 Force, tmpVrel, 0*tmpVrel,\
                                 NumDof, Settings.DimMat,\
                                 ms, MssFull, Msr,\
                                 cs, CssFull, Csr,\
                                 ks, KssFull, fs, FstrucFull,\
                                 Qstruc, XBOPTS, Cao)
            
            BeamLib.f_fem_m2v(ct.byref(NumNodes_tot),\
                              ct.byref(ct.c_int(6)),\
                              Force.ctypes.data_as(ct.POINTER(ct.c_double)),\
                              ct.byref(NumDof),\
                              Force_Dof.ctypes.data_as(ct.POINTER(ct.c_double)),\
                              XBNODE.Vdof.ctypes.data_as(ct.POINTER(ct.c_int)) )
                    
            
            #Assemble matrices for rigid-body dynamic analysis
            BeamLib.Xbeam_Asbly_Dynamic(XBINPUT, NumNodes_tot, XBELEM, XBNODE,\
                                 PosIni, PsiIni, PosDefor, PsiDefor,\
                                 PosDotDef, PsiDotDef, PosDotDotDef, PsiDotDotDef,\
                                 tmpVrel, 0*tmpVrel, tmpQuat,\
                                 NumDof, Settings.DimMat,\
                                 mr, MrsFull, Mrr,\
                                 cr, CrsFull, Crr, Cqr, Cqq,\
                                 kr, KrsFull, fs, FrigidFull,\
                                 Qrigid, XBOPTS, Cao)
    
            BeamLib.f_fem_m2v_nofilter(ct.byref(NumNodes_tot),\
                                       ct.byref(ct.c_int(6)),\
                                       Force.ctypes.data_as(ct.POINTER(ct.c_double)),\
                                       ct.byref(ct.c_int(NumDof.value+6)),\
                                       Force_All.ctypes.data_as(ct.POINTER(ct.c_double)) )
        
        
            #Residual at first iteration
            if(Iter == 1):
                Res0_Qglobal = max(max(abs(Qsys)),1)
                Res0_DeltaX  = max(max(abs(DQ)),1)
            
            
            #Assemble discrete system matrices with linearised quaternion equations          
            Msys[:NumDof.value,:NumDof.value] = MssFull.copy('F')
            Msys[:NumDof.value,NumDof.value:NumDof.value+6] = Msr.copy('F')
            Msys[NumDof.value:NumDof.value+6,:NumDof.value] = MrsFull.copy('F')
            Msys[NumDof.value:NumDof.value+6,NumDof.value:NumDof.value+6] = Mrr.copy('F')
            Msys[NumDof.value+6:,NumDof.value+6:] = Unit4.copy('F')
            
            Csys[:NumDof.value,:NumDof.value] = CssFull.copy('F')
            Csys[:NumDof.value,NumDof.value:NumDof.value+6] = Csr.copy('F')
            Csys[NumDof.value:NumDof.value+6,:NumDof.value] = CrsFull.copy('F')
            Csys[NumDof.value:NumDof.value+6,NumDof.value:NumDof.value+6] = Crr.copy('F')
            
            Csys[NumDof.value+6:,NumDof.value:NumDof.value+6] = Cqr.copy('F')
            Csys[NumDof.value+6:,NumDof.value+6:] = Cqq.copy('F')
            
            Ksys[:NumDof.value,:NumDof.value] = KssFull.copy('F')
            Ksys[NumDof.value:NumDof.value+6,:NumDof.value] = KrsFull.copy('F')
            
          
            #Separate assembly of follower and dead loads   
            #tmpForceTime=ForceTime[iStep+1].copy('F') 
            tmpQforces,Dummy,tmpQrigid = XbeamLib.LoadAssembly(XBINPUT, XBELEM, XBNODE, XBOPTS, NumDof, \
                                            PosIni, PsiIni, PosDefor, PsiDefor, \
                                            ### sm: increased rank of ForceDyn_*
                                            #(XBINPUT.ForceStatic_foll + XBINPUT.ForceDyn_foll*tmpForceTime), \
                                            #(XBINPUT.ForceStatic_dead + XBINPUT.ForceDyn_dead*tmpForceTime), \
                                            (XBINPUT.ForceStatic_foll + XBINPUT.ForceDyn_foll[iStep+1,:,:] ), \
                                            (XBINPUT.ForceStatic_dead + XBINPUT.ForceDyn_dead[iStep+1,:,:] ), \
                                            Cao,1)
                                   
            Qstruc -= tmpQforces      
            Qrigid -= tmpQrigid
    
            #Compute residual
            Qstruc += -np.dot(FstrucFull, Force_Dof)
            Qrigid += -np.dot(FrigidFull, Force_All)
            
            # final of last iter
            XBOUT.Qstruc=Qstruc.copy()
            XBOUT.Qrigid=Qrigid.copy()
            # final of initial iter
            if iStep==0:
                XBOUT.Qstruc0=Qstruc.copy()
                XBOUT.Qrigid0=Qrigid.copy()
  
            Qsys[:NumDof.value] = Qstruc
            Qsys[NumDof.value:NumDof.value+6] = Qrigid
            Qsys[NumDof.value+6:] = np.dot(Cqq,dQdt[NumDof.value+6:])
            
            Qsys += np.dot(Msys,dQddt)

            # include damping
            if XBINPUT.str_damping_model == 'prop':
                Cdamp = XBINPUT.str_damping_param['alpha'] * MssFull + \
                        XBINPUT.str_damping_param['beta']  * KssFull
                Csys[:NumDof.value,:NumDof.value] += Cdamp
                Qsys[:NumDof.value] += np.dot(Cdamp, dQdt[:NumDof.value])
                                
            # special BCs
            # if  SphFlag:
            if len(iiblock)>0: # allow to enforce only attitude while keeping velocity free
                Msys[iiblock,:] = 0.0
                Msys[iiblock,iiblock] = 1.0
                Csys[iiblock,:] = 0.0
                Ksys[iiblock,:] = 0.0
                Qsys[iiblock]   = 0.0
                if XBINPUT.sph_joint_damping is not None:
                    Csys[iirotfree,iirotfree] += XBINPUT.sph_joint_damping
                    Qsys[iirotfree] += XBINPUT.sph_joint_damping*dQdt[iirotfree]

                #XBOUT.Msys=Msys.copy('F')
                #XBOUT.Qsys=Qsys.copy('F')
                #XBOUT.Csys=Csys.copy('F')
                #XBOUT.Ksys=Ksys.copy('F')
  
            #Calculate system matrix for update calculation
            Asys = Ksys + Csys*gamma/(beta*dt) + Msys/(beta*dt**2)

            #Compute correction
            DQ[:] = np.dot(np.linalg.inv(Asys), -Qsys)

            Q += DQ
            dQdt += DQ*gamma/(beta*dt)
            dQddt += DQ/(beta*dt**2)
            
            #Update convergence criteria
            if XBOPTS.PrintInfo.value==True:                 
                sys.stdout.write('%-10.4e ' %(max(abs(Qsys))))
            
            Res_Qglobal = max(abs(Qsys))
            Res_DeltaX  = max(abs(DQ))
            
            ResLog10 = max(Res_Qglobal/Res0_Qglobal,Res_DeltaX/Res0_DeltaX)
            
            if XBOPTS.PrintInfo.value==True:
                sys.stdout.write('%-10.4e %8.4f\n' %(max(abs(DQ)),ResLog10))
                
        #END Newton-Raphson
        
        # debug
        #XBOUT.ForceRigidList.append( np.dot(FrigidFull, Force_All).copy() )
        
        #update to converged nodal displacements and velocities
        X=Q[:NumDof.value].copy('F') 
        dXdt=dQdt[:NumDof.value].copy('F'); 
        BeamLib.Cbeam3_Solv_State2Disp(XBINPUT, NumNodes_tot, XBELEM, XBNODE,
                           PosIni, PsiIni, NumDof, X, dXdt,\
                           PosDefor, PsiDefor, PosDotDef, PsiDotDef)
        
        PosPsiTime[iStep+1,:3] = PosDefor[(NumNodes_tot.value-1)/2+1,:]
        PosPsiTime[iStep+1,3:] = PsiDefor[-1,XBELEM.NumNodes[-1]-1,:]
        
        #Position of all grid points in global FoR
        i1 = (iStep+1)*NumNodes_tot.value
        i2 = (iStep+2)*NumNodes_tot.value
        DynOut[i1:i2,:] = PosDefor
        
        #Export rigid-body velocities/accelerations
        if XBOPTS.OutInaframe.value==True:
            Vrel[iStep+1,:] = dQdt[NumDof.value:NumDof.value+6].copy('F')
            VrelDot[iStep+1,:] = dQddt[NumDof.value:NumDof.value+6].copy('F')
        else:
            Quat = dQdt[NumDof.value+6:].copy('F')
            Quat = Quat/np.linalg.norm(Quat)
            Cao  = XbeamLib.Rot(Quat)
            ACoa[:3,:3] = np.transpose(Cao)
            ACoa[3:,3:] = np.transpose(Cao)
            
            Vrel[iStep+1,:] = np.dot(ACoa,dQdt[NumDof.value:NumDof.value+6].copy('F'))
            VrelDot[iStep+1,:] = np.dot(ACoa,dQddt[NumDof.value:NumDof.value+6].copy('F'))
    
        # sm: append outputs
        XBOUT.QuatList.append(Quat.copy())
 
        # sm I/O: FoR A velocities/accelerations
        XBOUT.Time=Time                     # ...dyn.dat
        #XBOUT.PosPsiTime = PosPsiTime       
        
        XBOUT.DynOut=DynOut                 # ...shape.dat
        
        XBOUT.Vrel=Vrel                     # ...rigid.dat
        XBOUT.VrelDot=VrelDot
        #XBOUT.PosPsiTime=PosPsiTime          
 
        XBOUT.PsiList.append(PsiDefor.copy())   

        XBOUT.cputime.append( time.clock() - XBOUT.cputime[0] )

        if SaveDict['SaveProgress']:
            iisave=np.arange(1,NumSteps.value,np.ceil(NumSteps.value/SaveDict['NumSavePoints']))
            if any(iisave==iStep):
                PyLibs.io.save.h5file(SaveDict['OutputDir'], SaveDict['OutputFileRoot']+'.h5', 
                                      *OutList)
    
    #END Time loop

    if SaveDict['Format'] == 'dat': 
        PyLibs.io.dat.write_SOL912_final(Time, PosPsiTime, 
                                         NumNodes_tot, DynOut, Vrel, VrelDot, SaveDict) 
    
    if XBOPTS.PrintInfo.value==True:
        sys.stdout.write(' ... done\n')

    # sm I/O: FoR A velocities/accelerations
    XBOUT.Time=Time                     # ...dyn.dat
    XBOUT.PosPsiTime = PosPsiTime       
    
    XBOUT.DynOut=DynOut                 # ...shape.dat
    
    XBOUT.Vrel=Vrel                     # ...rigid.dat
    XBOUT.VrelDot=VrelDot
    XBOUT.PosPsiTime=PosPsiTime    
    
    # save h5 
    XBINPUT.ForceDyn = XBINPUT.ForceDyn + XBINPUT.ForceDyn_foll + XBINPUT.ForceDyn_dead
    del(XBINPUT.ForceDyn_dead)
    del(XBINPUT.ForceDyn_foll)
    PyLibs.io.save.h5file(SaveDict['OutputDir'], SaveDict['OutputFileRoot']+'.h5', *OutList)
     
    
    return XBOUT