Simbody 3.7
SemiExplicitEulerTimeStepper.h
Go to the documentation of this file.
1#ifndef SimTK_SIMBODY_SEMI_EXPLICIT_EULER_TIME_STEPPER_H_
2#define SimTK_SIMBODY_SEMI_EXPLICIT_EULER_TIME_STEPPER_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, Thomas Uchida *
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"
33
34namespace SimTK {
35
53public:
66 enum RestitutionModel {Poisson=0, Newton=1, NoRestitution=2};
67 enum InducedImpactModel {Simultaneous=0, Sequential=1, Mixed=2};
68 enum PositionProjectionMethod {Bilateral=0,Unilateral=1,
69 NoPositionProjection=2};
70 enum ImpulseSolverType {PLUS=0, PGS=1};
71
72
74
78 clearImpulseSolver();
79 }
80
84 void initialize(const State& initState);
86 const State& getState() const {return m_state;}
89 State& updState() {return m_state;}
92 Real getTime() const {return m_state.getTime();}
93
95 const State& getAdvancedState() const {return m_state;}
96
98 State& updAdvancedState() {return m_state;}
99
101 Real getAdvancedTime() const {return m_state.getTime();}
102
106
108 void setAccuracy(Real accuracy) {m_accuracy=accuracy;}
110 void setConstraintTolerance(Real consTol) {m_consTol=consTol;}
111
113 { m_restitutionModel = restModel; }
115 { return m_restitutionModel; }
116
118 { m_inducedImpactModel = indModel; }
120 { return m_inducedImpactModel; }
121
131 void setMaxInducedImpactsPerStep(int maxInduced) {
132 SimTK_APIARGCHECK1_ALWAYS(maxInduced>=0, "SemiExplicitEulerTimeStepper",
133 "setMaxInducedImpactsPerStep", "Illegal argument %d", maxInduced);
134 m_maxInducedImpactsPerStep = maxInduced;
135 }
137 { return m_maxInducedImpactsPerStep; }
138
140 { m_projectionMethod = projMethod; }
142 { return m_projectionMethod; }
143
145 if (m_solverType != solverType) {
146 // The new solver will get allocated in initialize().
147 clearImpulseSolver();
148 m_solverType = solverType;
149 }
150 }
152 { return m_solverType; }
153
163 SimTK_ERRCHK1_ALWAYS(vCapture>=0,
164 "SemiExplicitEulerTimeStepper::setDefaultImpactCaptureVelocity()",
165 "The impact capture velocity must be nonnegative but was %g.",
166 vCapture);
167 m_defaultCaptureVelocity = vCapture;
168 }
169
178 SimTK_ERRCHK1_ALWAYS(vMinCOR>=0,
179 "SemiExplicitEulerTimeStepper::setDefaultImpactMinCORVelocity()",
180 "The velocity at which the minimum coefficient of restitution "
181 " is reached must be nonnegative but was %g.", vMinCOR);
182 m_defaultMinCORVelocity = vMinCOR;
183 }
184
194 SimTK_ERRCHK1_ALWAYS(vTransition>=0,
195 "SemiExplicitEulerTimeStepper::setDefaultFrictionTransitionVelocity()",
196 "The friction transition velocity must be nonnegative but was %g.",
197 vTransition);
198 m_defaultTransitionVelocity = vTransition;
199 }
200
205 void setMinSignificantForce(Real minSignificantForce) {
206 SimTK_ERRCHK1_ALWAYS(minSignificantForce>0,
207 "SemiExplicitEulerTimeStepper::setMinSignificantForce()",
208 "The minimum significant force magnitude must be greater than zero "
209 "but was %g.", minSignificantForce);
210 m_minSignificantForce = minSignificantForce;
211 }
213 { return m_minSignificantForce; }
214
217 Real getAccuracyInUse() const {return m_accuracy;}
218
223 Real getConstraintToleranceInUse() const {return m_consTol;}
224
229 { return m_defaultCaptureVelocity; }
234 { return m_defaultMinCORVelocity; }
239 { return m_defaultTransitionVelocity; }
240
244 { return std::max(m_defaultCaptureVelocity, 2*m_consTol); }
248 { return std::max(m_defaultMinCORVelocity,
249 getDefaultImpactCaptureVelocityInUse()); }
253 { return std::max(m_defaultTransitionVelocity, 2*m_consTol); }
254
257 const MultibodySystem& getMultibodySystem() const {return m_mbs;}
258
259
262 SimTK_ERRCHK_ALWAYS(m_solver!=0,
263 "SemiExplicitEulerTimeStepper::getImpulseSolver()",
264 "No solver is currently allocated.");
265 return *m_solver;
266 }
269 void setImpulseSolver(ImpulseSolver* impulseSolver) {
270 clearImpulseSolver();
271 m_solver = impulseSolver;
272 }
275 delete m_solver; m_solver=0;
276 }
277
287
288private:
289 // Determine which constraints will be involved for this step.
290 void findProximalConstraints(const State&);
291 // Enable all proximal constraints, disable all distal constraints,
292 // reassigning multipliers if needed. Returns true if anything changed.
293 bool enableProximalConstraints(State&);
294 // After constraints are enabled, gather up useful info about them.
295 void collectConstraintInfo(const State& s);
296 // Calculate velocity-dependent coefficients of restitution and friction
297 // and apply combining rules for dissimilar materials.
298 void calcCoefficientsOfFriction(const State&, const Vector& verr);
299 void calcCoefficientsOfRestitution(const State&, const Vector& verr,
300 bool disableRestitution);
301
302 // Easy if there are no constraints active.
303 void takeUnconstrainedStep(State& s, Real h);
304
305 // If we're in Newton restitution mode, calculating the verr change
306 // that is needed to represent restitution. Output must already be
307 // the same size as verr on entry if we're in Newton mode.
308 bool calcNewtonRestitutionIfAny(const State&, const Vector& verr,
309 Vector& newtonVerr) const;
310
311 // Adjust given compression impulse to include Poisson restitution impulse.
312 // Note which contacts are expanding.
313 bool applyPoissonRestitutionIfAny(const State&, Vector& impulse,
314 Array_<int>& expanders) const;
315
316 bool calcExpansionImpulseIfAny(const State& s, const Array_<int>& impacters,
317 const Vector& compressionImpulse,
318 Vector& expansionImpulse,
319 Array_<int>& expanders) const;
320
321 // Perform a simultaneous impact if needed. All proximal constraints are
322 // dealt with so after this call there will be no more impacters, and no
323 // unapplied expansion impulses. For Poisson restitution this may be a
324 // compression/expansion impulse pair (and rarely a final compression
325 // round to correct expanders that were forced back into impacting).
326 // For Newton restitution only a single impulse round is calculated.
327 // Returns the number of impulse rounds actually taken, usually zero.
328 int performSimultaneousImpact(const State& state,
329 Vector& verr, // in/out
330 Vector& totalImpulse);
331
332 // We identify impacters, observers, and expanders then perform a single
333 // impulse calculation that ignores the observers. On return there may
334 // be former observers and expanders that now have impacting approach
335 // velocities so will be impacters on the next round. For Poisson
336 // restitution, there may be expansion impulses that have not yet been
337 // applied; those contacts will be expanders on the next round.
338 bool performInducedImpactRound(const Vector& verr,
339 const Vector& expansionImpulse);
340
341 void classifyUnilateralContactsForSequentialImpact
342 (const Vector& verr,
343 const Vector& expansionImpulse,
344 bool includeAllProximals,
345 bool expansionOnly,
347 Array_<int>& impacters,
348 Array_<int>& expanders,
349 Array_<int>& observers,
350 Array_<MultiplierIndex>& participaters,
351 Array_<MultiplierIndex>& expanding) const;
352
353
354 void classifyUnilateralContactsForSimultaneousImpact
355 (const Vector& verr,
356 const Vector& expansionImpulse,
358 Array_<int>& impacters,
359 Array_<int>& expanders,
360 Array_<int>& observers,
361 Array_<MultiplierIndex>& participaters,
362 Array_<MultiplierIndex>& expanding) const;
363
364 // This phase uses all the proximal constraints and should use a starting
365 // guess for impulse saved from the last step if possible.
366 bool doCompressionPhase(const State& state,
367 Vector& verrStart, // in/out
368 Vector& verrApplied, // in/out
369 Vector& compressionImpulse);
370 // This phase uses all the proximal constraints, but we expect the result
371 // to be zero unless expansion causes new violations.
372 bool doExpansionPhase(const State& state,
373 const Array_<MultiplierIndex>& expanding,
374 Vector& expansionImpulse,
375 Vector& verrStart, // in/out
376 Vector& reactionImpulse);
377 bool doInducedImpactRound(const State& state,
378 const Array_<MultiplierIndex>& expanding,
379 Vector& expansionImpulse,
380 Vector& verrStart, // in/out
381 Vector& impulse);
382 bool anyPositionErrorsViolated(const State&, const Vector& perr) const;
383
384 // This phase uses only holonomic constraints, and zero is a good initial
385 // guess for the (hopefully small) position correction.
386 bool doPositionCorrectionPhase(const State& state,
387 Vector& pverr, // in/out
388 Vector& positionImpulse);
389
390
391private:
392 const MultibodySystem& m_mbs;
393 Real m_accuracy;
394 Real m_consTol;
395 RestitutionModel m_restitutionModel;
396 InducedImpactModel m_inducedImpactModel;
397 int m_maxInducedImpactsPerStep;
398 PositionProjectionMethod m_projectionMethod;
399 ImpulseSolverType m_solverType;
400
401
402 Real m_defaultCaptureVelocity;
403 Real m_defaultMinCORVelocity;
404 Real m_defaultTransitionVelocity;
405 Real m_minSignificantForce;
406
407 ImpulseSolver* m_solver;
408
409 // Persistent runtime data.
410 State m_state;
411 Vector m_emptyVector; // don't change this!
412
413 // Step temporaries.
414 Matrix m_GMInvGt; // G M\ ~G
415 Vector m_D; // soft diagonal
416 Vector m_deltaU;
417 Vector m_verr;
418 Vector m_totalImpulse;
419 Vector m_impulse;
420 Vector m_genImpulse; // ~G*impulse
421
422 Array_<UnilateralContactIndex> m_proximalUniContacts,
423 m_distalUniContacts;
424 Array_<StateLimitedFrictionIndex> m_proximalStateLtdFriction,
425 m_distalStateLtdFriction;
426
427 // This is for use in the no-impact phase where all proximals participate.
428 Array_<MultiplierIndex> m_allParticipating;
429
430 // These are for use in impact phases.
431 Array_<MultiplierIndex> m_participating;
432 Array_<MultiplierIndex> m_expanding;
433 Vector m_expansionImpulse;
434 Vector m_newtonRestitutionVerr;
435 Array_<int> m_impacters;
436 Array_<int> m_expanders;
437 Array_<int> m_observers;
438
439 Array_<ImpulseSolver::UncondRT> m_unconditional;
440 Array_<ImpulseSolver::UniContactRT> m_uniContact; // with fric
445
446 // These lists are for use in position projection and include only
447 // holonomic constraints (the unilateral contacts have friction off).
448 Array_<MultiplierIndex> m_posParticipating;
449 Array_<ImpulseSolver::UncondRT> m_posUnconditional;
450 Array_<ImpulseSolver::UniContactRT> m_posUniContact; // no fric
451 Array_<ImpulseSolver::UniSpeedRT> m_posNoUniSpeed;
454 Array_<ImpulseSolver::StateLtdFrictionRT> m_posNoStateLtdFriction;
455};
456
457} // namespace SimTK
458
459#endif // SimTK_SIMBODY_SEMI_EXPLICIT_EULER_TIME_STEPPER_H_
460
#define SimTK_ERRCHK_ALWAYS(cond, whereChecked, msg)
Definition: ExceptionMacros.h:281
#define SimTK_ERRCHK1_ALWAYS(cond, whereChecked, fmt, a1)
Definition: ExceptionMacros.h:285
#define SimTK_APIARGCHECK1_ALWAYS(cond, className, methodName, fmt, a1)
Definition: ExceptionMacros.h:228
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
This is the abstract base class for impulse solvers, which solve an important subproblem of the conta...
Definition: ImpulseSolver.h:109
SuccessfulStepStatus
When a step is successful, it will return an indication of what caused it to stop where it did.
Definition: Integrator.h:202
The job of the MultibodySystem class is to coordinate the activities of various subsystems which can ...
Definition: MultibodySystem.h:48
A low-accuracy, high performance, velocity-level time stepper for models containing unilateral rigid ...
Definition: SemiExplicitEulerTimeStepper.h:52
PositionProjectionMethod
Definition: SemiExplicitEulerTimeStepper.h:68
InducedImpactModel
Definition: SemiExplicitEulerTimeStepper.h:67
void setAccuracy(Real accuracy)
Set integration accuracy; requires variable length steps.
Definition: SemiExplicitEulerTimeStepper.h:108
State & updAdvancedState()
synonym for updState.
Definition: SemiExplicitEulerTimeStepper.h:98
void initialize(const State &initState)
Initialize the TimeStepper's internally maintained state to a copy of the given state; allocate and i...
Real getAdvancedTime() const
synonym for getTime.
Definition: SemiExplicitEulerTimeStepper.h:101
void setDefaultImpactCaptureVelocity(Real vCapture)
Set the impact capture velocity to be used by default when a contact does not provide its own.
Definition: SemiExplicitEulerTimeStepper.h:162
const State & getState() const
Get access to the TimeStepper's internally maintained State.
Definition: SemiExplicitEulerTimeStepper.h:86
Real getDefaultImpactCaptureVelocityInUse() const
Return the value actually being used as the default impact capture velocity.
Definition: SemiExplicitEulerTimeStepper.h:243
State & updState()
Get writable access to the TimeStepper's internally maintained State.
Definition: SemiExplicitEulerTimeStepper.h:89
SemiExplicitEulerTimeStepper(const MultibodySystem &mbs)
void setInducedImpactModel(InducedImpactModel indModel)
Definition: SemiExplicitEulerTimeStepper.h:117
Real getDefaultFrictionTransitionVelocityInUse() const
Return the value actually being used as the default sliding-to-rolling friction transition velocity.
Definition: SemiExplicitEulerTimeStepper.h:252
void setImpulseSolver(ImpulseSolver *impulseSolver)
(Advanced) Set your own ImpulseSolver; the TimeStepper takes over ownership so don't delete afterward...
Definition: SemiExplicitEulerTimeStepper.h:269
static const char * getPositionProjectionMethodName(PositionProjectionMethod ppm)
Get human-readable string representing the given enum value.
Integrator::SuccessfulStepStatus stepTo(Real time)
Advance to the indicated time in one or more steps, using repeated induced impacts.
Real getDefaultImpactCaptureVelocity() const
Return the value set for this parameter, but the actual value used during execution will be no smalle...
Definition: SemiExplicitEulerTimeStepper.h:228
~SemiExplicitEulerTimeStepper()
The contained ImpulseSolver will be destructed here; don't reference it afterwards!
Definition: SemiExplicitEulerTimeStepper.h:77
Real getTime() const
Shortcut to getting the current time from the TimeStepper's internally maintained State.
Definition: SemiExplicitEulerTimeStepper.h:92
void setRestitutionModel(RestitutionModel restModel)
Definition: SemiExplicitEulerTimeStepper.h:112
void setDefaultFrictionTransitionVelocity(Real vTransition)
Set the friction sliding-to-rolling transition velocity to be used by default when a frictional conta...
Definition: SemiExplicitEulerTimeStepper.h:193
RestitutionModel
If an impact occurs at a contact where the coefficient of restitution (COR) is non zero,...
Definition: SemiExplicitEulerTimeStepper.h:66
Real getConstraintToleranceInUse() const
Return the tolerance to which we require constraints to be satisfied.
Definition: SemiExplicitEulerTimeStepper.h:223
Real getMinSignificantForce() const
Definition: SemiExplicitEulerTimeStepper.h:212
void setMaxInducedImpactsPerStep(int maxInduced)
Limit the number of induced impact rounds per time step.
Definition: SemiExplicitEulerTimeStepper.h:131
void setConstraintTolerance(Real consTol)
Set the tolerance to which constraints must be satisfied.
Definition: SemiExplicitEulerTimeStepper.h:110
ImpulseSolverType
Definition: SemiExplicitEulerTimeStepper.h:70
void setMinSignificantForce(Real minSignificantForce)
Set the threshold below which we can ignore forces.
Definition: SemiExplicitEulerTimeStepper.h:205
void setImpulseSolverType(ImpulseSolverType solverType)
Definition: SemiExplicitEulerTimeStepper.h:144
int getMaxInducedImpactsPerStep() const
Definition: SemiExplicitEulerTimeStepper.h:136
const ImpulseSolver & getImpulseSolver() const
(Advanced) Get direct access to the ImpulseSolver.
Definition: SemiExplicitEulerTimeStepper.h:261
static const char * getImpulseSolverTypeName(ImpulseSolverType ist)
Get human-readable string representing the given enum value.
void setDefaultImpactMinCORVelocity(Real vMinCOR)
Set the minimum coefficient of restitution (COR) velocity to be used by default when a unilateral con...
Definition: SemiExplicitEulerTimeStepper.h:177
Real getDefaultImpactMinCORVelocityInUse() const
Return the value actually being used as the default impact minimum coefficient of restitution velocit...
Definition: SemiExplicitEulerTimeStepper.h:247
static const char * getRestitutionModelName(RestitutionModel rm)
Get human-readable string representing the given enum value.
void clearImpulseSolver()
(Advanced) Delete the existing ImpulseSolver if any.
Definition: SemiExplicitEulerTimeStepper.h:274
Real getDefaultFrictionTransitionVelocity() const
Return the value set for this parameter, but the actual value used during execution will be no smalle...
Definition: SemiExplicitEulerTimeStepper.h:238
PositionProjectionMethod getPositionProjectionMethod() const
Definition: SemiExplicitEulerTimeStepper.h:141
const MultibodySystem & getMultibodySystem() const
Get access to the MultibodySystem for which this TimeStepper was constructed.
Definition: SemiExplicitEulerTimeStepper.h:257
const State & getAdvancedState() const
synonym for getState.
Definition: SemiExplicitEulerTimeStepper.h:95
Real getDefaultImpactMinCORVelocity() const
Return the value set for this parameter, but the actual value used during execution will be no smalle...
Definition: SemiExplicitEulerTimeStepper.h:233
static const char * getInducedImpactModelName(InducedImpactModel iim)
Get human-readable string representing the given enum value.
InducedImpactModel getInducedImpactModel() const
Definition: SemiExplicitEulerTimeStepper.h:119
RestitutionModel getRestitutionModel() const
Definition: SemiExplicitEulerTimeStepper.h:114
void setPositionProjectionMethod(PositionProjectionMethod projMethod)
Definition: SemiExplicitEulerTimeStepper.h:139
ImpulseSolverType getImpulseSolverType() const
Definition: SemiExplicitEulerTimeStepper.h:151
Real getAccuracyInUse() const
Return the integration accuracy setting.
Definition: SemiExplicitEulerTimeStepper.h:217
This object is intended to contain all state information for a SimTK::System, except topological info...
Definition: State.h:280
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
ELEM max(const VectorBase< ELEM > &v)
Definition: VectorMath.h:251
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