Simbody 3.7
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
29namespace SimTK {
30
35public:
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;
62
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;
71
73
74private:
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_
#define SimTK_SIMBODY_EXPORT
Definition: Simbody/include/simbody/internal/common.h:68
The Array_<T> container class is a plug-compatible replacement for the C++ standard template library ...
Definition: Array.h:1520
This is the abstract base class for impulse solvers, which solve an important subproblem of the conta...
Definition: ImpulseSolver.h:109
TODO: PLUS (Poisson-Lankarani-Uchida-Sherman) impulse solver.
Definition: PLUSImpulseSolver.h:34
bool solveBilateral(const Array_< MultiplierIndex > &participating, const Matrix &A, const Vector &D, const Vector &rhs, Vector &pi) const override
Solve with only unconditional constraints.
SimTK_DEFINE_UNIQUE_LOCAL_INDEX_TYPE(PLUSImpulseSolver, ActiveIndex)
PLUSImpulseSolver(Real roll2slipTransitionSpeed)
Definition: PLUSImpulseSolver.h:36
bool solve(int phase, const Array_< MultiplierIndex > &participating, const Matrix &A, const Vector &D, const Array_< MultiplierIndex > &expanding, Vector &piExpand, Vector &verrStart, Vector &verrApplied, Vector &pi, Array_< UncondRT > &unconditional, Array_< UniContactRT > &uniContact, Array_< UniSpeedRT > &uniSpeed, Array_< BoundedRT > &bounded, Array_< ConstraintLtdFrictionRT > &consLtdFriction, Array_< StateLtdFrictionRT > &stateLtdFriction) const override
Solve with conditional constraints.
const Real SqrtEps
This is the square root of Eps, ~1e-8 if Real is double, ~3e-4 if Real is float.
const Real Pi
Real(pi)
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
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:606