dynamic_system_solver.h

/* Copyright (C) 2004-2018 MBSim Development Team
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301 USA
 *
 * Contact: martin.o.foerg@googlemail.com
 */
 
#ifndef _DYNAMIC_SYSTEM_SOLVER_H_
#define _DYNAMIC_SYSTEM_SOLVER_H_
 
#include "mbsim/group.h"
#include "fmatvec/sparse_matrix.h"
#include "mbsim/functions/function.h"
#include "mbsim/environment.h"
 
#include <atomic>
 
namespace MBSim {
 
  class Graph;
  class MBSimEnvironment;
  class MultiDimNewtonMethod;
  class ConstraintResiduum;
  class ConstraintJacobian;
 
  struct StateTable {
    std::string name;
    char label;
    int number;
    StateTable() = default;
    StateTable(const std::string &name_, char label_, int number_) : name(name_), label(label_), number(number_) { }
  };
 
  class DynamicSystemSolver : public Group {
 
    class Residuum : public MBSim::Function<fmatvec::Vec(fmatvec::Vec)> {
      public:
        Residuum(MBSim::DynamicSystemSolver *sys_) : sys(sys_) { }
        fmatvec::Vec operator()(const fmatvec::Vec &z);
      private:
        MBSim::DynamicSystemSolver *sys;
    };
 
    public:
 
      enum Solver { fixedpoint, GaussSeidel, direct, rootfinding, unknownSolver, directNonlinear };
 
      DynamicSystemSolver(const std::string &name="");
 
      ~DynamicSystemSolver() override;
 
      void initialize();
 
      /*
       * If true (the default) then
       * the simulation output files (h5 files) are deleted/truncated/regenerated.
       * If false, then these files left are untouched from a previous run.
       * This is useful to regenerate the
       * e.g. OpenMBV XML files without doing a new integration.
       */
      void setTruncateSimulationFiles(bool trunc) { truncateSimulationFiles=trunc; }
 
      /* INHERITED INTERFACE OF GROUP */
      void init(InitStage stage, const InitConfigSet &config) override;
      using Group::plot;
      /***************************************************/
 
      /* INHERITED INTERFACE OF DYNAMICSYSTEM */
      int solveConstraintsFixpointSingle() override;
      int solveImpactsFixpointSingle() override;
      int solveConstraintsGaussSeidel() override;
      int solveImpactsGaussSeidel() override;
      int solveConstraintsRootFinding() override;
      int solveImpactsRootFinding() override;
      void checkConstraintsForTermination() override;
      void checkImpactsForTermination() override;
      /***************************************************/
 
      /* INHERITED INTERFACE OF OBJECTINTERFACE */
      void updateT() override;
      void updateh(int i=0) override;
      void updateM() override;
      void updateLLM() override;
      void updatezd() override;
      /***************************************************/
 
      /* INHERITED INTERFACE OF LINKINTERFACE */
 
      void updater(int j=0) override;
      void updateJrla(int j=0) override;
      virtual void updaterdt();
      void updatewb() override;
      void updateg() override;
      void updategd() override;
      void updateW(int j=0) override;
      void updateV(int j=0) override;
      virtual void updatebc();
      virtual void updatebi();
      virtual void updatela();
      virtual void updateLa();
      /***************************************************/
 
      /* INHERITED INTERFACE OF ELEMENT */
      /***************************************************/
 
      /* GETTER / SETTER */
 
      void setSmoothSolver(Solver solver_) { smoothSolver = solver_; }
      void setConstraintSolver(Solver solver_) { contactSolver = solver_; }
      void setImpactSolver(Solver solver_) { impactSolver = solver_; }
      const Solver& getSmoothSolver() { return smoothSolver; }
      const Solver& getConstraintSolver() { return contactSolver; }
      const Solver& getImpactSolver() { return impactSolver; }
      void setTermination(bool term_) { term = term_; }
      void setMaximumNumberOfIterations(int iter) { maxIter = iter; }
      void setHighNumberOfIterations(int iter) { highIter = iter; }
      void setNumericalJacobian(bool numJac_) { numJac = numJac_; }
      void setMaximumDampingSteps(int maxDSteps) { maxDampingSteps = maxDSteps; }
      void setLevenbergMarquardtParamater(double lmParm_) { lmParm = lmParm_; }
 
