SemiExplicitEulerTimeStepper.h

Go to the documentation of this file.

#ifndef SimTK_SIMBODY_SEMI_EXPLICIT_EULER_TIME_STEPPER_H_
#define SimTK_SIMBODY_SEMI_EXPLICIT_EULER_TIME_STEPPER_H_

/* -------------------------------------------------------------------------- *
 *                               Simbody(tm)                                  *
 * -------------------------------------------------------------------------- *
 * This is part of the SimTK biosimulation toolkit originating from           *
 * Simbios, the NIH National Center for Physics-Based Simulation of           *
 * Biological Structures at Stanford, funded under the NIH Roadmap for        *
 * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody.  *
 *                                                                            *
 * Portions copyright (c) 2014 Stanford University and the Authors.           *
 * Authors: Michael Sherman, Thomas Uchida                                    *
 * Contributors:                                                              *
 *                                                                            *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may    *
 * not use this file except in compliance with the License. You may obtain a  *
 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0.         *
 *                                                                            *
 * Unless required by applicable law or agreed to in writing, software        *
 * distributed under the License is distributed on an "AS IS" BASIS,          *
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *
 * See the License for the specific language governing permissions and        *
 * limitations under the License.                                             *
 * -------------------------------------------------------------------------- */

#include "SimTKmath.h"
#include "simbody/internal/common.h"
#include "simbody/internal/MultibodySystem.h"
#include "simbody/internal/ImpulseSolver.h"
#include "simbody/internal/PGSImpulseSolver.h"
#include "simbody/internal/PLUSImpulseSolver.h"

namespace SimTK {

class SimTK_SIMBODY_EXPORT SemiExplicitEulerTimeStepper {
public:
    enum RestitutionModel {Poisson=0, Newton=1, NoRestitution=2};
    enum InducedImpactModel {Simultaneous=0, Sequential=1, Mixed=2};
    enum PositionProjectionMethod {Bilateral=0,Unilateral=1,
                                   NoPositionProjection=2};
    enum ImpulseSolverType {PLUS=0, PGS=1};


    explicit SemiExplicitEulerTimeStepper(const MultibodySystem& mbs);

    ~SemiExplicitEulerTimeStepper() {
        clearImpulseSolver();
    }

    void initialize(const State& initState);
    const State& getState() const {return m_state;}
    State& updState() {return m_state;}
    Real getTime() const {return m_state.getTime();}

    const State& getAdvancedState() const {return m_state;}

    State& updAdvancedState() {return m_state;}

    Real getAdvancedTime() const {return m_state.getTime();}

    Integrator::SuccessfulStepStatus stepTo(Real time);

    void setAccuracy(Real accuracy) {m_accuracy=accuracy;}
    void setConstraintTolerance(Real consTol) {m_consTol=consTol;}
   
    void setRestitutionModel(RestitutionModel restModel)
    {   m_restitutionModel = restModel; }
    RestitutionModel getRestitutionModel() const 
    {   return m_restitutionModel; }
     
    void setInducedImpactModel(InducedImpactModel indModel)
    {   m_inducedImpactModel = indModel; }
    InducedImpactModel getInducedImpactModel() const 
    {   return m_inducedImpactModel; }
    
    void setMaxInducedImpactsPerStep(int maxInduced) {
        SimTK_APIARGCHECK1_ALWAYS(maxInduced>=0, "SemiExplicitEulerTimeStepper",
            "setMaxInducedImpactsPerStep", "Illegal argument %d", maxInduced);
        m_maxInducedImpactsPerStep = maxInduced;
    }
    int getMaxInducedImpactsPerStep() const 
    {   return m_maxInducedImpactsPerStep; }
     
    void setPositionProjectionMethod(PositionProjectionMethod projMethod)
    {   m_projectionMethod = projMethod; }
    PositionProjectionMethod getPositionProjectionMethod() const 
    {   return m_projectionMethod; }
    
    void setImpulseSolverType(ImpulseSolverType solverType) {
        if (m_solverType != solverType) {
            // The new solver will get allocated in initialize().
            clearImpulseSolver();
            m_solverType = solverType; 
        }
    }
    ImpulseSolverType getImpulseSolverType() const 
    {   return m_solverType; }

    void setDefaultImpactCaptureVelocity(Real vCapture) {
        SimTK_ERRCHK1_ALWAYS(vCapture>=0,
        "SemiExplicitEulerTimeStepper::setDefaultImpactCaptureVelocity()",
        "The impact capture velocity must be nonnegative but was %g.",
        vCapture);
        m_defaultCaptureVelocity = vCapture;
    }

    void setDefaultImpactMinCORVelocity(Real vMinCOR) {
        SimTK_ERRCHK1_ALWAYS(vMinCOR>=0,
        "SemiExplicitEulerTimeStepper::setDefaultImpactMinCORVelocity()",
        "The velocity at which the minimum coefficient of restitution "
        " is reached must be nonnegative but was %g.", vMinCOR);
        m_defaultMinCORVelocity = vMinCOR;
    }

