Simbody 3.7
Loading...
Searching...
No Matches
ImpulseSolver.h
Go to the documentation of this file.
1#ifndef SimTK_SIMBODY_IMPULSE_SOLVER_H_
2#define SimTK_SIMBODY_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: 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
27#include "SimTKmath.h"
29
30namespace SimTK {
31
110public:
111 struct UncondRT;
112 struct UniContactRT;
113 struct UniSpeedRT;
114 struct BoundedRT;
116 struct StateLtdFrictionRT;
117
118 // How to treat a unilateral contact (input to solver).
119 enum ContactType {TypeNA=-1, Observing=0, Known=1, Participating=2};
120
121 // These are the results after the solve is complete. Don't mess with the
122 // enum numbering here; we're counting on it.
123 enum UniCond {UniNA=-1, UniOff=0, UniActive=1, UniKnown=2};
124 enum FricCond {FricNA=-1, FricOff=0, Sliding=1, Impending=2, Rolling=3};
125 enum BndCond {BndNA=-1, SlipLow=0, ImpendLow=1, Engaged=2,
126 ImpendHigh=3, SlipHigh=4};
127
128
129 ImpulseSolver(Real roll2slipTransitionSpeed,
130 Real convergenceTol,
131 int maxIters)
132 : m_maxRollingTangVel(roll2slipTransitionSpeed),
133 m_convergenceTol(convergenceTol),
134 m_maxIters(maxIters)
135 {
136 clearStats();
137 }
138
139 virtual ~ImpulseSolver() {}
140
141 void setMaxRollingSpeed(Real roll2slipTransitionSpeed) {
142 assert(roll2slipTransitionSpeed >= 0);
143 m_maxRollingTangVel = roll2slipTransitionSpeed;
144 }
145 Real getMaxRollingSpeed() const {return m_maxRollingTangVel;}
146
148 assert(tol >= 0);
149 m_convergenceTol = tol;
150 }
151 Real getConvergenceTol() const {return m_convergenceTol;}
152
153 void setMaxIterations(int maxIts) {
154 assert(maxIts > 0);
155 m_maxIters = maxIts;
156 }
157 int getMaxIterations() const {return m_maxIters;}
158
159 // We'll keep stats separately for different "phases". The meaning of a
160 // phase is up to the caller.
161 static const int MaxNumPhases = 3;
162
163 void clearStats() const {
164 for (int i=0; i < MaxNumPhases; ++i)
165 clearStats(i);
166 m_nBilateralSolves = m_nBilateralIters = m_nBilateralFail = 0;
167 }
168
169 void clearStats(int phase) const {
170 SimTK_ERRCHK2(0<=phase&&phase<MaxNumPhases,
171 "ImpulseSolver::clearStats(phase)",
172 "Phase must be 0..%d but was %d\n", MaxNumPhases-1, phase);
173 m_nSolves[phase] = m_nIters[phase] = m_nFail[phase] = 0;
174 }
175
177 virtual bool solve
178 (int phase,
179 const Array_<MultiplierIndex>& participating, // p<=m of these
180 const Matrix& A, // m X m, symmetric
181 const Vector& D, // m, diag>=0 added to A
182 const Array_<MultiplierIndex>& expanding, // nx<=m of these
183 Vector& piExpand, // m
184 Vector& verrStart, // m, RHS (in/out)
185 Vector& verrApplied, // m
186 Vector& pi, // m, known+unknown
187 Array_<UncondRT>& unconditional,
188 Array_<UniContactRT>& uniContact, // with friction
189 Array_<UniSpeedRT>& uniSpeed,
190 Array_<BoundedRT>& bounded,
191 Array_<ConstraintLtdFrictionRT>& consLtdFriction,
192 Array_<StateLtdFrictionRT>& stateLtdFriction
193 ) const = 0;
194
195
217 virtual bool solveBilateral
218 (const Array_<MultiplierIndex>& participating, // p<=m of these
219 const Matrix& A, // m X m, symmetric
220 const Vector& D, // m, diag>=0 added to A
221 const Vector& rhs, // m, RHS
222 Vector& pi // m, unknown result
223 ) const = 0;
224
225 // Printable names for the enum values for debugging.
226 static const char* getContactTypeName(ContactType ct);
227 static const char* getUniCondName(UniCond uc);
228 static const char* getFricCondName(FricCond fc);
229 static const char* getBndCondName(BndCond bc);
230
231 // Show details for each uni contact in the array.
232 static void dumpUniContacts(const String& msg,
233 const Array_<UniContactRT>& uniContacts);
234
235protected:
236 Real m_maxRollingTangVel; // Sliding above this speed if solver cares.
237 Real m_convergenceTol; // Meaning depends on concrete solver.
238 int m_maxIters; // Meaning depends on concrete solver.
239
240 mutable long long m_nSolves[MaxNumPhases];
241 mutable long long m_nIters[MaxNumPhases];
242 mutable long long m_nFail[MaxNumPhases];
243 mutable long long m_nBilateralSolves;
244 mutable long long m_nBilateralIters;
245 mutable long long m_nBilateralFail;
246};
247
250
251 // Input to solver.
252 ConstraintIndex m_constraint; // Back pointer to Simbody element.
253 Array_<MultiplierIndex> m_mults; // Which constraint multipliers?
254
255 // Set by solver on return.
256 Array_<Real> m_impulse; // Same size as m_mults.
257};
258
259// A unilateral contact (possibly with friction), joint stop, rope.
260// These are the only constraints that can undergo impacts. Note that the COR
261// is here for the convenience of the time stepper; it doesn't affect the
262// impulse solvers. "Known" here means the normal constraint does not
263// participate (that is, the constraint equation cannot be active), but an
264// expansion impulse has been supplied for it.
271
272 bool hasFriction() const {return !m_Fk.empty();}
273
274 // Input to solver.
275 UnilateralContactIndex m_ucx; // Back pointer to Simbody element.
276 MultiplierIndex m_Nk; // multiplier for the normal constraint
277 Real m_sign; // sign convention for normal multiplier
278
279 Array_<MultiplierIndex> m_Fk; // optional friction multipliers
280
281 // These solver inputs can change during a step.
282 ContactType m_type; // Observing, Known, Participating
283 Real m_effCOR; // velocity-dependent COR
284 Real m_effMu; // if there is friction, else NaN
285
286 // Working values for use by the solver, with final values returned.
292};
293
294// Ratchet.
298 { assert(sign==-1 || sign==1); }
299
300 // Input to solver.
301 MultiplierIndex m_ix; // which constraint multiplier
302 Real m_sign; // allowable sign for non-zero multiplier
303
304 // Set by solver on return.
307};
308
309// Torque-limited motor.
312 : m_ix(ix), m_lb(lb), m_ub(ub), m_boundedCond(BndNA), m_impulse(NaN)
313 { assert(m_lb<=m_ub); }
314
315 // Input to solver.
316 MultiplierIndex m_ix; // which constraint multiplier
317 Real m_lb, m_ub; // effective lower, upper bounds; lb <= ub
318
319 // Set by solver on return.
322};
323
324// Friction acting at a joint-like constraint, bead-on-a-wire.
327 (const Array_<MultiplierIndex>& frictionComponents,
328 const Array_<MultiplierIndex>& normalComponents,
329 Real effMu)
330 : m_Fk(frictionComponents), m_Nk(normalComponents),
331 m_effMu(effMu), m_frictionCond(FricNA),
332 m_Fimpulse(frictionComponents.size(), NaN)
333 { assert(m_Fk.size()<=3 && m_Nk.size()<=3);
334 assert(isNaN(m_effMu) || m_effMu>=0); }
335
336 // Inputs to solver.
337 ConstraintLimitedFrictionIndex m_clfx; // Back pointer to Simbody element.
340
341 // Set by solver on return.
343 Array_<Real> m_Fimpulse; // same size as m_Fk
344};
345
346// Friction acting at a compliant contact.
348 StateLtdFrictionRT(const Array_<MultiplierIndex>& frictionComponents,
349 Real knownN, Real muEff)
350 : m_Fk(frictionComponents), m_knownN(knownN), m_effMu(muEff),
351 m_frictionCond(FricNA), m_Fimpulse(frictionComponents.size(), NaN)
352 { assert(m_Fk.size()<=3); assert(m_knownN >= 0);
353 assert(isNaN(m_effMu) || m_effMu>=0); }
354
355 // Inputs to solver.
356 StateLimitedFrictionIndex m_slfx; // Back pointer to Simbody element.
360
361 // Set by solver on return.
363 Array_<Real> m_Fimpulse; // same size as m_Fk
364};
365
366} // namespace SimTK
367
368#endif // SimTK_SIMBODY_IMPULSE_SOLVER_H_
#define SimTK_ERRCHK2(cond, whereChecked, fmt, a1, a2)
Definition ExceptionMacros.h:328
Every Simbody header and source file should include this header before any other Simbody header.
#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 for arrays indexed by constraint number within a subsystem (typically the SimbodyMatterSubsys...
This is the abstract base class for impulse solvers, which solve an important subproblem of the conta...
Definition ImpulseSolver.h:109
Real m_maxRollingTangVel
Definition ImpulseSolver.h:236
BndCond
Definition ImpulseSolver.h:125
@ BndNA
Definition ImpulseSolver.h:125
UniCond
Definition ImpulseSolver.h:123
@ UniNA
Definition ImpulseSolver.h:123
int getMaxIterations() const
Definition ImpulseSolver.h:157
static const char * getContactTypeName(ContactType ct)
static const char * getBndCondName(BndCond bc)
long long m_nBilateralIters
Definition ImpulseSolver.h:244
long long m_nBilateralFail
Definition ImpulseSolver.h:245
ContactType
Definition ImpulseSolver.h:119
@ TypeNA
Definition ImpulseSolver.h:119
virtual bool solveBilateral(const Array_< MultiplierIndex > &participating, const Matrix &A, const Vector &D, const Vector &rhs, Vector &pi) const =0
Solve a set of bilateral (unconditional) constraints for the impulse necessary to enforce them.
virtual ~ImpulseSolver()
Definition ImpulseSolver.h:139
static const char * getFricCondName(FricCond fc)
long long m_nBilateralSolves
Definition ImpulseSolver.h:243
Real m_convergenceTol
Definition ImpulseSolver.h:237
void clearStats(int phase) const
Definition ImpulseSolver.h:169
static void dumpUniContacts(const String &msg, const Array_< UniContactRT > &uniContacts)
static const char * getUniCondName(UniCond uc)
FricCond
Definition ImpulseSolver.h:124
@ FricNA
Definition ImpulseSolver.h:124
int m_maxIters
Definition ImpulseSolver.h:238
void setMaxIterations(int maxIts)
Definition ImpulseSolver.h:153
void setMaxRollingSpeed(Real roll2slipTransitionSpeed)
Definition ImpulseSolver.h:141
Real getMaxRollingSpeed() const
Definition ImpulseSolver.h:145
ImpulseSolver(Real roll2slipTransitionSpeed, Real convergenceTol, int maxIters)
Definition ImpulseSolver.h:129
void setConvergenceTol(Real tol)
Definition ImpulseSolver.h:147
Real getConvergenceTol() const
Definition ImpulseSolver.h:151
void clearStats() const
Definition ImpulseSolver.h:163
virtual 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 =0
Solve.
Unique integer type for Subsystem-local multiplier indexing.
SimTK::String is a plug-compatible std::string replacement (plus some additional functionality) inten...
Definition String.h:62
const Real NaN
This is the IEEE "not a number" constant for this implementation of the default-precision Real type; ...
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition Assembler.h:37
unsigned int sign(unsigned char u)
Definition Scalar.h:311
bool isNaN(const negator< float > &x)
Definition negator.h:272
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
Definition ImpulseSolver.h:310
Real m_ub
Definition ImpulseSolver.h:317
Real m_lb
Definition ImpulseSolver.h:317
Real m_impulse
Definition ImpulseSolver.h:321
BoundedRT(MultiplierIndex ix, Real lb, Real ub)
Definition ImpulseSolver.h:311
MultiplierIndex m_ix
Definition ImpulseSolver.h:316
BndCond m_boundedCond
Definition ImpulseSolver.h:320
Definition ImpulseSolver.h:325
ConstraintLimitedFrictionIndex m_clfx
Definition ImpulseSolver.h:337
Real m_effMu
Definition ImpulseSolver.h:339
ConstraintLtdFrictionRT(const Array_< MultiplierIndex > &frictionComponents, const Array_< MultiplierIndex > &normalComponents, Real effMu)
Definition ImpulseSolver.h:327
FricCond m_frictionCond
Definition ImpulseSolver.h:342
Array_< Real > m_Fimpulse
Definition ImpulseSolver.h:343
Array_< MultiplierIndex > m_Nk
Definition ImpulseSolver.h:338
Array_< MultiplierIndex > m_Fk
Definition ImpulseSolver.h:338
Definition ImpulseSolver.h:347
StateLtdFrictionRT(const Array_< MultiplierIndex > &frictionComponents, Real knownN, Real muEff)
Definition ImpulseSolver.h:348
Array_< MultiplierIndex > m_Fk
Definition ImpulseSolver.h:357
Real m_effMu
Definition ImpulseSolver.h:359
FricCond m_frictionCond
Definition ImpulseSolver.h:362
Real m_knownN
Definition ImpulseSolver.h:358
StateLimitedFrictionIndex m_slfx
Definition ImpulseSolver.h:356
Array_< Real > m_Fimpulse
Definition ImpulseSolver.h:363
Definition ImpulseSolver.h:248
ConstraintIndex m_constraint
Definition ImpulseSolver.h:252
UncondRT()
Definition ImpulseSolver.h:249
Array_< MultiplierIndex > m_mults
Definition ImpulseSolver.h:253
Array_< Real > m_impulse
Definition ImpulseSolver.h:256
Definition ImpulseSolver.h:265
Real m_effMu
Definition ImpulseSolver.h:284
FricCond m_frictionCond
Definition ImpulseSolver.h:288
MultiplierIndex m_Nk
Definition ImpulseSolver.h:276
Array_< MultiplierIndex > m_Fk
Definition ImpulseSolver.h:279
Real m_sign
Definition ImpulseSolver.h:277
UniContactRT()
Definition ImpulseSolver.h:266
Vec2 m_slipVel
Definition ImpulseSolver.h:289
ContactType m_type
Definition ImpulseSolver.h:282
UniCond m_contactCond
Definition ImpulseSolver.h:287
Vec3 m_impulse
Definition ImpulseSolver.h:291
bool hasFriction() const
Definition ImpulseSolver.h:272
Real m_slipMag
Definition ImpulseSolver.h:290
Real m_effCOR
Definition ImpulseSolver.h:283
UnilateralContactIndex m_ucx
Definition ImpulseSolver.h:275
Definition ImpulseSolver.h:295
UniSpeedRT(MultiplierIndex ix, int sign)
Definition ImpulseSolver.h:296
Vec3 m_impulse
Definition ImpulseSolver.h:306
MultiplierIndex m_ix
Definition ImpulseSolver.h:301
Real m_sign
Definition ImpulseSolver.h:302
UniCond m_speedCond
Definition ImpulseSolver.h:305