      void setUseOldla(bool flag) { useOldla = flag; }
      void setDecreaseLevels(const fmatvec::VecInt &decreaseLevels_) { decreaseLevels = decreaseLevels_; }
      void setCheckTermLevels(const fmatvec::VecInt &checkTermLevels_) { checkTermLevels = checkTermLevels_; }
      void setCheckGSize(bool checkGSize_) { checkGSize = checkGSize_; }
      void setLimitGSize(int limitGSize_) { limitGSize = limitGSize_; checkGSize = false; }
 
      double& getTime() { return t; }
      const double& getTime() const { return t; }
      void setTime(double t_) { t = t_; }
 
      double getStepSize() const { return dt; }
      void setStepSize(double dt_) { dt = dt_; }
 
      int getzSize() const { return zSize; }
 
      fmatvec::Vec& getState() { return z; }
      const fmatvec::Vec& getState() const { return z; }
      void setState(const fmatvec::Vec &z_) { z = z_; }
 
      const fmatvec::Vec& getzd(bool check=true) const { assert((not check) or (not updzd)); return zd; }
      void setzd(const fmatvec::Vec &zd_) { zd = zd_; }
 
      const fmatvec::SqrMat& getG(bool check=true) const { assert((not check) or (not updG)); return G; }
      const fmatvec::SparseMat& getGs(bool check=true) const { assert((not check) or (not updG)); return Gs; }
      const fmatvec::Vec& getbc(bool check=true) const { assert((not check) or (not updbc)); return bc; }
      const fmatvec::Vec& getbi(bool check=true) const { assert((not check) or (not updbi)); return bi; }
      const fmatvec::SqrMat& getJprox() const { return Jprox; }
      fmatvec::SqrMat& getG(bool check=true) { assert((not check) or (not updG)); return G; }
      fmatvec::SparseMat& getGs(bool check=true) { assert((not check) or (not updG)); return Gs; }
      fmatvec::Vec& getbc(bool check=true) { assert((not check) or (not updbc)); return bc; }
      fmatvec::Vec& getbi(bool check=true) { assert((not check) or (not updbi)); return bi; }
      fmatvec::SqrMat& getJprox() { return Jprox; }
      const fmatvec::Vec& getr(int j=0, bool check=true) const { assert((not check) or (not updr[j])); return r[j]; }
      fmatvec::Vec& getr(int j=0, bool check=true) { assert((not check) or (not updr[j])); return r[j]; }
 
      // Jacobian of dr/dla
      const fmatvec::Mat& getJrla(int j=0, bool check=true) const { assert((not check) or (not updJrla[j])); return Jrla[j]; }
      fmatvec::Mat& getJrla(int j=0, bool check=true) { assert((not check) or (not updJrla[j])); return Jrla[j]; }
 
      const fmatvec::Vec& evaldq() { if(upddq) updatedq(); return dq; }
      const fmatvec::Vec& evaldu() { if(upddu) updatedu(); return du; }
      const fmatvec::Vec& evaldx() { if(upddx) updatedx(); return dx; }
      const fmatvec::Vec& evalzd();
      const fmatvec::SqrMat& evalG() { if(updG) updateG(); return G; }
      const fmatvec::SparseMat& evalGs() { if(updG) updateG(); return Gs; }
      const fmatvec::Vec& evalbc() { if(updbc) updatebc(); return bc; }
      const fmatvec::Vec& evalbi() { if(updbi) updatebi(); return bi; }
      const fmatvec::Vec& evalsv();
      const fmatvec::Vec& evalz0();
      const fmatvec::Vec& evalla() { if(updla) updatela(); return la; }
      const fmatvec::Vec& evalLa() { if(updLa) updateLa(); return La; }
 
      fmatvec::Vec& getzParent() { return zParent; }
      fmatvec::Vec& getzdParent() { return zdParent; }
      fmatvec::Vec& getlaParent() { return laParent; }
      fmatvec::Vec& getLaParent() { return LaParent; }
//      const fmatvec::Vec& getzParent() const { return zParent; }
//      const fmatvec::Vec& getzdParent() const { return zdParent; }
//      const fmatvec::Mat& getWParent(int i=0) const { return WParent[i]; }
      fmatvec::Mat& getWParent(int i=0) { return WParent[i]; }
      fmatvec::Mat& getVParent(int i=0) { return VParent[i]; }
//      fmatvec::Vec& getlaParent() { return laParent; }
//      fmatvec::Vec& getLaParent() { return LaParent; }
//      const fmatvec::Vec& getgParent() const { return gParent; }
      fmatvec::Vec& getgParent() { return gParent; }
//      const fmatvec::Vec& getgdParent() const { return gdParent; }
      fmatvec::Vec& getgdParent() { return gdParent; }
      fmatvec::Vec& getresParent() { return resParent; }
      fmatvec::Vec& getrFactorParent() { return rFactorParent; }
 
