mbsim 4.0.0 MBSim Kernel

lcp_reformulation_functions.h
1 /* Copyright (C) 2004-2012 MBSim Development Team
2 *
3 * This library is free software; you can redistribute it and/or
4 * modify it under the terms of the GNU Lesser General Public
5 * License as published by the Free Software Foundation; either
6 * version 2.1 of the License, or (at your option) any later version.
7 *
8 * This library is distributed in the hope that it will be useful,
9 * but WITHOUT ANY WARRANTY; without even the implied warranty of
10 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
11 * Lesser General Public License for more details.
12 *
13 * You should have received a copy of the GNU Lesser General Public
14 * License along with this library; if not, write to the Free Software
15 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
16 *
17 * Contact: martin.o.foerg@googlemail.com
18 */
19
20 #ifndef NUMERICSLCPREFORMULATEDFUNCTION_H_
21 #define NUMERICSLCPREFORMULATEDFUNCTION_H_
22
23 #include <mbsim/functions/function.h>
24
25 #include <mbsim/numerics/functions/newton_method_jacobian_functions.h>
26
27 namespace MBSim {
28
29 class LCPReformulationFunction : public Function<fmatvec::Vec(fmatvec::Vec)> {
30 public:
34 LCPReformulationFunction(const double &r_ = 10) :
35 r(r_) {
36 }
37
41 LCPReformulationFunction(const fmatvec::Vec &q_, const fmatvec::SqrMat &M_, const double &r_ = 10);
42
43 /*
44 * \brief destructor
45 */
46 ~LCPReformulationFunction() override;
47
48 /* INHERITED INTERFACE */
53 fmatvec::Vec operator()(const fmatvec::Vec &q) override = 0;
54 /***************************************************/
55
57 fmatvec::Vec getq() {
58 return q;
59 }
60 void setSystem(const fmatvec::SqrMat & M_, const fmatvec::Vec & q_) {
61 M <<= M_;
62 q <<= q_;
63 assert(M.rows() == q.rows());
64 NumberOfContacts = q.rows();
65 }
66 fmatvec::SqrMat getM() {
67 return M;
68 }
69 double getr() {
70 return r;
71 }
72 void setr(const double & r_) {
73 r = r_;
74 }
75 /*****************/
76
77 protected:
81 int NumberOfContacts;
82
86 fmatvec::Vec q;
87
91 fmatvec::SqrMat M;
92
96 double r;
97 };
98
99 class LCPNewtonReformulationFunction : public LCPReformulationFunction {
100 public:
104 LCPNewtonReformulationFunction() :
105 LCPReformulationFunction() {
106 }
107
114 LCPNewtonReformulationFunction(const fmatvec::Vec &q_, const fmatvec::SqrMat &M_, const double &r_ = 10);
115
119 ~LCPNewtonReformulationFunction() override;
120
121 /* INHERITED INTERFACE */
122 fmatvec::Vec operator()(const fmatvec::Vec &q) override;
123 /***************************************************/
124
125 protected:
126
127 };
128
129 class LCPFixpointReformulationFunction : public LCPReformulationFunction {
130 public:
134 LCPFixpointReformulationFunction() :
135 LCPReformulationFunction() {
136 }
137
144 LCPFixpointReformulationFunction(const fmatvec::Vec &q_, const fmatvec::SqrMat &M_, const double &r_ = 10);
145
149 ~LCPFixpointReformulationFunction() override;
150
151 /* INHERITED INTERFACE */
152 fmatvec::Vec operator()(const fmatvec::Vec &q) override;
153 /***************************************************/
154 };
155
156 class LinearComplementarityJacobianFunction : public NewtonJacobianFunction {
157 public:
161 LinearComplementarityJacobianFunction();
162
163 /*
164 * \brief destructor
165 */
166 ~LinearComplementarityJacobianFunction() override;
167
168 fmatvec::SqrMat operator ()(const fmatvec::Vec & x) override;
169
170 void setFunction(Function<fmatvec::Vec(fmatvec::Vec)> *function_) override;
171
172 void updateJacobian(const fmatvec::Vec & x);
173
174 protected:
178 fmatvec::SqrMat J;
179
180 };
181 }
182 #endif //NUMERICSLCPREFORMULATEDFUNCTION_H_
MBSim::LCPFixpointReformulationFunction
Definition: lcp_reformulation_functions.h:129
MBSim::LCPReformulationFunction::NumberOfContacts
int NumberOfContacts
Number of possible contact points (= dimension of the LCP)
Definition: lcp_reformulation_functions.h:81
MBSim::LCPReformulationFunction::getq
fmatvec::Vec getq()
Definition: lcp_reformulation_functions.h:57
MBSim::LCPNewtonReformulationFunction::operator()
fmatvec::Vec operator()(const fmatvec::Vec &q) override
Definition: lcp_reformulation_functions.cc:48
MBSim::LinearComplementarityJacobianFunction
Definition: lcp_reformulation_functions.h:156
MBSim::LCPReformulationFunction::q
fmatvec::Vec q
vector of all rigid body gaps
Definition: lcp_reformulation_functions.h:86
MBSim::LCPFixpointReformulationFunction::operator()
fmatvec::Vec operator()(const fmatvec::Vec &q) override
Definition: lcp_reformulation_functions.cc:95
MBSim::NewtonJacobianFunction
base class for square Jacobians used for the newton method
Definition: newton_method_jacobian_functions.h:31
MBSim::LCPReformulationFunction::operator()
fmatvec::Vec operator()(const fmatvec::Vec &q) override=0
MBSim::LCPReformulationFunction::r
double r
parameter for the prox-function (r>0)
Definition: lcp_reformulation_functions.h:96
MBSim::LCPNewtonReformulationFunction::LCPNewtonReformulationFunction
LCPNewtonReformulationFunction()
standard constructor
Definition: lcp_reformulation_functions.h:104
MBSim::LCPNewtonReformulationFunction::~LCPNewtonReformulationFunction
~LCPNewtonReformulationFunction() override
destructor
MBSim::LCPNewtonReformulationFunction
Definition: lcp_reformulation_functions.h:99
MBSim::Function
Definition: contact_kinematics.h:32
MBSim::LinearComplementarityJacobianFunction::LinearComplementarityJacobianFunction
LinearComplementarityJacobianFunction()
constructor
Definition: lcp_reformulation_functions.cc:131
MBSim::LCPReformulationFunction::LCPReformulationFunction
LCPReformulationFunction(const double &r_=10)
standard constructor
Definition: lcp_reformulation_functions.h:34
MBSim::LCPFixpointReformulationFunction::~LCPFixpointReformulationFunction
~LCPFixpointReformulationFunction() override
destructor
MBSim::LCPReformulationFunction::M
fmatvec::SqrMat M
Influence matrix for the contacts.
Definition: lcp_reformulation_functions.h:91
MBSim::LCPReformulationFunction
Definition: lcp_reformulation_functions.h:29
MBSim::LCPFixpointReformulationFunction::LCPFixpointReformulationFunction
LCPFixpointReformulationFunction()
standard constructor
Definition: lcp_reformulation_functions.h:134
MBSim::LinearComplementarityJacobianFunction::J
fmatvec::SqrMat J
Jacobian function (stays saved to save computation time, as some parts stay constant)
Definition: lcp_reformulation_functions.h:178
MBSim
namespace MBSim
Definition: bilateral_constraint.cc:30