    void setDefaultFrictionTransitionVelocity(Real vTransition) {
        SimTK_ERRCHK1_ALWAYS(vTransition>=0,
        "SemiExplicitEulerTimeStepper::setDefaultFrictionTransitionVelocity()",
        "The friction transition velocity must be nonnegative but was %g.",
        vTransition);
        m_defaultTransitionVelocity = vTransition;
    }

    void setMinSignificantForce(Real minSignificantForce) {
        SimTK_ERRCHK1_ALWAYS(minSignificantForce>0,
        "SemiExplicitEulerTimeStepper::setMinSignificantForce()",
        "The minimum significant force magnitude must be greater than zero "
        "but was %g.", minSignificantForce);
        m_minSignificantForce = minSignificantForce;
    }
    Real getMinSignificantForce() const 
    {   return m_minSignificantForce; }

    Real getAccuracyInUse() const {return m_accuracy;}

    Real getConstraintToleranceInUse() const {return m_consTol;}

    Real getDefaultImpactCaptureVelocity() const 
    {   return m_defaultCaptureVelocity; }
    Real getDefaultImpactMinCORVelocity() const 
    {   return m_defaultMinCORVelocity; }
    Real getDefaultFrictionTransitionVelocity() const 
    {   return m_defaultTransitionVelocity; }

    Real getDefaultImpactCaptureVelocityInUse() const 
    {   return std::max(m_defaultCaptureVelocity, 2*m_consTol); }
    Real getDefaultImpactMinCORVelocityInUse() const 
    {   return std::max(m_defaultMinCORVelocity, 
                        getDefaultImpactCaptureVelocityInUse()); }
    Real getDefaultFrictionTransitionVelocityInUse() const 
    {   return std::max(m_defaultTransitionVelocity, 2*m_consTol); }

    const MultibodySystem& getMultibodySystem() const {return m_mbs;}


    const ImpulseSolver& getImpulseSolver() const {
        SimTK_ERRCHK_ALWAYS(m_solver!=0, 
            "SemiExplicitEulerTimeStepper::getImpulseSolver()",
            "No solver is currently allocated.");
        return *m_solver;
    }
    void setImpulseSolver(ImpulseSolver* impulseSolver) {
        clearImpulseSolver();
        m_solver = impulseSolver;
    }
    void clearImpulseSolver() {
        delete m_solver; m_solver=0;
    }

    static const char* getRestitutionModelName(RestitutionModel rm);
    static const char* getInducedImpactModelName(InducedImpactModel iim);
    static const char* getPositionProjectionMethodName
       (PositionProjectionMethod ppm);
    static const char* getImpulseSolverTypeName(ImpulseSolverType ist);

private:
    // Determine which constraints will be involved for this step.
    void findProximalConstraints(const State&);
    // Enable all proximal constraints, disable all distal constraints, 
    // reassigning multipliers if needed. Returns true if anything changed.
    bool enableProximalConstraints(State&);
    // After constraints are enabled, gather up useful info about them.
    void collectConstraintInfo(const State& s);
    // Calculate velocity-dependent coefficients of restitution and friction
    // and apply combining rules for dissimilar materials.
    void calcCoefficientsOfFriction(const State&, const Vector& verr);
    void calcCoefficientsOfRestitution(const State&, const Vector& verr,
                                       bool disableRestitution);

    // Easy if there are no constraints active.
    void takeUnconstrainedStep(State& s, Real h);

    // If we're in Newton restitution mode, calculating the verr change
    // that is needed to represent restitution. Output must already be
    // the same size as verr on entry if we're in Newton mode.
    bool calcNewtonRestitutionIfAny(const State&, const Vector& verr,
                                    Vector& newtonVerr) const;

    // Adjust given compression impulse to include Poisson restitution impulse.
    // Note which contacts are expanding.
    bool applyPoissonRestitutionIfAny(const State&, Vector& impulse,
                                      Array_<int>& expanders) const;

    bool calcExpansionImpulseIfAny(const State& s, const Array_<int>& impacters,
                                   const Vector& compressionImpulse,
                                   Vector& expansionImpulse,
                                   Array_<int>& expanders) const;

    // Perform a simultaneous impact if needed. All proximal constraints are 
    // dealt with so after this call there will be no more impacters, and no 
    // unapplied expansion impulses. For Poisson restitution this may be a 
    // compression/expansion impulse pair (and rarely a final compression
    // round to correct expanders that were forced back into impacting). 
    // For Newton restitution only a single impulse round is calculated.
    // Returns the number of impulse rounds actually taken, usually zero.
    int performSimultaneousImpact(const State& state, 
                                   Vector&      verr, // in/out
                                   Vector&      totalImpulse);

    // We identify impacters, observers, and expanders then perform a single
    // impulse calculation that ignores the observers. On return there may
    // be former observers and expanders that now have impacting approach
    // velocities so will be impacters on the next round. For Poisson
    // restitution, there may be expansion impulses that have not yet been 
    // applied; those contacts will be expanders on the next round.
    bool performInducedImpactRound(const Vector& verr, 
                                   const Vector& expansionImpulse);