      void resizezParent(int nz) { zParent.resize(nz); }
      void resizezdParent(int nz) { zdParent.resize(nz); }
 
      DynamicSystemSolver* getDynamicSystemSolver() { return this; }
      int getMaxIter()  {return maxIter;}
      int getHighIter()  {return highIter;}
      int getIterC()  {return iterc;}
      int getIterI()  {return iteri;}
      /***************************************************/
 
      void computeInitialCondition();
 
      int (DynamicSystemSolver::*solveSmooth_)();
 
      int (DynamicSystemSolver::*solveConstraints_)();
 
      int (DynamicSystemSolver::*solveImpacts_)();
 
      int solveConstraintsLinearEquations();
 
      int solveConstraintsNonlinearEquations();
 
      int solveImpactsLinearEquations();
 
      int solveImpactsNonlinearEquations();
 
      virtual void updateG();
 
      void decreaserFactors();
 
      virtual const fmatvec::Vec& shift(std::optional<std::reference_wrapper<bool>> &&velProjWasCalled={},
                                        std::optional<std::reference_wrapper<bool>> &&posProjWasCalled={});
 
      void getLinkStatus(fmatvec::VecInt &LinkStatusExt);
 
      void getLinkStatusReg(fmatvec::VecInt &LinkStatusRegExt);
 
      bool positionDriftCompensationNeeded(double gmax);
 
      bool velocityDriftCompensationNeeded(double gdmax);
 
      void projectGeneralizedPositions(int mode, bool fullUpdate=false);
 
      void projectGeneralizedVelocities(int mode);
 
      void savela();
 
      void initla();
 
      void saveLa();
 
      void initLa();
 
      void addElement(Element *element_);
 
      Element* getElement(const std::string &name);
 
      std::string getSolverInfo();
 
      void setStopIfNoConvergence(bool flag, bool dropInfo = false) { stopIfNoConvergence = flag; dropContactInfo=dropInfo; }
 
      bool getStopIfNoConvergence() { return stopIfNoConvergence; }
 
      void dropContactMatrices();
 
      // MBSim signal handler
#ifndef SWIG
      class SignalHandler {
        public:
          SignalHandler();
          ~SignalHandler();
        private:
          using SigHandle = void (*)(int);
          #ifndef _WIN32
          SigHandle oldSigHup;
          #endif
          SigHandle oldSigInt;
          SigHandle oldSigTerm;
      };
#endif
 
      void throwIfExitRequested() {
        if(exitRequest) {
          if(!exitRequestPrinted) {
            exitRequestPrinted = true;
            msg(fmatvec::Atom::Error)<<"User requested a exit (throw exception now)."<<std::endl;
          }
          throw std::runtime_error("Exception due to user requested exit.");
        }
      }
 
      bool exitRequested() {
        if(exitRequest)
          if(!exitRequestPrinted) {
            exitRequestPrinted = true;
            msg(fmatvec::Atom::Error)<<"User requested a exit (caller will handle this request now)."<<std::endl;
          }
        return exitRequest;
      }
 
      // TODO just for testing
      void setPartialEventDrivenSolver(bool peds_) { peds = peds_; }
 
      void writez(std::string fileName, bool formatH5=true);
 
      void writeStateTable(std::string fileName);
 
      void readz0(std::string fileName);
 
      void initializeUsingXML(xercesc::DOMElement *element) override;
 
      void setProjectionTolerance(double tol) { tolProj = tol; }
 
      void setLocalSolverTolerance(double tol) { tolLocalSolver = tol; }
      double getLocalSolverTolerance() { return tolLocalSolver; }
 
      void setDynamicSystemSolverTolerance(double tol) { tolDSS = tol; }
      double getDynamicSystemSolverTolerance() { return tolDSS; }
 
      void updatezRef(fmatvec::Vec &ext);
 
      void updatezdRef(fmatvec::Vec &ext);
 
      void setAlwaysConsiderContact(bool alwaysConsiderContact_) { alwaysConsiderContact = alwaysConsiderContact_; }
 
      void setInverseKinetics(bool inverseKinetics_) { inverseKinetics = inverseKinetics_; }
      bool getInverseKinetics() const { return inverseKinetics; }
 
