All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
MlcpGaussSeidelSolver.h
Go to the documentation of this file.
1 // This file is a part of the OpenSurgSim project.
2 // Copyright 2013, SimQuest Solutions Inc.
3 //
4 // Licensed under the Apache License, Version 2.0 (the "License");
5 // you may not use this file except in compliance with the License.
6 // You may obtain a copy of the License at
7 //
8 // http://www.apache.org/licenses/LICENSE-2.0
9 //
10 // Unless required by applicable law or agreed to in writing, software
11 // distributed under the License is distributed on an "AS IS" BASIS,
12 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 // See the License for the specific language governing permissions and
14 // limitations under the License.
15 
16 #ifndef SURGSIM_MATH_MLCPGAUSSSEIDELSOLVER_H
17 #define SURGSIM_MATH_MLCPGAUSSSEIDELSOLVER_H
18 
20 #include <Eigen/Core>
23 
24 namespace SurgSim
25 {
26 namespace Math
27 {
28 
48 {
49 public:
53  m_substepRatio(1.0),
56  m_verbose(false),
58  {
59  }
60 
61  MlcpGaussSeidelSolver(double epsilonConvergence, double contactTolerance, unsigned int maxIterations) :
62  m_epsilonConvergence(epsilonConvergence),
63  m_contactTolerance(contactTolerance),
64  m_substepRatio(1.0),
65  m_maxIterations(maxIterations),
67  m_verbose(false),
69  {
70  }
71 
73  {
74  }
75 
76 
77  bool solve(const MlcpProblem& problem, MlcpSolution* solution);
78 
79 
80  double getEpsilonConvergence() const
81  {
82  return m_epsilonConvergence;
83  }
84  void setEpsilonConvergence(double val)
85  {
87  }
88  double getContactTolerance() const
89  {
90  return m_contactTolerance;
91  }
92  void setContactTolerance(double val)
93  {
94  m_contactTolerance = val;
95  }
96  double getSubstepRatio() const
97  {
98  return m_substepRatio;
99  }
100  void setSubstepRatio(double val)
101  {
102  m_substepRatio = val;
103  }
104  unsigned int getMaxIterations() const
105  {
106  return m_maxIterations;
107  }
108  void setMaxIterations(unsigned int val)
109  {
110  m_maxIterations = val;
111  }
113  {
115  }
117  {
119  }
120  bool isVerbose() const
121  {
122  return m_verbose;
123  }
124  void setVerbose(bool val)
125  {
126  m_verbose = val;
127  }
128 
129 
131  {
132  return 1e-4;
133  }
134  static double defaultContactTolerance()
135  {
136  return 2e-5;
137  }
138  static int defaultMaxIterations()
139  {
140  return 30;
141  }
142 
143 private:
144  void computeEnforcementSystem(int n, const MlcpProblem::Matrix& A, int nbColumnInA,
145  const MlcpProblem::Vector& b,
146  const MlcpSolution::Vector& initialGuess_and_solution,
147  const MlcpProblem::Vector& frictionCoefs,
148  const std::vector<MlcpConstraintType>& constraintsType, double subStep,
149  int constraintID, int matrixEntryForConstraintID);
150 
151  void calculateConvergenceCriteria(int n, const MlcpProblem::Matrix& A, int nbColumnInA,
152  const MlcpProblem::Vector& b,
153  const MlcpSolution::Vector& initialGuess_and_solution,
154  const std::vector<MlcpConstraintType>& constraintsType,
155  double subStep,
156  double constraint_convergence_criteria[MLCP_NUM_CONSTRAINT_TYPES],
157  double& convergence_criteria,
158  bool& signoriniVerified, bool& signoriniValid);
159 
160  void doOneIteration(int n, const MlcpProblem::Matrix& A, int nbColumnInA, const MlcpProblem::Vector& b,
161  MlcpSolution::Vector* initialGuess_and_solution,
162  const MlcpProblem::Vector& frictionCoefs,
163  const std::vector<MlcpConstraintType>& constraintsType, double subStep,
164  double constraint_convergence_criteria[MLCP_NUM_CONSTRAINT_TYPES], double& convergence_criteria,
165  bool& signoriniVerified);
166 
167  void printViolationsAndConvergence(int n, const MlcpProblem::Matrix& A, int nbColumnInA,
168  const MlcpProblem::Vector& b,
169  const MlcpSolution::Vector& initialGuess_and_solution,
170  const std::vector<MlcpConstraintType>& constraintsType,
171  double subStep, double convergence_criteria,
172  bool signorini_verified, int nbLoop);
173 
174 
175  typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> Matrix;
176  typedef Eigen::Matrix<double, Eigen::Dynamic, 1> Vector;
177 
178 
182  unsigned int m_maxIterations;
184  bool m_verbose;
185 
189 };
190 
191 }; // namespace Math
192 }; // namespace SurgSim
193 
194 #endif // SURGSIM_MATH_MLCPGAUSSSEIDELSOLVER_H
void setEpsilonConvergence(double val)
Definition: MlcpGaussSeidelSolver.h:84
bool isCatchingExplodingConvergenceCriteria() const
Definition: MlcpGaussSeidelSolver.h:112
MlcpGaussSeidelSolver(double epsilonConvergence, double contactTolerance, unsigned int maxIterations)
Definition: MlcpGaussSeidelSolver.h:61
bool solve(const MlcpProblem &problem, MlcpSolution *solution)
MLCP_GaussSeidel::solve Resolution of a given MLCP (Gauss Seidel iterative solver) ...
Definition: MlcpGaussSeidelSolver.cpp:47
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > Matrix
Definition: MlcpGaussSeidelSolver.h:175
double getEpsilonConvergence() const
Definition: MlcpGaussSeidelSolver.h:80
A solver for mixed LCP problems using the Gauss-Seidel iterative method.
Definition: MlcpGaussSeidelSolver.h:47
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: MlcpProblem.h:63
Definition: MlcpConstraintType.h:44
void setContactTolerance(double val)
Definition: MlcpGaussSeidelSolver.h:92
virtual ~MlcpGaussSeidelSolver()
Definition: MlcpGaussSeidelSolver.h:72
unsigned int m_maxIterations
Definition: MlcpGaussSeidelSolver.h:182
Matrix m_lhsEnforcedLocalSystem
Definition: MlcpGaussSeidelSolver.h:187
void setCatchingExplodingConvergenceCriteria(bool val)
Definition: MlcpGaussSeidelSolver.h:116
bool m_catchExplodingConvergenceCriteria
Definition: MlcpGaussSeidelSolver.h:183
double m_substepRatio
Definition: MlcpGaussSeidelSolver.h:181
double m_epsilonConvergence
Definition: MlcpGaussSeidelSolver.h:179
void setSubstepRatio(double val)
Definition: MlcpGaussSeidelSolver.h:100
void setMaxIterations(unsigned int val)
Definition: MlcpGaussSeidelSolver.h:108
bool isVerbose() const
Definition: MlcpGaussSeidelSolver.h:120
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: MlcpSolution.h:35
MlcpGaussSeidelSolver()
Definition: MlcpGaussSeidelSolver.h:50
double getSubstepRatio() const
Definition: MlcpGaussSeidelSolver.h:96
static double defaultContactTolerance()
Definition: MlcpGaussSeidelSolver.h:134
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > Matrix
Definition: MlcpProblem.h:62
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: MlcpGaussSeidelSolver.h:176
The description of a solution to a mixed linear complementarity problem.
Definition: MlcpSolution.h:33
This class provides a solver interface for mixed linear complementarity problems. ...
Definition: MlcpSolver.h:30
int m_numEnforcedAtomicConstraints
Definition: MlcpGaussSeidelSolver.h:186
void calculateConvergenceCriteria(int n, const MlcpProblem::Matrix &A, int nbColumnInA, const MlcpProblem::Vector &b, const MlcpSolution::Vector &initialGuess_and_solution, const std::vector< MlcpConstraintType > &constraintsType, double subStep, double constraint_convergence_criteria[MLCP_NUM_CONSTRAINT_TYPES], double &convergence_criteria, bool &signoriniVerified, bool &signoriniValid)
Definition: MlcpGaussSeidelSolver.cpp:250
double getContactTolerance() const
Definition: MlcpGaussSeidelSolver.h:88
void computeEnforcementSystem(int n, const MlcpProblem::Matrix &A, int nbColumnInA, const MlcpProblem::Vector &b, const MlcpSolution::Vector &initialGuess_and_solution, const MlcpProblem::Vector &frictionCoefs, const std::vector< MlcpConstraintType > &constraintsType, double subStep, int constraintID, int matrixEntryForConstraintID)
Definition: MlcpGaussSeidelSolver.cpp:424
void setVerbose(bool val)
Definition: MlcpGaussSeidelSolver.h:124
Vector m_rhsEnforcedLocalSystem
Definition: MlcpGaussSeidelSolver.h:188
static double defaultEpsilonConvergence()
Definition: MlcpGaussSeidelSolver.h:130
double m_contactTolerance
Definition: MlcpGaussSeidelSolver.h:180
A description of an MLCP (mixed linear complementarity problem, or mixed LCP) system to be solved...
Definition: MlcpProblem.h:57
bool m_verbose
Definition: MlcpGaussSeidelSolver.h:184
static int defaultMaxIterations()
Definition: MlcpGaussSeidelSolver.h:138
void doOneIteration(int n, const MlcpProblem::Matrix &A, int nbColumnInA, const MlcpProblem::Vector &b, MlcpSolution::Vector *initialGuess_and_solution, const MlcpProblem::Vector &frictionCoefs, const std::vector< MlcpConstraintType > &constraintsType, double subStep, double constraint_convergence_criteria[MLCP_NUM_CONSTRAINT_TYPES], double &convergence_criteria, bool &signoriniVerified)
Definition: MlcpGaussSeidelSolver.cpp:735
unsigned int getMaxIterations() const
Definition: MlcpGaussSeidelSolver.h:104
void printViolationsAndConvergence(int n, const MlcpProblem::Matrix &A, int nbColumnInA, const MlcpProblem::Vector &b, const MlcpSolution::Vector &initialGuess_and_solution, const std::vector< MlcpConstraintType > &constraintsType, double subStep, double convergence_criteria, bool signorini_verified, int nbLoop)
Definition: MlcpGaussSeidelSolver.cpp:1601