Simbody
3.5
|
00001 #ifndef SimTK_SIMBODY_SEMI_EXPLICIT_EULER_TIME_STEPPER_H_ 00002 #define SimTK_SIMBODY_SEMI_EXPLICIT_EULER_TIME_STEPPER_H_ 00003 00004 /* -------------------------------------------------------------------------- * 00005 * Simbody(tm) * 00006 * -------------------------------------------------------------------------- * 00007 * This is part of the SimTK biosimulation toolkit originating from * 00008 * Simbios, the NIH National Center for Physics-Based Simulation of * 00009 * Biological Structures at Stanford, funded under the NIH Roadmap for * 00010 * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody. * 00011 * * 00012 * Portions copyright (c) 2014 Stanford University and the Authors. * 00013 * Authors: Michael Sherman, Thomas Uchida * 00014 * Contributors: * 00015 * * 00016 * Licensed under the Apache License, Version 2.0 (the "License"); you may * 00017 * not use this file except in compliance with the License. You may obtain a * 00018 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * 00019 * * 00020 * Unless required by applicable law or agreed to in writing, software * 00021 * distributed under the License is distributed on an "AS IS" BASIS, * 00022 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * 00023 * See the License for the specific language governing permissions and * 00024 * limitations under the License. * 00025 * -------------------------------------------------------------------------- */ 00026 00027 #include "SimTKmath.h" 00028 #include "simbody/internal/common.h" 00029 #include "simbody/internal/MultibodySystem.h" 00030 #include "simbody/internal/ImpulseSolver.h" 00031 #include "simbody/internal/PGSImpulseSolver.h" 00032 #include "simbody/internal/PLUSImpulseSolver.h" 00033 00034 namespace SimTK { 00035 00052 class SimTK_SIMBODY_EXPORT SemiExplicitEulerTimeStepper { 00053 public: 00066 enum RestitutionModel {Poisson=0, Newton=1, NoRestitution=2}; 00067 enum InducedImpactModel {Simultaneous=0, Sequential=1, Mixed=2}; 00068 enum PositionProjectionMethod {Bilateral=0,Unilateral=1, 00069 NoPositionProjection=2}; 00070 enum ImpulseSolverType {PLUS=0, PGS=1}; 00071 00072 00073 explicit SemiExplicitEulerTimeStepper(const MultibodySystem& mbs); 00074 00077 ~SemiExplicitEulerTimeStepper() { 00078 clearImpulseSolver(); 00079 } 00080 00084 void initialize(const State& initState); 00086 const State& getState() const {return m_state;} 00089 State& updState() {return m_state;} 00092 Real getTime() const {return m_state.getTime();} 00093 00095 const State& getAdvancedState() const {return m_state;} 00096 00098 State& updAdvancedState() {return m_state;} 00099 00101 Real getAdvancedTime() const {return m_state.getTime();} 00102 00105 Integrator::SuccessfulStepStatus stepTo(Real time); 00106 00108 void setAccuracy(Real accuracy) {m_accuracy=accuracy;} 00110 void setConstraintTolerance(Real consTol) {m_consTol=consTol;} 00111 00112 void setRestitutionModel(RestitutionModel restModel) 00113 { m_restitutionModel = restModel; } 00114 RestitutionModel getRestitutionModel() const 00115 { return m_restitutionModel; } 00116 00117 void setInducedImpactModel(InducedImpactModel indModel) 00118 { m_inducedImpactModel = indModel; } 00119 InducedImpactModel getInducedImpactModel() const 00120 { return m_inducedImpactModel; } 00121 00131 void setMaxInducedImpactsPerStep(int maxInduced) { 00132 SimTK_APIARGCHECK1_ALWAYS(maxInduced>=0, "SemiExplicitEulerTimeStepper", 00133 "setMaxInducedImpactsPerStep", "Illegal argument %d", maxInduced); 00134 m_maxInducedImpactsPerStep = maxInduced; 00135 } 00136 int getMaxInducedImpactsPerStep() const 00137 { return m_maxInducedImpactsPerStep; } 00138 00139 void setPositionProjectionMethod(PositionProjectionMethod projMethod) 00140 { m_projectionMethod = projMethod; } 00141 PositionProjectionMethod getPositionProjectionMethod() const 00142 { return m_projectionMethod; } 00143 00144 void setImpulseSolverType(ImpulseSolverType solverType) { 00145 if (m_solverType != solverType) { 00146 // The new solver will get allocated in initialize(). 00147 clearImpulseSolver(); 00148 m_solverType = solverType; 00149 } 00150 } 00151 ImpulseSolverType getImpulseSolverType() const 00152 { return m_solverType; } 00153 00162 void setDefaultImpactCaptureVelocity(Real vCapture) { 00163 SimTK_ERRCHK1_ALWAYS(vCapture>=0, 00164 "SemiExplicitEulerTimeStepper::setDefaultImpactCaptureVelocity()", 00165 "The impact capture velocity must be nonnegative but was %g.", 00166 vCapture); 00167 m_defaultCaptureVelocity = vCapture; 00168 } 00169 00177 void setDefaultImpactMinCORVelocity(Real vMinCOR) { 00178 SimTK_ERRCHK1_ALWAYS(vMinCOR>=0, 00179 "SemiExplicitEulerTimeStepper::setDefaultImpactMinCORVelocity()", 00180 "The velocity at which the minimum coefficient of restitution " 00181 " is reached must be nonnegative but was %g.", vMinCOR); 00182 m_defaultMinCORVelocity = vMinCOR; 00183 } 00184 00193 void setDefaultFrictionTransitionVelocity(Real vTransition) { 00194 SimTK_ERRCHK1_ALWAYS(vTransition>=0, 00195 "SemiExplicitEulerTimeStepper::setDefaultFrictionTransitionVelocity()", 00196 "The friction transition velocity must be nonnegative but was %g.", 00197 vTransition); 00198 m_defaultTransitionVelocity = vTransition; 00199 } 00200 00205 void setMinSignificantForce(Real minSignificantForce) { 00206 SimTK_ERRCHK1_ALWAYS(minSignificantForce>0, 00207 "SemiExplicitEulerTimeStepper::setMinSignificantForce()", 00208 "The minimum significant force magnitude must be greater than zero " 00209 "but was %g.", minSignificantForce); 00210 m_minSignificantForce = minSignificantForce; 00211 } 00212 Real getMinSignificantForce() const 00213 { return m_minSignificantForce; } 00214 00217 Real getAccuracyInUse() const {return m_accuracy;} 00218 00223 Real getConstraintToleranceInUse() const {return m_consTol;} 00224 00228 Real getDefaultImpactCaptureVelocity() const 00229 { return m_defaultCaptureVelocity; } 00233 Real getDefaultImpactMinCORVelocity() const 00234 { return m_defaultMinCORVelocity; } 00238 Real getDefaultFrictionTransitionVelocity() const 00239 { return m_defaultTransitionVelocity; } 00240 00243 Real getDefaultImpactCaptureVelocityInUse() const 00244 { return std::max(m_defaultCaptureVelocity, 2*m_consTol); } 00247 Real getDefaultImpactMinCORVelocityInUse() const 00248 { return std::max(m_defaultMinCORVelocity, 00249 getDefaultImpactCaptureVelocityInUse()); } 00252 Real getDefaultFrictionTransitionVelocityInUse() const 00253 { return std::max(m_defaultTransitionVelocity, 2*m_consTol); } 00254 00257 const MultibodySystem& getMultibodySystem() const {return m_mbs;} 00258 00259 00261 const ImpulseSolver& getImpulseSolver() const { 00262 SimTK_ERRCHK_ALWAYS(m_solver!=0, 00263 "SemiExplicitEulerTimeStepper::getImpulseSolver()", 00264 "No solver is currently allocated."); 00265 return *m_solver; 00266 } 00269 void setImpulseSolver(ImpulseSolver* impulseSolver) { 00270 clearImpulseSolver(); 00271 m_solver = impulseSolver; 00272 } 00274 void clearImpulseSolver() { 00275 delete m_solver; m_solver=0; 00276 } 00277 00279 static const char* getRestitutionModelName(RestitutionModel rm); 00281 static const char* getInducedImpactModelName(InducedImpactModel iim); 00283 static const char* getPositionProjectionMethodName 00284 (PositionProjectionMethod ppm); 00286 static const char* getImpulseSolverTypeName(ImpulseSolverType ist); 00287 00288 private: 00289 // Determine which constraints will be involved for this step. 00290 void findProximalConstraints(const State&); 00291 // Enable all proximal constraints, disable all distal constraints, 00292 // reassigning multipliers if needed. Returns true if anything changed. 00293 bool enableProximalConstraints(State&); 00294 // After constraints are enabled, gather up useful info about them. 00295 void collectConstraintInfo(const State& s); 00296 // Calculate velocity-dependent coefficients of restitution and friction 00297 // and apply combining rules for dissimilar materials. 00298 void calcCoefficientsOfFriction(const State&, const Vector& verr); 00299 void calcCoefficientsOfRestitution(const State&, const Vector& verr, 00300 bool disableRestitution); 00301 00302 // Easy if there are no constraints active. 00303 void takeUnconstrainedStep(State& s, Real h); 00304 00305 // If we're in Newton restitution mode, calculating the verr change 00306 // that is needed to represent restitution. Output must already be 00307 // the same size as verr on entry if we're in Newton mode. 00308 bool calcNewtonRestitutionIfAny(const State&, const Vector& verr, 00309 Vector& newtonVerr) const; 00310 00311 // Adjust given compression impulse to include Poisson restitution impulse. 00312 // Note which contacts are expanding. 00313 bool applyPoissonRestitutionIfAny(const State&, Vector& impulse, 00314 Array_<int>& expanders) const; 00315 00316 bool calcExpansionImpulseIfAny(const State& s, const Array_<int>& impacters, 00317 const Vector& compressionImpulse, 00318 Vector& expansionImpulse, 00319 Array_<int>& expanders) const; 00320 00321 // Perform a simultaneous impact if needed. All proximal constraints are 00322 // dealt with so after this call there will be no more impacters, and no 00323 // unapplied expansion impulses. For Poisson restitution this may be a 00324 // compression/expansion impulse pair (and rarely a final compression 00325 // round to correct expanders that were forced back into impacting). 00326 // For Newton restitution only a single impulse round is calculated. 00327 // Returns the number of impulse rounds actually taken, usually zero. 00328 int performSimultaneousImpact(const State& state, 00329 Vector& verr, // in/out 00330 Vector& totalImpulse); 00331 00332 // We identify impacters, observers, and expanders then perform a single 00333 // impulse calculation that ignores the observers. On return there may 00334 // be former observers and expanders that now have impacting approach 00335 // velocities so will be impacters on the next round. For Poisson 00336 // restitution, there may be expansion impulses that have not yet been 00337 // applied; those contacts will be expanders on the next round. 00338 bool performInducedImpactRound(const Vector& verr, 00339 const Vector& expansionImpulse); 00340 00341 void classifyUnilateralContactsForSequentialImpact 00342 (const Vector& verr, 00343 const Vector& expansionImpulse, 00344 bool includeAllProximals, 00345 bool expansionOnly, 00346 Array_<ImpulseSolver::UniContactRT>& uniContacts, 00347 Array_<int>& impacters, 00348 Array_<int>& expanders, 00349 Array_<int>& observers, 00350 Array_<MultiplierIndex>& participaters, 00351 Array_<MultiplierIndex>& expanding) const; 00352 00353 00354 void classifyUnilateralContactsForSimultaneousImpact 00355 (const Vector& verr, 00356 const Vector& expansionImpulse, 00357 Array_<ImpulseSolver::UniContactRT>& uniContacts, 00358 Array_<int>& impacters, 00359 Array_<int>& expanders, 00360 Array_<int>& observers, 00361 Array_<MultiplierIndex>& participaters, 00362 Array_<MultiplierIndex>& expanding) const; 00363 00364 // This phase uses all the proximal constraints and should use a starting 00365 // guess for impulse saved from the last step if possible. 00366 bool doCompressionPhase(const State& state, 00367 Vector& verrStart, // in/out 00368 Vector& verrApplied, // in/out 00369 Vector& compressionImpulse); 00370 // This phase uses all the proximal constraints, but we expect the result 00371 // to be zero unless expansion causes new violations. 00372 bool doExpansionPhase(const State& state, 00373 const Array_<MultiplierIndex>& expanding, 00374 Vector& expansionImpulse, 00375 Vector& verrStart, // in/out 00376 Vector& reactionImpulse); 00377 bool doInducedImpactRound(const State& state, 00378 const Array_<MultiplierIndex>& expanding, 00379 Vector& expansionImpulse, 00380 Vector& verrStart, // in/out 00381 Vector& impulse); 00382 bool anyPositionErrorsViolated(const State&, const Vector& perr) const; 00383 00384 // This phase uses only holonomic constraints, and zero is a good initial 00385 // guess for the (hopefully small) position correction. 00386 bool doPositionCorrectionPhase(const State& state, 00387 Vector& pverr, // in/out 00388 Vector& positionImpulse); 00389 00390 00391 private: 00392 const MultibodySystem& m_mbs; 00393 Real m_accuracy; 00394 Real m_consTol; 00395 RestitutionModel m_restitutionModel; 00396 InducedImpactModel m_inducedImpactModel; 00397 int m_maxInducedImpactsPerStep; 00398 PositionProjectionMethod m_projectionMethod; 00399 ImpulseSolverType m_solverType; 00400 00401 00402 Real m_defaultCaptureVelocity; 00403 Real m_defaultMinCORVelocity; 00404 Real m_defaultTransitionVelocity; 00405 Real m_minSignificantForce; 00406 00407 ImpulseSolver* m_solver; 00408 00409 // Persistent runtime data. 00410 State m_state; 00411 Vector m_emptyVector; // don't change this! 00412 00413 // Step temporaries. 00414 Matrix m_GMInvGt; // G M\ ~G 00415 Vector m_D; // soft diagonal 00416 Vector m_deltaU; 00417 Vector m_verr; 00418 Vector m_totalImpulse; 00419 Vector m_impulse; 00420 Vector m_genImpulse; // ~G*impulse 00421 00422 Array_<UnilateralContactIndex> m_proximalUniContacts, 00423 m_distalUniContacts; 00424 Array_<StateLimitedFrictionIndex> m_proximalStateLtdFriction, 00425 m_distalStateLtdFriction; 00426 00427 // This is for use in the no-impact phase where all proximals participate. 00428 Array_<MultiplierIndex> m_allParticipating; 00429 00430 // These are for use in impact phases. 00431 Array_<MultiplierIndex> m_participating; 00432 Array_<MultiplierIndex> m_expanding; 00433 Vector m_expansionImpulse; 00434 Vector m_newtonRestitutionVerr; 00435 Array_<int> m_impacters; 00436 Array_<int> m_expanders; 00437 Array_<int> m_observers; 00438 00439 Array_<ImpulseSolver::UncondRT> m_unconditional; 00440 Array_<ImpulseSolver::UniContactRT> m_uniContact; // with fric 00441 Array_<ImpulseSolver::UniSpeedRT> m_uniSpeed; 00442 Array_<ImpulseSolver::BoundedRT> m_bounded; 00443 Array_<ImpulseSolver::ConstraintLtdFrictionRT> m_consLtdFriction; 00444 Array_<ImpulseSolver::StateLtdFrictionRT> m_stateLtdFriction; 00445 00446 // These lists are for use in position projection and include only 00447 // holonomic constraints (the unilateral contacts have friction off). 00448 Array_<MultiplierIndex> m_posParticipating; 00449 Array_<ImpulseSolver::UncondRT> m_posUnconditional; 00450 Array_<ImpulseSolver::UniContactRT> m_posUniContact; // no fric 00451 Array_<ImpulseSolver::UniSpeedRT> m_posNoUniSpeed; 00452 Array_<ImpulseSolver::BoundedRT> m_posNoBounded; 00453 Array_<ImpulseSolver::ConstraintLtdFrictionRT> m_posNoConsLtdFriction; 00454 Array_<ImpulseSolver::StateLtdFrictionRT> m_posNoStateLtdFriction; 00455 }; 00456 00457 } // namespace SimTK 00458 00459 #endif // SimTK_SIMBODY_SEMI_EXPLICIT_EULER_TIME_STEPPER_H_ 00460