      void setInitialProjection(bool initialProjection_) { initialProjection = initialProjection_; }
      bool getInitialProjection() const { return initialProjection; }
 
      void setDetermineEquilibriumState(bool determineEquilibriumState_) { determineEquilibriumState = determineEquilibriumState_; }
      bool getDetermineEquilibriumState() const { return determineEquilibriumState; }
 
      void setUseConstraintSolverForPlot(bool useConstraintSolverForPlot_) { useConstraintSolverForPlot = useConstraintSolverForPlot_; }
      bool getUseConstraintSolverForPlot() const { return useConstraintSolverForPlot; }
 
      fmatvec::Mat dhdq(int lb=0, int ub=0);
      fmatvec::Mat dhdu(int lb=0, int ub=0);
      fmatvec::Mat dhdx();
      fmatvec::Vec dhdt();
 
      void setRootID(int ID) {rootID = ID;}
      int getRootID() const {return rootID;}
 
      void resetUpToDate() override;
 
      bool getUpdateT() { return updT; }
      bool getUpdateM() { return updM; }
      bool getUpdateLLM() { return updLLM; }
      bool getUpdateh(int j) { return updh[j]; }
      bool getUpdater(int j) { return updr[j]; }
      bool getUpdateJrla(int j) { return updJrla[j]; }
      bool getUpdaterdt() { return updrdt; }
      bool getUpdateW(int j) { return updW[j]; }
      bool getUpdateV(int j) { return updV[j]; }
      bool getUpdatewb() { return updwb; }
      bool getUpdateg() { return updg; }
      bool getUpdategd() { return updgd; }
      bool getUpdatela() { return updla; }
      bool getUpdateLa() { return updLa; }
      bool getUpdatezd() { return updzd; }
      bool getUpdatedq() { return upddq; }
      bool getUpdatedu() { return upddu; }
      bool getUpdatedx() { return upddx; }
      void setUpdatela(bool updla_) { updla = updla_; }
      void setUpdateLa(bool updLa_) { updLa = updLa_; }
      void setUpdateG(bool updG_) { updG = updG_; }
      void setUpdatebi(bool updbi_) { updbi = updbi_; }
      void setUpdatebc(bool updbc_) { updbc = updbc_; }
      void setUpdatezd(bool updzd_) { updzd = updzd_; }
      void setUpdateW(bool updW_, int i=0) { updW[i] = updW_; }
 
      void updategRef(fmatvec::Vec &ref) override { Group::updategRef(ref); updg = true; }
 
      void updategdRef(fmatvec::Vec &ref) override { Group::updategdRef(ref); updgd = true; }
 
      void updatewbRef(fmatvec::Vec &ref) override { Group::updatewbRef(ref); updwb = true; }
 
      void updateWRef(fmatvec::Mat &ref, int i=0) override { Group::updateWRef(ref,i); updW[i] = true; }
 
      void updateVRef(fmatvec::Mat &ref, int i=0) override { Group::updateVRef(ref,i); updV[i] = true; }
 
      virtual void updatelaInverseKinetics();
 
      void updatedq() override;
      void updatedu() override;
      void updatedx() override;
 
      void updateStopVector() override;
 
      void updateInternalState();
 
      void plot() override;
 
      void addEnvironment(Environment* env);
 
      template<class Env>
      Env* getEnvironment();
 
      MBSimEnvironment* getMBSimEnvironment();
 
      std::vector<StateTable>& getStateTable() { return tabz; }
 
      void setCompressionLevel(int level) { compressionLevel=level; }
      void setChunkSize(int size) { chunkSize=size; }
      void setCacheSize(int size) { cacheSize=size; }
 
      void setqdequ(bool qdequ_) { qdequ = qdequ_; }
      bool getqdequ() { return qdequ; }
 
    protected:
      double t;
 
      double dt;
 
      int zSize;
 
      fmatvec::Vec z;
 
      fmatvec::Vec zd;
 
      fmatvec::SymMat MParent;
 
      fmatvec::SymMat LLMParent;
 
      fmatvec::Mat TParent;
 
      fmatvec::Mat WParent[2];
 
      fmatvec::Mat VParent[2];
 
      fmatvec::Vec wbParent;
 
      fmatvec::Vec laParent, LaParent;
 
      fmatvec::Vec rFactorParent;
 
      fmatvec::Vec sParent;
 
      fmatvec::Vec resParent;
 
