Simbody  3.8
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 
34 namespace SimTK {
35 
53 public:
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 
288 private:
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 
391 private:
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;
452  Array_<ImpulseSolver::BoundedRT> m_posNoBounded;
453  Array_<ImpulseSolver::ConstraintLtdFrictionRT> m_posNoConsLtdFriction;
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
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
Real getDefaultImpactCaptureVelocityInUse() const
Return the value actually being used as the default impact capture velocity.
Definition: SemiExplicitEulerTimeStepper.h:243
const State & getAdvancedState() const
synonym for getState.
Definition: SemiExplicitEulerTimeStepper.h:95
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
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
static const char * getInducedImpactModelName(InducedImpactModel iim)
Get human-readable string representing the given enum value.
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 MultibodySystem & getMultibodySystem() const
Get access to the MultibodySystem for which this TimeStepper was constructed.
Definition: SemiExplicitEulerTimeStepper.h:257
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.
const State & getState() const
Get access to the TimeStepper's internally maintained State.
Definition: SemiExplicitEulerTimeStepper.h:86
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
State & updAdvancedState()
synonym for updState.
Definition: SemiExplicitEulerTimeStepper.h:98
PositionProjectionMethod getPositionProjectionMethod() const
Definition: SemiExplicitEulerTimeStepper.h:141
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
InducedImpactModel getInducedImpactModel() const
Definition: SemiExplicitEulerTimeStepper.h:119
static const char * getPositionProjectionMethodName(PositionProjectionMethod ppm)
Get human-readable string representing the given enum value.
RestitutionModel getRestitutionModel() const
Definition: SemiExplicitEulerTimeStepper.h:114
static const char * getImpulseSolverTypeName(ImpulseSolverType ist)
Get human-readable string representing the given enum value.
void setPositionProjectionMethod(PositionProjectionMethod projMethod)
Definition: SemiExplicitEulerTimeStepper.h:139
ImpulseSolverType getImpulseSolverType() const
Definition: SemiExplicitEulerTimeStepper.h:151
const ImpulseSolver & getImpulseSolver() const
(Advanced) Get direct access to the ImpulseSolver.
Definition: SemiExplicitEulerTimeStepper.h:261
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:607