Simbody  3.5
PLUSImpulseSolver.h
Go to the documentation of this file.
1 #ifndef SimTK_SIMBODY_PLUS_IMPULSE_SOLVER_H_
2 #define SimTK_SIMBODY_PLUS_IMPULSE_SOLVER_H_
3 
4 /* -------------------------------------------------------------------------- *
5  * Simbody(tm) *
6  * -------------------------------------------------------------------------- *
7  * This is part of the SimTK biosimulation toolkit originating from *
8  * Simbios, the NIH National Center for Physics-Based Simulation of *
9  * Biological Structures at Stanford, funded under the NIH Roadmap for *
10  * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody. *
11  * *
12  * Portions copyright (c) 2014 Stanford University and the Authors. *
13  * Authors: Thomas Uchida, Michael Sherman *
14  * Contributors: *
15  * *
16  * Licensed under the Apache License, Version 2.0 (the "License"); you may *
17  * not use this file except in compliance with the License. You may obtain a *
18  * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. *
19  * *
20  * Unless required by applicable law or agreed to in writing, software *
21  * distributed under the License is distributed on an "AS IS" BASIS, *
22  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. *
23  * See the License for the specific language governing permissions and *
24  * limitations under the License. *
25  * -------------------------------------------------------------------------- */
26 
28 
29 namespace SimTK {
30 
35 public:
36  explicit PLUSImpulseSolver(Real roll2slipTransitionSpeed)
37  : ImpulseSolver(roll2slipTransitionSpeed,
38  1e-10, // default PLUS convergence tol
39  20), // default Newton iteration limit
40  m_minSmoothness(SqrtEps), // sharpness of smoothed discontinuities
41  m_cosMaxSlidingDirChange(std::cos(Pi/6)) // 30 degrees
42  {}
43 
45  bool solve
46  (int phase,
47  const Array_<MultiplierIndex>& participating,
48  const Matrix& A,
49  const Vector& D,
50  const Array_<MultiplierIndex>& expanding,
51  Vector& piExpand, // in/out
52  Vector& verrStart, // in/out
53  Vector& verrApplied,
54  Vector& pi,
55  Array_<UncondRT>& unconditional,
56  Array_<UniContactRT>& uniContact,
57  Array_<UniSpeedRT>& uniSpeed,
58  Array_<BoundedRT>& bounded,
59  Array_<ConstraintLtdFrictionRT>& consLtdFriction,
60  Array_<StateLtdFrictionRT>& stateLtdFriction
61  ) const OVERRIDE_11;
62 
64  bool solveBilateral
65  (const Array_<MultiplierIndex>& participating, // p<=m of these
66  const Matrix& A, // m X m, symmetric
67  const Vector& D, // m, diag>=0 added to A
68  const Vector& rhs, // m, RHS
69  Vector& pi // m, unknown result
70  ) const OVERRIDE_11;
71 
73 
74 private:
75 
76  // Given point P and line segment AB, find the point closest to P that lies
77  // on AB, which we call Q. Returns stepLength, the ratio AQ:AB. In our case,
78  // P is the origin and AB is the line segment connecting the initial and
79  // final tangential velocity vectors.
80  // @author Thomas Uchida
81  Real calcSlidingStepLengthToOrigin(const Vec2& A, const Vec2& B, Vec2& Q)
82  const;
83  Real calcSlidingStepLengthToOrigin(const Vec3& A, const Vec3& B, Vec3& Q)
84  const;
85 
86  // Given vectors A and B, find step length alpha such that the angle between
87  // A and A+alpha*(B-A) is MaxSlidingDirChange. The solutions were generated
88  // in Maple using the law of cosines, then exported as optimized code.
89  // @author Thomas Uchida
90  Real calcSlidingStepLengthToMaxChange(const Vec2& A, const Vec2& B) const;
91  Real calcSlidingStepLengthToMaxChange(const Vec3& A, const Vec3& B) const;
92 
93  void classifyFrictionals(Array_<UniContactRT>& uniContacts) const;
94 
95  // Go through the given set of active constraints and build a reverse map
96  // from the multipliers to the active index.
97  void fillMult2Active(const Array_<MultiplierIndex,ActiveIndex>& active,
98  Array_<ActiveIndex,MultiplierIndex>& mult2active) const;
99 
100  // Copy the active rows and columns of A into the Jacobian. These will
101  // be the right values for the linear equations, but rows for nonlinear
102  // equations (sliding, impending) will get overwritten. Initialize piActive
103  // from pi.
104  void initializeNewton(const Matrix& A,
105  const Vector& piGuess,
106  const Vector& verrApplied,
107  const Array_<UniContactRT>& bounded) const;
108 
109  // Given a new piActive, update the impending slip directions and calculate
110  // the new err(piActive).
111  void updateDirectionsAndCalcCurrentError
112  (const Matrix& A, Array_<UniContactRT>& uniContact,
113  const Vector& piELeft, const Vector& verrAppliedLeft,
114  const Vector& piActive,
115  Vector& errActive) const;
116 
117  // Replace rows of Jacobian for constraints corresponding to sliding or
118  // impending slip frictional elements. This is the partial derivative of the
119  // constraint error w.r.t. pi. Also set rhs m_verrActive.
120  void updateJacobianForSliding(const Matrix& A,
121  const Array_<UniContactRT>& uniContact,
122  const Vector& piELeft,
123  const Vector& verrAppliedLeft) const;
124 
125  // These are set on construction.
126  Real m_minSmoothness;
127  Real m_cosMaxSlidingDirChange;
128 
129  // This starts out as verr and is then reduced during each interval.
130  mutable Vector m_verrLeft; // m of these
131  mutable Vector m_verrExpand; // -A*piExpand for not-yet-applied piE
132 
133  // This is a subset of the given participating constraints that are
134  // presently active. Only the rows and columns of A that are listed here
135  // can be used (and we'll replace some of those rows). Note that a
136  // "known" unilateral contact (typically one undergoing Poisson expansion)
137  // is *not* active, although its friction constraints are.
138  mutable Array_<MultiplierIndex,ActiveIndex> m_active; // na of these
139 
140  // This is the inverse mapping from m_active. Given an index into the full
141  // A matrix (all proximal constraint equations, each with a Simbody-assigned
142  // multiplier), this returns either the corresponding index into the
143  // m_active array, or an invalid index if this proximal constraint is not
144  // active.
145  mutable Array_<ActiveIndex,MultiplierIndex> m_mult2active; // m of these
146 
147  // Each of these is indexed by ActiveIndex; they have dimension na.
148  mutable Matrix m_JacActive; // Jacobian for Newton iteration
149  mutable Vector m_rhsActive; // per-interval RHS for Newton iteration
150  mutable Vector m_piActive; // Current impulse during Newton.
151  mutable Vector m_errActive; // Error(piActive)
152 
153  mutable Matrix m_bilateralActive; // temp for use by solveBilateral()
154 };
155 
156 } // namespace SimTK
157 
158 #endif // SimTK_SIMBODY_PLUS_IMPULSE_SOLVER_H_
PLUSImpulseSolver(Real roll2slipTransitionSpeed)
Definition: PLUSImpulseSolver.h:36
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
This is the abstract base class for impulse solvers, which solve an important subproblem of the conta...
Definition: ImpulseSolver.h:109
STL namespace.
TODO: PLUS (Poisson-Lankarani-Uchida-Sherman) impulse solver.
Definition: PLUSImpulseSolver.h:34
SimTK_Real Real
This is the default compiled-in floating point type for SimTK, either float or double.
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:593
#define OVERRIDE_11
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:267
The SimTK::Array_<T> container class is a plug-compatible replacement for the C++ standard template l...
Definition: Array.h:50
const Real SqrtEps
This is the square root of Eps, ~1e-8 if Real is double, ~3e-4 if Real is float.
#define SimTK_DEFINE_UNIQUE_LOCAL_INDEX_TYPE(PARENT, NAME)
Define a local Index class within a Parent class.
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:446
const Real Pi
Real(pi)
#define SimTK_SIMBODY_EXPORT
Definition: Simbody/include/simbody/internal/common.h:72