Simbody  3.5
SemiExplicitEulerTimeStepper.h
Go to the documentation of this file.
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 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines