DART  6.10.1
BoxedLcpConstraintSolver.hpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2011-2021, The DART development contributors
3  * All rights reserved.
4  *
5  * The list of contributors can be found at:
6  * https://github.com/dartsim/dart/blob/master/LICENSE
7  *
8  * This file is provided under the following "BSD-style" License:
9  * Redistribution and use in source and binary forms, with or
10  * without modification, are permitted provided that the following
11  * conditions are met:
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
19  * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
20  * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
21  * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
23  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
24  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
25  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
26  * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  */
32 
33 #ifndef DART_CONSTRAINT_BOXEDLCPCONSTRAINTSOLVER_HPP_
34 #define DART_CONSTRAINT_BOXEDLCPCONSTRAINTSOLVER_HPP_
35 
38 
39 namespace dart {
40 namespace constraint {
41 
43 {
44 public:
58  DART_DEPRECATED(6.8)
60  double timeStep,
61  BoxedLcpSolverPtr boxedLcpSolver = nullptr,
62  BoxedLcpSolverPtr secondaryBoxedLcpSolver = nullptr);
63 
67 
73 
81  BoxedLcpSolverPtr boxedLcpSolver,
82  BoxedLcpSolverPtr secondaryBoxedLcpSolver);
83 
88  void setBoxedLcpSolver(BoxedLcpSolverPtr lcpSolver);
89 
92 
95 
99 
100 protected:
101  // Documentation inherited.
102  void solveConstrainedGroup(ConstrainedGroup& group) override;
103 
106  // TODO(JS): Hold as unique_ptr because there is no reason to share. Make this
107  // change in DART 7 because it's API breaking change.
108 
111  // TODO(JS): Hold as unique_ptr because there is no reason to share. Make this
112  // change in DART 7 because it's API breaking change.
113 
115  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> mA;
116 
118  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>
120 
122  Eigen::VectorXd mX;
123 
125  Eigen::VectorXd mXBackup;
126 
128  Eigen::VectorXd mB;
129 
131  Eigen::VectorXd mBBackup;
132 
134  Eigen::VectorXd mW;
135 
137  Eigen::VectorXd mLo;
138 
140  Eigen::VectorXd mLoBackup;
141 
143  Eigen::VectorXd mHi;
144 
146  Eigen::VectorXd mHiBackup;
147 
149  Eigen::VectorXi mFIndex;
150 
152  Eigen::VectorXi mFIndexBackup;
153 
155  Eigen::VectorXi mOffset;
156 
157 #ifndef NDEBUG
158 private:
160  bool isSymmetric(std::size_t n, double* A);
161 
163  bool isSymmetric(
164  std::size_t n, double* A, std::size_t begin, std::size_t end);
165 
167  void print(
168  std::size_t n,
169  double* A,
170  double* x,
171  double* lo,
172  double* hi,
173  double* b,
174  double* w,
175  int* findex);
176 #endif
177 };
178 
179 } // namespace constraint
180 } // namespace dart
181 
182 #endif // DART_CONSTRAINT_BOXEDLCPCONSTRAINTSOLVER_HPP_
#define DART_DEPRECATED(version)
Definition: Deprecated.hpp:51
Definition: BoxedLcpConstraintSolver.hpp:43
BoxedLcpSolverPtr mSecondaryBoxedLcpSolver
Boxed LCP solver to be used when the primary solver failed.
Definition: BoxedLcpConstraintSolver.hpp:110
ConstBoxedLcpSolverPtr getSecondaryBoxedLcpSolver() const
Returns boxed LCP (BLCP) solver that is used when the primary solver failed.
Definition: BoxedLcpConstraintSolver.cpp:141
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > mA
Cache data for boxed LCP formulation.
Definition: BoxedLcpConstraintSolver.hpp:115
Eigen::VectorXd mLo
Cache data for boxed LCP formulation.
Definition: BoxedLcpConstraintSolver.hpp:137
bool isSymmetric(std::size_t n, double *A)
Return true if the matrix is symmetric.
Definition: BoxedLcpConstraintSolver.cpp:315
Eigen::VectorXi mFIndexBackup
Cache data for boxed LCP formulation.
Definition: BoxedLcpConstraintSolver.hpp:152
Eigen::VectorXd mHiBackup
Cache data for boxed LCP formulation.
Definition: BoxedLcpConstraintSolver.hpp:146
Eigen::VectorXi mOffset
Cache data for boxed LCP formulation.
Definition: BoxedLcpConstraintSolver.hpp:155
ConstBoxedLcpSolverPtr getBoxedLcpSolver() const
Returns boxed LCP (BLCP) solver.
Definition: BoxedLcpConstraintSolver.cpp:120
Eigen::VectorXd mBBackup
Cache data for boxed LCP formulation.
Definition: BoxedLcpConstraintSolver.hpp:131
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > mABackup
Cache data for boxed LCP formulation.
Definition: BoxedLcpConstraintSolver.hpp:119
Eigen::VectorXd mX
Cache data for boxed LCP formulation.
Definition: BoxedLcpConstraintSolver.hpp:122
BoxedLcpSolverPtr mBoxedLcpSolver
Boxed LCP solver.
Definition: BoxedLcpConstraintSolver.hpp:105
Eigen::VectorXd mW
Cache data for boxed LCP formulation.
Definition: BoxedLcpConstraintSolver.hpp:134
BoxedLcpConstraintSolver()
Constructos with default primary and secondary LCP solvers, which are Dantzig and PGS,...
Definition: BoxedLcpConstraintSolver.cpp:64
void print(std::size_t n, double *A, double *x, double *lo, double *hi, double *b, double *w, int *findex)
Print debug information.
Definition: BoxedLcpConstraintSolver.cpp:380
void setSecondaryBoxedLcpSolver(BoxedLcpSolverPtr lcpSolver)
Sets boxed LCP (BLCP) solver that is used when the primary solver failed.
Definition: BoxedLcpConstraintSolver.cpp:126
void setBoxedLcpSolver(BoxedLcpSolverPtr lcpSolver)
Sets boxed LCP (BLCP) solver.
Definition: BoxedLcpConstraintSolver.cpp:100
Eigen::VectorXd mB
Cache data for boxed LCP formulation.
Definition: BoxedLcpConstraintSolver.hpp:128
Eigen::VectorXd mHi
Cache data for boxed LCP formulation.
Definition: BoxedLcpConstraintSolver.hpp:143
Eigen::VectorXi mFIndex
Cache data for boxed LCP formulation.
Definition: BoxedLcpConstraintSolver.hpp:149
void solveConstrainedGroup(ConstrainedGroup &group) override
Definition: BoxedLcpConstraintSolver.cpp:148
Eigen::VectorXd mXBackup
Cache data for boxed LCP formulation.
Definition: BoxedLcpConstraintSolver.hpp:125
Eigen::VectorXd mLoBackup
Cache data for boxed LCP formulation.
Definition: BoxedLcpConstraintSolver.hpp:140
ConstrainedGroup is a group of skeletons that interact each other with constraints.
Definition: ConstrainedGroup.hpp:58
ConstraintSolver manages constraints and computes constraint impulses.
Definition: ConstraintSolver.hpp:57
std::shared_ptr< const BoxedLcpSolver > ConstBoxedLcpSolverPtr
Definition: SmartPointer.hpp:55
std::shared_ptr< BoxedLcpSolver > BoxedLcpSolverPtr
Definition: SmartPointer.hpp:55
Definition: BulletCollisionDetector.cpp:65