21 #ifndef _DYNAMIC_SYSTEM_SOLVER_H_
22 #define _DYNAMIC_SYSTEM_SOLVER_H_
24 #include "mbsim/group.h"
25 #include "sparse_matrix.h"
32 class ExtraDynamicInterface;
38 enum Solver { FixedPointTotal, FixedPointSingle, GaussSeidel, LinearEquations, RootFinding };
48 enum LinAlg { LUDecomposition, LevenbergMarquardt, PseudoInverse };
132 void setTruncateSimulationFiles(
bool trunc) { truncateSimulationFiles=trunc; }
151 virtual void updateh(
double t,
int i=0);
152 virtual void updateh0Fromh1(
double t);
153 virtual void updateW0FromW1(
double t);
154 virtual void updateV0FromV1(
double t);
155 virtual void updateM(
double t,
int i=0);
156 virtual void updateStateDependentVariables(
double t);
171 virtual void updater(
double t,
int j=0);
172 virtual void updatewb(
double t,
int j=0);
173 virtual void updateW(
double t,
int j=0);
174 virtual void updateV(
double t,
int j=0);
179 virtual std::string
getType()
const {
return "DynamicSystemSolver"; }
180 virtual void plot(
const fmatvec::Vec& z,
double t,
double dt=1);
181 virtual void plot2(
const fmatvec::Vec& z,
double t,
double dt=1);
202 void setImpactSolver(
Solver solver_) { impactSolver = solver_; }
203 const Solver& getImpactSolver() {
return impactSolver; }
204 void setTermination(
bool term_) {
term = term_; }
206 void setMaxIter(
int iter) {
maxIter = iter; }
207 void setHighIter(
int iter) { highIter = iter; }
208 void setNumJacProj(
bool numJac_) {
numJac = numJac_; }
209 void setMaxDampingSteps(
int maxDSteps) { maxDampingSteps = maxDSteps; }
210 void setLevenbergMarquardtParam(
double lmParm_) {
lmParm = lmParm_; }
213 void setUseOldla(
bool flag) {
useOldla = flag; }
214 void setDecreaseLevels(
const fmatvec::Vector<int> &decreaseLevels_) {
decreaseLevels = decreaseLevels_; }
215 void setCheckTermLevels(
const fmatvec::Vector<int> &checkTermLevels_) {
checkTermLevels = checkTermLevels_; }
216 void setCheckGSize(
bool checkGSize_) {
checkGSize = checkGSize_; }
219 const fmatvec::Matrix<fmatvec::Sparse, double>& getGs()
const {
return Gs; }
220 fmatvec::Matrix<fmatvec::Sparse, double>& getGs() {
return Gs; }
221 const fmatvec::SqrMat& getG()
const {
return G; }
222 fmatvec::SqrMat& getG() {
return G; }
223 const fmatvec::Vec& getb()
const {
return b; }
224 fmatvec::Vec& getb() {
return b; }
225 const fmatvec::SqrMat& getJprox()
const {
return Jprox; }
226 fmatvec::SqrMat& getJprox() {
return Jprox; }
228 const fmatvec::Mat& getWParent(
int i=0)
const {
return WParent[i]; }
229 const fmatvec::Mat& getVParent(
int i=0)
const {
return VParent[i]; }
230 const fmatvec::Vec& getlaParent()
const {
return laParent; }
231 const fmatvec::Vec& getgdParent()
const {
return gdParent; }
232 const fmatvec::Vec& getresParent()
const {
return resParent; }
233 const fmatvec::Vec& getrFactorParent()
const {
return rFactorParent; }
237 int getMaxIter() {
return maxIter;}
251 fmatvec::Vec
deltau(
const fmatvec::Vec &zParent,
double t,
double dt);
259 fmatvec::Vec
deltaq(
const fmatvec::Vec &zParent,
double t,
double dt);
267 fmatvec::Vec
deltax(
const fmatvec::Vec &zParent,
double t,
double dt);
272 void initz(fmatvec::Vec& z0);
301 void updateG(
double t,
int i=0);
315 void update(
const fmatvec::Vec &z,
double t,
int options=0);
323 virtual void shift(fmatvec::Vec& z,
const fmatvec::Vector<int>&
jsv,
double t);
331 virtual void zdot(
const fmatvec::Vec& z, fmatvec::Vec& zd,
double t);
338 virtual fmatvec::Vec
zdot(
const fmatvec::Vec &zParent,
double t);
346 virtual void getsv(
const fmatvec::Vec& z, fmatvec::Vec& svExt,
double t);
352 void getLinkStatus(fmatvec::Vector<int> &LinkStatusExt,
double t);
376 void savela(
double dt=1.0);
382 void initla(
double dt=1.0);
435 void setPartialEventDrivenSolver(
bool peds_) {
peds = peds_; }
442 void writez(std::string fileName,
bool formatH5=
true);
448 void readz0(std::string fileName);
450 virtual void initializeUsingXML(TiXmlElement *element);
481 void setAlwaysConsiderContact(
bool alwaysConsiderContact_) {alwaysConsiderContact = alwaysConsiderContact_;}
483 void setInverseKinetics(
bool inverseKinetics_) {inverseKinetics = inverseKinetics_;}
485 fmatvec::Mat dhdq(
double t,
int lb=0,
int ub=0);
486 fmatvec::Mat dhdu(
double t,
int lb=0,
int ub=0);
487 fmatvec::Mat dhdx(
double t);
488 fmatvec::Vec dhdt(
double t);
490 void setRootID(
int ID) {rootID = ID;}
491 int getRootID()
const {
return rootID;}
558 fmatvec::Vec udParent1;
598 fmatvec::Matrix<fmatvec::Sparse, double>
Gs;
744 fmatvec::Vec laInverseKineticsParent;
745 fmatvec::Mat bInverseKineticsParent;
750 fmatvec::Mat WInverseKineticsParent[2];
752 bool alwaysConsiderContact;
753 bool inverseKinetics;
755 fmatvec::Vec corrParent;
793 void addToGraph(
Graph* graph, fmatvec::SqrMat &A,
int i, std::vector<Object*> &objList);
795 bool truncateSimulationFiles;