20 #ifndef _DYNAMIC_SYSTEM_SOLVER_H_
21 #define _DYNAMIC_SYSTEM_SOLVER_H_
23 #include "mbsim/group.h"
24 #include "fmatvec/sparse_matrix.h"
54 enum Solver { FixedPointTotal, FixedPointSingle, GaussSeidel, LinearEquations, RootFinding };
64 enum LinAlg { LUDecomposition, LevenbergMarquardt, PseudoInverse };
124 void setTruncateSimulationFiles(
bool trunc) { truncateSimulationFiles=trunc; }
143 virtual void updateT();
144 virtual void updateh(
int i=0);
145 virtual void updateM();
147 virtual void updatezd();
163 virtual void updaterdt();
164 virtual void updatewb();
165 virtual void updateg();
166 virtual void updategd();
167 virtual void updateW(
int j=0);
168 virtual void updateV(
int j=0);
169 virtual void updatebc();
170 virtual void updatebi();
171 virtual void updatela();
172 virtual void updateLa();
177 virtual std::string
getType()
const {
return "DynamicSystemSolver"; }
185 void setImpactSolver(
Solver solver_) { impactSolver = solver_; }
187 const Solver& getImpactSolver() {
return impactSolver; }
188 void setTermination(
bool term_) {
term = term_; }
190 void setMaxIter(
int iter) {
maxIter = iter; }
191 void setHighIter(
int iter) { highIter = iter; }
192 void setNumJacProj(
bool numJac_) {
numJac = numJac_; }
193 void setMaxDampingSteps(
int maxDSteps) { maxDampingSteps = maxDSteps; }
194 void setLevenbergMarquardtParam(
double lmParm_) {
lmParm = lmParm_; }
197 void setUseOldla(
bool flag) {
useOldla = flag; }
198 void setDecreaseLevels(
const fmatvec::VecInt &decreaseLevels_) {
decreaseLevels = decreaseLevels_; }
199 void setCheckTermLevels(
const fmatvec::VecInt &checkTermLevels_) {
checkTermLevels = checkTermLevels_; }
200 void setCheckGSize(
bool checkGSize_) {
checkGSize = checkGSize_; }
203 double& getTime() {
return t; }
204 const double& getTime()
const {
return t; }
205 void setTime(
double t_) {
t = t_; }
207 double getStepSize()
const {
return dt; }
208 void setStepSize(
double dt_) {
dt = dt_; }
216 const fmatvec::SqrMat& getG(
bool check=
true)
const { assert((not check) or (not updG));
return G; }
217 const fmatvec::SparseMat& getGs(
bool check=
true)
const { assert((not check) or (not updG));
return Gs; }
218 const fmatvec::Vec& getbc(
bool check=
true)
const { assert((not check) or (not updbc));
return bc; }
219 const fmatvec::Vec& getbi(
bool check=
true)
const { assert((not check) or (not updbi));
return bi; }
221 fmatvec::SqrMat& getG(
bool check=
true) { assert((not check) or (not updG));
return G; }
222 fmatvec::SparseMat& getGs(
bool check=
true) { assert((not check) or (not updG));
return Gs; }
223 fmatvec::Vec& getbc(
bool check=
true) { assert((not check) or (not updbc));
return bc; }
224 fmatvec::Vec& getbi(
bool check=
true) { assert((not check) or (not updbi));
return bi; }
227 const fmatvec::Vec& evaldq() {
if(upddq) updatedq();
return dq; }
228 const fmatvec::Vec& evaldu() {
if(upddu) updatedu();
return du; }
229 const fmatvec::Vec& evaldx() {
if(upddx) updatedx();
return dx; }
232 const fmatvec::SparseMat& evalGs() {
if(updG)
updateG();
return Gs; }
234 const fmatvec::Vec& evalbi() {
if(updbi) updatebi();
return bi; }
238 const fmatvec::Vec& evalLa() {
if(updLa) updateLa();
return La; }
243 const fmatvec::Vec& getLaParent()
const {
return LaParent; }
250 int getMaxIter() {
return maxIter;}
251 int getIterC() {
return iterc;}
252 int getIterI() {
return iteri;}
404 static void installSignalHandler();
416 static void sigSegfaultHandler(
int);
418 void checkExitRequest();
421 void setPartialEventDrivenSolver(
bool peds_) {
peds = peds_; }
428 void writez(std::string fileName,
bool formatH5=
true);
434 void readz0(std::string fileName);
436 virtual void initializeUsingXML(xercesc::DOMElement *element);
489 void setAlwaysConsiderContact(
bool alwaysConsiderContact_) {alwaysConsiderContact = alwaysConsiderContact_;}
491 void setInverseKinetics(
bool inverseKinetics_) {inverseKinetics = inverseKinetics_;}
493 void setInitialProjection(
bool initialProjection_) {initialProjection = initialProjection_;}
495 void setUseConstraintSolverForPlot(
bool useConstraintSolverForPlot_) {useConstraintSolverForPlot = useConstraintSolverForPlot_;}
496 bool getUseConstraintSolverForPlot()
const {
return useConstraintSolverForPlot; }
503 void setRootID(
int ID) {rootID = ID;}
504 int getRootID()
const {
return rootID;}
506 void resetUpToDate();
508 bool getUpdateT() {
return updT; }
509 bool getUpdateM() {
return updM; }
510 bool getUpdateLLM() {
return updLLM; }
511 bool getUpdateh(
int j) {
return updh[j]; }
512 bool getUpdater(
int j) {
return updr[j]; }
513 bool getUpdaterdt() {
return updrdt; }
514 bool getUpdateW(
int j) {
return updW[j]; }
515 bool getUpdateV(
int j) {
return updV[j]; }
516 bool getUpdatewb() {
return updwb; }
517 bool getUpdateg() {
return updg; }
518 bool getUpdategd() {
return updgd; }
519 bool getUpdatela() {
return updla; }
520 bool getUpdateLa() {
return updLa; }
521 bool getUpdatezd() {
return updzd; }
522 bool getUpdatedq() {
return upddq; }
523 bool getUpdatedu() {
return upddu; }
524 bool getUpdatedx() {
return upddx; }
525 void setUpdatela(
bool updla_) { updla = updla_; }
526 void setUpdateLa(
bool updLa_) { updLa = updLa_; }
527 void setUpdatebi(
bool updbi_) { updbi = updbi_; }
528 void setUpdatebc(
bool updbc_) { updbc = updbc_; }
529 void setUpdatezd(
bool updzd_) { updzd = updzd_; }
570 virtual void updatedq();
571 virtual void updatedu();
572 virtual void updatedx();
574 virtual void updateStopVector();
695 fmatvec::SparseMat
Gs;
720 int maxIter, highIter, maxDampingSteps, iterc, iteri;
826 bool alwaysConsiderContact;
827 bool inverseKinetics;
828 bool initialProjection;
829 bool useConstraintSolverForPlot;
835 double gTol, gdTol, gddTol, laTol, LaTol;
837 bool updT, updh[2], updr[2], updrdt, updM, updLLM, updW[2], updV[2], updwb, updg, updgd, updG, updbc, updbi, updsv, updzd, updla, updLa, upddq, upddu, upddx;
872 bool truncateSimulationFiles;
879 std::vector<DynamicSystem*> dynamicsystemPreReorganize;
bool useOldla
flag if contac force parameter of last time step should be used
Definition: dynamic_system_solver.h:755
void updateVRef(const fmatvec::Mat &ref, int i=0)
references to condensed contact force direction matrix of dynamic system parent
Definition: dynamic_system_solver.h:563
void updatewbRef(const fmatvec::Vec &ref)
references to TODO of dynamic system parent
Definition: dynamic_system_solver.h:549
int solveConstraintsLinearEquations()
solution of contact equations with Cholesky decomposition
Definition: dynamic_system_solver.cc:885
solver-interface for dynamic systems
Definition: solver.h:38
virtual int solveConstraintsRootFinding()
solve contact equations with Newton scheme
Definition: dynamic_system_solver.cc:544
virtual void setlaTol(double tol)
Definition: dynamic_system.cc:1302
fmatvec::Vec gParent
relative distances
Definition: dynamic_system_solver.h:642
void updatezRef(const fmatvec::Vec &ext)
references to external state
Definition: dynamic_system_solver.cc:1347
double t
time
Definition: dynamic_system_solver.h:582
bool checkGSize
boolean if force action matrix should be resized in each step
Definition: dynamic_system_solver.h:775
int solveImpactsLinearEquations()
solution of contact equations with Cholesky decomposition on velocity level
Definition: dynamic_system_solver.cc:890
fmatvec::Vec rParent[2]
nonsmooth right hand side
Definition: dynamic_system_solver.h:669
void initLa()
load contact impulses for use as starting value
Definition: dynamic_system_solver.cc:1159
bool term
boolean to check for termination of contact equations solution
Definition: dynamic_system_solver.h:715
void readz0(std::string fileName)
reads state from a file
Definition: dynamic_system_solver.cc:1339
bool dropContactInfo
flag if contact matrices for debugging should be dropped in no-convergence case
Definition: dynamic_system_solver.h:750
double tolProj
Tolerance for projection of generalized position.
Definition: dynamic_system_solver.h:807
solver interface for modelling and simulation of dynamic systems
Definition: dynamic_system_solver.h:48
void initla()
load contact forces for use as starting value
Definition: dynamic_system_solver.cc:1149
fmatvec::SparseMat Gs
sparse mass action matrix
Definition: dynamic_system_solver.h:695
Strategy strategy
relaxarion strategy for solution of fixed-point scheme
Definition: dynamic_system_solver.h:735
void setgddTol(double tol)
Definition: dynamic_system_solver.h:459
fmatvec::VecInt checkTermLevels
TODO.
Definition: dynamic_system_solver.h:770
DynamicSystemSolver(const std::string &name="")
constructor
Definition: dynamic_system_solver.cc:62
Solver contactSolver
solver for contact equations and impact equations
Definition: dynamic_system_solver.h:730
fmatvec::Vec bc
TODO.
Definition: dynamic_system_solver.h:710
void updateWRef(const fmatvec::Mat &ref, int i=0)
references to contact force direction matrix of dynamic system parent
Definition: dynamic_system_solver.h:556
void setgdTol(double tol)
Definition: dynamic_system_solver.h:454
virtual void plot()
plots time dependent data
Definition: dynamic_system.cc:330
bool stopIfNoConvergence
flag if the contact equations should be stopped if there is no convergence
Definition: dynamic_system_solver.h:745
virtual const fmatvec::Vec & shift()
update for event driven integrator for event
Definition: dynamic_system_solver.cc:1497
virtual void updateWRef(const fmatvec::Mat &ref, int i=0)
references to contact force direction matrix of dynamic system parent
Definition: dynamic_system.cc:824
void updategRef(const fmatvec::Vec &ref)
references to relative distances of dynamic system parent
Definition: dynamic_system_solver.h:537
void setgTol(double tol)
Definition: dynamic_system_solver.h:449
fmatvec::Mat TParent
matrix of linear relation between differentiated positions and velocities
Definition: dynamic_system_solver.h:602
fmatvec::Vec sParent
TODO.
Definition: dynamic_system_solver.h:632
fmatvec::Vec u
velocities, differentiated velocities, initial velocities
Definition: dynamic_system.h:836
void plot()
plots time dependent data
Definition: dynamic_system_solver.cc:1662
fmatvec::VecInt LinkStatusRegParent
status vector of single valued links
Definition: dynamic_system_solver.h:690
void getLinkStatusReg(fmatvec::VecInt &LinkStatusRegExt)
Definition: dynamic_system_solver.cc:1019
fmatvec::Vec gdParent
relative velocities
Definition: dynamic_system_solver.h:647
void projectGeneralizedVelocities(int mode)
drift projection for positions
Definition: dynamic_system_solver.cc:1104
fmatvec::VecInt LinkStatusParent
status vector of set valued links with piecewise link equation (which piece is valid) ...
Definition: dynamic_system_solver.h:684
virtual void updateLLM()
compute Cholesky decomposition of mass matrix TODO necessary?
Definition: dynamic_system_solver.cc:814
group ingredients do not depend on each other
Definition: group.h:35
LinAlg
linear algebra for Newton scheme in solution of contact equations
Definition: dynamic_system_solver.h:64
virtual void closePlot()
closes plot file
Definition: dynamic_system_solver.cc:862
unsigned int flushCount
counts plot-calls until files to be flushed TODO
Definition: dynamic_system_solver.h:802
std::string getSolverInfo()
Definition: dynamic_system_solver.cc:1216
virtual void setgTol(double tol)
Definition: dynamic_system.cc:1281
basic class of MBSim mainly for plotting
Definition: element.h:58
static void sigInterruptHandler(int)
handler for user interrupt signal
Definition: dynamic_system_solver.cc:1285
double lmParm
Levenberg-Marquard parameter.
Definition: dynamic_system_solver.h:725
void addElement(Element *element_)
Definition: dynamic_system_solver.cc:1177
virtual void setLaTol(double tol)
Definition: dynamic_system.cc:1309
fmatvec::Vec rFactorParent
relaxation parameters for contact equations
Definition: dynamic_system_solver.h:627
fmatvec::VecInt decreaseLevels
decreasing relaxation factors is done in levels containing the number of contact iterations as condit...
Definition: dynamic_system_solver.h:765
Strategy
relaxation strategies in solution of contact equations
Definition: dynamic_system_solver.h:59
virtual void updatelaInverseKinetics()
update inverse kinetics constraint forces
Definition: dynamic_system_solver.cc:1590
void setStopIfNoConvergence(bool flag, bool dropInfo=false)
Definition: dynamic_system_solver.h:396
virtual void updategdRef(const fmatvec::Vec &ref)
references to relative velocities of dynamic system parent
Definition: dynamic_system.cc:786
virtual void updater(int j=0)
update smooth link force law
Definition: dynamic_system_solver.cc:819
void dropContactMatrices()
Definition: dynamic_system_solver.cc:1255
void initialize()
Initialize the system.
Definition: dynamic_system_solver.cc:82
virtual void updateG()
updates mass action matrix
Definition: dynamic_system_solver.cc:895
bool numJac
flag if Jacobian for Newton scheme should be calculated numerically
Definition: dynamic_system_solver.h:760
int(DynamicSystemSolver::* solveImpacts_)()
function pointer for election of prox-solver for impact equations on velocity level ...
Definition: dynamic_system_solver.h:271
unsigned int flushEvery
flushes all hdf5-files every x-times the plot-routine is called TODO
Definition: dynamic_system_solver.h:796
fmatvec::Mat WParent[2]
contact force directions
Definition: dynamic_system_solver.h:607
int warnLevel
level for warning output (0-2)
Definition: dynamic_system_solver.h:785
virtual void updategRef(const fmatvec::Vec &ref)
references to relative distances of dynamic system parent
Definition: dynamic_system.cc:779
double evalPotentialEnergy()
compute potential energy of entire dynamic system change? TODO
Definition: dynamic_system_solver.cc:1164
class for tree-structured mechanical systems with recursive and flat memory mechanism ...
Definition: graph.h:37
InitStage
The stages of the initialization.
Definition: element.h:97
void setProjectionTolerance(double tol)
set tolerance for projection of generalized position
Definition: dynamic_system_solver.h:444
int(DynamicSystemSolver::* solveConstraints_)()
Definition: dynamic_system_solver.h:264
Element * getElement(const std::string &name)
Definition: dynamic_system_solver.cc:1189
bool positionDriftCompensationNeeded(double gmax)
check if drift compensation on position level is needed
Definition: dynamic_system_solver.cc:1027
fmatvec::VecInt jsvParent
boolean evaluation of stopvector
Definition: dynamic_system_solver.h:679
void savela()
save contact forces for use as starting value in next time step
Definition: dynamic_system_solver.cc:1144
virtual int solveImpactsRootFinding()
solve impact equations with Newton scheme on velocity level
Definition: dynamic_system_solver.cc:620
LinAlg linAlg
linear system solver used for Newton scheme in contact equations
Definition: dynamic_system_solver.h:740
virtual void setgdTol(double tol)
Definition: dynamic_system.cc:1288
virtual void updateVRef(const fmatvec::Mat &ref, int i=0)
references to condensed contact force direction matrix of dynamic system parent
Definition: dynamic_system.cc:845
void projectGeneralizedPositions(int mode, bool fullUpdate=false)
drift projection for positions
Definition: dynamic_system_solver.cc:1053
fmatvec::SymMat LLMParent
Cholesky decomposition of mass matrix.
Definition: dynamic_system_solver.h:597
std::string name
name of element
Definition: element.h:298
virtual void checkImpactsForTermination()
validate force laws concerning given tolerances on velocity level
Definition: dynamic_system_solver.cc:706
int limitGSize
TODO.
Definition: dynamic_system_solver.h:780
virtual int solveConstraintsFixpointSingle()
solve contact equations with single step fixed point scheme
Definition: dynamic_system_solver.cc:440
virtual void updaterFactors()
update relaxation factors for contact equations
Definition: dynamic_system_solver.cc:1371
virtual int solveImpactsFixpointSingle()
solve impact equations with single step fixed point scheme on velocity level
Definition: dynamic_system_solver.cc:472
static bool exitRequest
boolean signal evaluation for end integration set by user
Definition: dynamic_system_solver.h:855
fmatvec::SymMat MParent
mass matrix
Definition: dynamic_system_solver.h:592
bool peds
TODO, flag for occuring impact and sticking in event driven solver.
Definition: dynamic_system_solver.h:790
fmatvec::Vec hParent[2]
smooth, smooth with respect to objects, smooth with respect to links right hand side ...
Definition: dynamic_system_solver.h:664
void constructor()
set plot feature default values
Definition: dynamic_system_solver.cc:1393
fmatvec::SqrMat G
mass action matrix
Definition: dynamic_system_solver.h:705
double dt
step size
Definition: dynamic_system_solver.h:587
void setlaTol(double tol)
Definition: dynamic_system_solver.h:464
void saveLa()
save contact impulses for use as starting value in next time step
Definition: dynamic_system_solver.cc:1154
bool integratorExitRequest
boolean signal evaluation for end integration set by program
Definition: dynamic_system_solver.h:850
fmatvec::Vec wbParent
TODO.
Definition: dynamic_system_solver.h:617
void addToGraph(Graph *graph, fmatvec::SqrMat &A, int i, std::vector< Element * > &objList)
adds list of objects to tree
Definition: dynamic_system_solver.cc:1484
virtual void checkConstraintsForTermination()
validate force laws concerning given tolerances
Definition: dynamic_system_solver.cc:695
void updategdRef(const fmatvec::Vec &ref)
references to relative velocities of dynamic system parent
Definition: dynamic_system_solver.h:543
void init(InitStage stage)
plots time series header
Definition: dynamic_system_solver.cc:101
fmatvec::Vec la
contact force parameters
Definition: dynamic_system.h:856
int maxIter
maximum number of contact iterations, high number of contact iterations for warnings, maximum number of damping steps for Newton scheme
Definition: dynamic_system_solver.h:720
void getLinkStatus(fmatvec::VecInt &LinkStatusExt)
Definition: dynamic_system_solver.cc:1011
Solver
solver for contact equations
Definition: dynamic_system_solver.h:54
fmatvec::Vec zParent
state
Definition: dynamic_system_solver.h:652
virtual int solveImpactsGaussSeidel()
solve impact equations with Gauss-Seidel scheme on velocity level
Definition: dynamic_system_solver.cc:524
fmatvec::Vec laParent
contact force parameters
Definition: dynamic_system_solver.h:622
virtual std::string getType() const
Definition: dynamic_system_solver.h:177
fmatvec::Vec resParent
residuum of contact equations
Definition: dynamic_system_solver.h:637
void setLaTol(double tol)
Definition: dynamic_system_solver.h:469
void setFlushEvery(unsigned int every)
set the number of plot-routine-calls after which all hdf5-files will be flushed
Definition: dynamic_system_solver.h:487
virtual ~DynamicSystemSolver()
destructor
Definition: dynamic_system_solver.cc:72
bool velocityDriftCompensationNeeded(double gdmax)
check if drift compensation on velocity level is needed
Definition: dynamic_system_solver.cc:1040
fmatvec::Vec svParent
stopvector (rootfunctions for event driven integration
Definition: dynamic_system_solver.h:674
fmatvec::SqrMat Jprox
JACOBIAN of contact equations for Newton scheme.
Definition: dynamic_system_solver.h:700
fmatvec::Mat VParent[2]
condensed contact force directions
Definition: dynamic_system_solver.h:612
void writez(std::string fileName, bool formatH5=true)
writes state to a file
Definition: dynamic_system_solver.cc:1319
virtual int solveConstraintsGaussSeidel()
solve contact equations with Gauss-Seidel scheme
Definition: dynamic_system_solver.cc:504
static void sigAbortHandler(int)
handler for abort signals
Definition: dynamic_system_solver.cc:1290
fmatvec::SymMat M
mass matrix
Definition: dynamic_system.h:821
void decreaserFactors()
decrease relaxation factors if mass action matrix is not diagonal dominant
Definition: dynamic_system_solver.cc:990
void computeInitialCondition()
compute initial condition for links for event driven integrator
Definition: dynamic_system_solver.cc:868
void updatezdRef(const fmatvec::Vec &ext)
references to differentiated external state
Definition: dynamic_system_solver.cc:1358
bool READZ0
is a state read from a file
Definition: dynamic_system_solver.h:860
virtual void updatewbRef(const fmatvec::Vec &ref)
references to TODO of dynamic system parent
Definition: dynamic_system.cc:817
virtual void setgddTol(double tol)
Definition: dynamic_system.cc:1295
fmatvec::Vec zdParent
differentiated state
Definition: dynamic_system_solver.h:657
double evalKineticEnergy()
compute kinetic energy of entire dynamic system
Definition: dynamic_system_solver.h:366