      fmatvec::Vec curisParent, nextisParent;
 
      fmatvec::Vec gParent;
 
      fmatvec::Vec gdParent;
 
      fmatvec::Vec zParent;
 
      fmatvec::Vec zdParent;
 
      fmatvec::Vec dxParent, dqParent, duParent;
 
      fmatvec::Vec hParent[2];
 
      fmatvec::Vec rParent[2], rdtParent;
 
      fmatvec::Mat JrlaParent[2];
 
      fmatvec::Vec svParent;
 
      fmatvec::VecInt jsvParent;
 
      fmatvec::VecInt LinkStatusParent;
 
      fmatvec::VecInt LinkStatusRegParent;
 
      fmatvec::SparseMat Gs;
 
      fmatvec::SqrMat Jprox;
 
      fmatvec::SqrMat G;
 
      fmatvec::Vec bc, bi;
 
      bool term;
 
      int maxIter, highIter, maxDampingSteps, iterc, iteri;
 
      double lmParm;
 
      Solver smoothSolver, contactSolver, impactSolver;
 
      bool stopIfNoConvergence;
 
      bool dropContactInfo;
 
      bool useOldla;
 
      bool numJac;
 
      fmatvec::VecInt decreaseLevels;
 
      fmatvec::VecInt checkTermLevels;
 
      bool checkGSize;
 
      int limitGSize;
 
      bool peds;
 
      double tolProj;
 
      double tolLocalSolver { 1e-10 };
 
      double tolDSS { 1e-9 };
 
      fmatvec::Vec laInverseKineticsParent;
      fmatvec::Mat bInverseKineticsParent;
 
      fmatvec::Mat WInverseKineticsParent;
 
      bool alwaysConsiderContact;
      bool inverseKinetics;
      bool initialProjection;
      bool determineEquilibriumState;
      bool useConstraintSolverForPlot;
 
      fmatvec::Vec corrParent;
 
      int rootID;
 
      double gTol, gdTol, gddTol, laTol, LaTol;
 
      bool updT, updh[2], updr[2], updJrla[2], updrdt, updM, updLLM, updW[2], updV[2], updwb, updg, updgd, updG, updbc, updbi, updsv, updzd, updla, updLa, upddq, upddu, upddx;
 
      bool useSmoothSolver;
 
      bool qdequ;
 
      std::vector<StateTable> tabz;
 
      int compressionLevel { H5::File::getDefaultCompression() };
      int chunkSize { H5::File::getDefaultChunkSize() };
      int cacheSize { H5::File::getDefaultCacheSize() };
 
    private:
      static void sigInterruptHandler(int);
 
      void constructor();
 
      static std::atomic<bool> exitRequest;
      static_assert(decltype(exitRequest)::is_always_lock_free);
      bool exitRequestPrinted;
 
      bool READZ0;
 
      void addToGraph(Graph* graph, fmatvec::SqrMat &A, int i, std::vector<Element*> &objList);
 
      bool truncateSimulationFiles;
 
      // Holds the dynamic systems before the "reorganize hierarchy" takes place.
      // This is required since all elements of all other containers from DynamicSystem are readded to DynamicSystemSolver,
      // except this container (which is a "pure" container = no calculation is done in DynamicSystem)
      // However, we must hold this container until the dtor of DynamicSystemSolver is called to avoid the deallocation of other
      // elements hold by DynamicSystem elements (especially (currently only) hdf5File)
      std::vector<DynamicSystem*> dynamicsystemPreReorganize;
 
      double facSizeGs;
 
      std::vector<std::unique_ptr<Environment>> environments;
      MBSimEnvironment *mbsimEnvironment = nullptr;
 
      bool firstPlot { true };
 
      std::unique_ptr<MultiDimNewtonMethod> nonlinearConstraintNewtonSolver;
      std::unique_ptr<ConstraintResiduum> constraintResiduum;
      std::unique_ptr<ConstraintJacobian> constraintJacobian;
  };
 
  template<class Env>
  Env* DynamicSystemSolver::getEnvironment() {
    // get the Environment of type Env
    auto &reqType=typeid(Env);
    for(auto &e : environments) {
      auto &e_=*e;
      if(reqType==typeid(e_))
        return static_cast<Env*>(e.get());
    }
    auto newEnv=new Env;
    addEnvironment(newEnv);
    return newEnv;
  }
 
}
 
#endif /* _DYNAMIC_SYSTEM_SOLVER_H_ */