    void classifyUnilateralContactsForSequentialImpact
       (const Vector&                           verr,
        const Vector&                           expansionImpulse,
        bool                                    includeAllProximals,
        bool                                    expansionOnly,
        Array_<ImpulseSolver::UniContactRT>&    uniContacts, 
        Array_<int>&                            impacters,
        Array_<int>&                            expanders,
        Array_<int>&                            observers,
        Array_<MultiplierIndex>&                participaters,
        Array_<MultiplierIndex>&                expanding) const;


    void classifyUnilateralContactsForSimultaneousImpact
       (const Vector&                           verr,
        const Vector&                           expansionImpulse,
        Array_<ImpulseSolver::UniContactRT>&    uniContacts, 
        Array_<int>&                            impacters,
        Array_<int>&                            expanders,
        Array_<int>&                            observers,
        Array_<MultiplierIndex>&                participaters,
        Array_<MultiplierIndex>&                expanding) const;

    // This phase uses all the proximal constraints and should use a starting
    // guess for impulse saved from the last step if possible.
    bool doCompressionPhase(const State&    state, 
                            Vector&         verrStart, // in/out
                            Vector&         verrApplied, // in/out
                            Vector&         compressionImpulse);
    // This phase uses all the proximal constraints, but we expect the result
    // to be zero unless expansion causes new violations.
    bool doExpansionPhase(const State&  state, 
                          const Array_<MultiplierIndex>& expanding,
                          Vector&       expansionImpulse,
                          Vector&       verrStart, // in/out
                          Vector&       reactionImpulse);
    bool doInducedImpactRound(const State&  state, 
                              const Array_<MultiplierIndex>& expanding,
                              Vector&       expansionImpulse,
                              Vector&       verrStart, // in/out
                              Vector&       impulse);
    bool anyPositionErrorsViolated(const State&, const Vector& perr) const;

    // This phase uses only holonomic constraints, and zero is a good initial
    // guess for the (hopefully small) position correction.
    bool doPositionCorrectionPhase(const State& state, 
                                   Vector&      pverr, // in/out
                                   Vector&      positionImpulse);


private:
    const MultibodySystem&      m_mbs;
    Real                        m_accuracy;
    Real                        m_consTol;
    RestitutionModel            m_restitutionModel;
    InducedImpactModel          m_inducedImpactModel;
    int                         m_maxInducedImpactsPerStep;
    PositionProjectionMethod    m_projectionMethod;
    ImpulseSolverType           m_solverType;


    Real                        m_defaultCaptureVelocity;
    Real                        m_defaultMinCORVelocity;
    Real                        m_defaultTransitionVelocity;
    Real                        m_minSignificantForce;

    ImpulseSolver*              m_solver;

    // Persistent runtime data.
    State                       m_state;
    Vector                      m_emptyVector; // don't change this!

    // Step temporaries.
    Matrix                      m_GMInvGt; // G M\ ~G
    Vector                      m_D; // soft diagonal
    Vector                      m_deltaU;
    Vector                      m_verr;
    Vector                      m_totalImpulse;
    Vector                      m_impulse;
    Vector                      m_genImpulse; // ~G*impulse

    Array_<UnilateralContactIndex>      m_proximalUniContacts, 
                                        m_distalUniContacts;
    Array_<StateLimitedFrictionIndex>   m_proximalStateLtdFriction,
                                        m_distalStateLtdFriction;

    // This is for use in the no-impact phase where all proximals participate.
    Array_<MultiplierIndex>                         m_allParticipating;

    // These are for use in impact phases.
    Array_<MultiplierIndex>                         m_participating;
    Array_<MultiplierIndex>                         m_expanding;
    Vector                                          m_expansionImpulse;
    Vector                                          m_newtonRestitutionVerr;
    Array_<int>                                     m_impacters;
    Array_<int>                                     m_expanders;
    Array_<int>                                     m_observers;

    Array_<ImpulseSolver::UncondRT>                 m_unconditional;
    Array_<ImpulseSolver::UniContactRT>             m_uniContact; // with fric
    Array_<ImpulseSolver::UniSpeedRT>               m_uniSpeed;
    Array_<ImpulseSolver::BoundedRT>                m_bounded;
    Array_<ImpulseSolver::ConstraintLtdFrictionRT>  m_consLtdFriction;
    Array_<ImpulseSolver::StateLtdFrictionRT>       m_stateLtdFriction;

    // These lists are for use in position projection and include only
    // holonomic constraints (the unilateral contacts have friction off).
    Array_<MultiplierIndex>                         m_posParticipating;
    Array_<ImpulseSolver::UncondRT>                 m_posUnconditional;
    Array_<ImpulseSolver::UniContactRT>             m_posUniContact; // no fric
    Array_<ImpulseSolver::UniSpeedRT>               m_posNoUniSpeed;
    Array_<ImpulseSolver::BoundedRT>                m_posNoBounded;
    Array_<ImpulseSolver::ConstraintLtdFrictionRT>  m_posNoConsLtdFriction;
    Array_<ImpulseSolver::StateLtdFrictionRT>       m_posNoStateLtdFriction;
};

} // namespace SimTK

#endif // SimTK_SIMBODY_SEMI_EXPLICIT_EULER_TIME_STEPPER_H_