Simbody  3.5
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 
73  explicit SemiExplicitEulerTimeStepper(const MultibodySystem& mbs);
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 
279  static const char* getRestitutionModelName(RestitutionModel rm);
281  static const char* getInducedImpactModelName(InducedImpactModel iim);
283  static const char* getPositionProjectionMethodName
286  static const char* getImpulseSolverTypeName(ImpulseSolverType ist);
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 
Real getDefaultImpactCaptureVelocityInUse() const
Return the value actually being used as the default impact capture velocity.
Definition: SemiExplicitEulerTimeStepper.h:243
void setConstraintTolerance(Real consTol)
Set the tolerance to which constraints must be satisfied.
Definition: SemiExplicitEulerTimeStepper.h:110
const ImpulseSolver & getImpulseSolver() const
(Advanced) Get direct access to the ImpulseSolver.
Definition: SemiExplicitEulerTimeStepper.h:261
#define SimTK_APIARGCHECK1_ALWAYS(cond, className, methodName, fmt, a1)
Definition: ExceptionMacros.h:228
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 getConstraintToleranceInUse() const
Return the tolerance to which we require constraints to be satisfied.
Definition: SemiExplicitEulerTimeStepper.h:223
Real getAccuracyInUse() const
Return the integration accuracy setting.
Definition: SemiExplicitEulerTimeStepper.h:217
Real getDefaultFrictionTransitionVelocityInUse() const
Return the value actually being used as the default sliding-to-rolling friction transition velocity...
Definition: SemiExplicitEulerTimeStepper.h:252
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
Real getMinSignificantForce() const
Definition: SemiExplicitEulerTimeStepper.h:212
A low-accuracy, high performance, velocity-level time stepper for models containing unilateral rigid ...
Definition: SemiExplicitEulerTimeStepper.h:52
void setPositionProjectionMethod(PositionProjectionMethod projMethod)
Definition: SemiExplicitEulerTimeStepper.h:139
This is the abstract base class for impulse solvers, which solve an important subproblem of the conta...
Definition: ImpulseSolver.h:109
void setAccuracy(Real accuracy)
Set integration accuracy; requires variable length steps.
Definition: SemiExplicitEulerTimeStepper.h:108
Every Simbody header and source file should include this header before any other Simbody header...
void setImpulseSolver(ImpulseSolver *impulseSolver)
(Advanced) Set your own ImpulseSolver; the TimeStepper takes over ownership so don&#39;t delete afterward...
Definition: SemiExplicitEulerTimeStepper.h:269
void setImpulseSolverType(ImpulseSolverType solverType)
Definition: SemiExplicitEulerTimeStepper.h:144
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
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
Real getAdvancedTime() const
synonym for getTime.
Definition: SemiExplicitEulerTimeStepper.h:101
void clearImpulseSolver()
(Advanced) Delete the existing ImpulseSolver if any.
Definition: SemiExplicitEulerTimeStepper.h:274
#define SimTK_ERRCHK_ALWAYS(cond, whereChecked, msg)
Definition: ExceptionMacros.h:281
This object is intended to contain all state information for a SimTK::System, except topological info...
Definition: State.h:276
void setMinSignificantForce(Real minSignificantForce)
Set the threshold below which we can ignore forces.
Definition: SemiExplicitEulerTimeStepper.h:205
void setRestitutionModel(RestitutionModel restModel)
Definition: SemiExplicitEulerTimeStepper.h:112
~SemiExplicitEulerTimeStepper()
The contained ImpulseSolver will be destructed here; don&#39;t reference it afterwards! ...
Definition: SemiExplicitEulerTimeStepper.h:77
#define SimTK_ERRCHK1_ALWAYS(cond, whereChecked, fmt, a1)
Definition: ExceptionMacros.h:285
RestitutionModel
If an impact occurs at a contact where the coefficient of restitution (COR) is non zero...
Definition: SemiExplicitEulerTimeStepper.h:66
void setInducedImpactModel(InducedImpactModel indModel)
Definition: SemiExplicitEulerTimeStepper.h:117
ImpulseSolverType getImpulseSolverType() const
Definition: SemiExplicitEulerTimeStepper.h:151
InducedImpactModel getInducedImpactModel() const
Definition: SemiExplicitEulerTimeStepper.h:119
ELEM max(const VectorBase< ELEM > &v)
Definition: VectorMath.h:251
The job of the MultibodySystem class is to coordinate the activities of various subsystems which can ...
Definition: MultibodySystem.h:48
Real getDefaultImpactMinCORVelocityInUse() const
Return the value actually being used as the default impact minimum coefficient of restitution velocit...
Definition: SemiExplicitEulerTimeStepper.h:247
const State & getAdvancedState() const
synonym for getState.
Definition: SemiExplicitEulerTimeStepper.h:95
void setMaxInducedImpactsPerStep(int maxInduced)
Limit the number of induced impact rounds per time step.
Definition: SemiExplicitEulerTimeStepper.h:131
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
State & updState()
Get writable access to the TimeStepper&#39;s internally maintained State.
Definition: SemiExplicitEulerTimeStepper.h:89
InducedImpactModel
Definition: SemiExplicitEulerTimeStepper.h:67
SuccessfulStepStatus
When a step is successful, it will return an indication of what caused it to stop where it did...
Definition: Integrator.h:202
PositionProjectionMethod
Definition: SemiExplicitEulerTimeStepper.h:68
#define SimTK_SIMBODY_EXPORT
Definition: Simbody/include/simbody/internal/common.h:72
Real getTime() const
Shortcut to getting the current time from the TimeStepper&#39;s internally maintained State...
Definition: SemiExplicitEulerTimeStepper.h:92
ImpulseSolverType
Definition: SemiExplicitEulerTimeStepper.h:70
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
PositionProjectionMethod getPositionProjectionMethod() const
Definition: SemiExplicitEulerTimeStepper.h:141
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
const MultibodySystem & getMultibodySystem() const
Get access to the MultibodySystem for which this TimeStepper was constructed.
Definition: SemiExplicitEulerTimeStepper.h:257
const State & getState() const
Get access to the TimeStepper&#39;s internally maintained State.
Definition: SemiExplicitEulerTimeStepper.h:86
RestitutionModel getRestitutionModel() const
Definition: SemiExplicitEulerTimeStepper.h:114
int getMaxInducedImpactsPerStep() const
Definition: SemiExplicitEulerTimeStepper.h:136
State & updAdvancedState()
synonym for updState.
Definition: SemiExplicitEulerTimeStepper.h:98
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