State.h

Go to the documentation of this file.

#ifndef SimTK_SimTKCOMMON_STATE_H_
#define SimTK_SimTKCOMMON_STATE_H_

/* -------------------------------------------------------------------------- *
 *                      SimTK Core: SimTKcommon                               *
 * -------------------------------------------------------------------------- *
 * This is part of the SimTK Core 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.               *
 *                                                                            *
 * Portions copyright (c) 2005-10 Stanford University and the Authors.        *
 * Authors: Michael Sherman                                                   *
 * Contributors: Peter Eastman                                                *
 *                                                                            *
 * Permission is hereby granted, free of charge, to any person obtaining a    *
 * copy of this software and associated documentation files (the "Software"), *
 * to deal in the Software without restriction, including without limitation  *
 * the rights to use, copy, modify, merge, publish, distribute, sublicense,   *
 * and/or sell copies of the Software, and to permit persons to whom the      *
 * Software is furnished to do so, subject to the following conditions:       *
 *                                                                            *
 * The above copyright notice and this permission notice shall be included in *
 * all copies or substantial portions of the Software.                        *
 *                                                                            *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,   *
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL    *
 * THE AUTHORS, CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,    *
 * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR      *
 * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE  *
 * USE OR OTHER DEALINGS IN THE SOFTWARE.                                     *
 * -------------------------------------------------------------------------- */

#include "SimTKcommon/basics.h"
#include "SimTKcommon/Simmatrix.h"
#include "SimTKcommon/internal/Event.h"

#include <ostream>
#include <cassert>
#include <set>

namespace SimTK {


SimTK_DEFINE_UNIQUE_INDEX_TYPE(SubsystemIndex);

SimTK_DEFINE_UNIQUE_INDEX_TYPE(SystemYIndex);

SimTK_DEFINE_UNIQUE_INDEX_TYPE(SystemQIndex);
SimTK_DEFINE_UNIQUE_INDEX_TYPE(QIndex);

SimTK_DEFINE_UNIQUE_INDEX_TYPE(SystemUIndex);
SimTK_DEFINE_UNIQUE_INDEX_TYPE(UIndex);

SimTK_DEFINE_UNIQUE_INDEX_TYPE(SystemZIndex);
SimTK_DEFINE_UNIQUE_INDEX_TYPE(ZIndex);

SimTK_DEFINE_UNIQUE_INDEX_TYPE(DiscreteVariableIndex);

SimTK_DEFINE_UNIQUE_INDEX_TYPE(CacheEntryIndex);

SimTK_DEFINE_UNIQUE_INDEX_TYPE(SystemYErrIndex);

SimTK_DEFINE_UNIQUE_INDEX_TYPE(SystemQErrIndex);
SimTK_DEFINE_UNIQUE_INDEX_TYPE(QErrIndex);

SimTK_DEFINE_UNIQUE_INDEX_TYPE(SystemUErrIndex);
SimTK_DEFINE_UNIQUE_INDEX_TYPE(UErrIndex);

SimTK_DEFINE_UNIQUE_INDEX_TYPE(SystemUDotErrIndex);
SimTK_DEFINE_UNIQUE_INDEX_TYPE(UDotErrIndex);

SimTK_DEFINE_UNIQUE_INDEX_TYPE(SystemMultiplierIndex);
SimTK_DEFINE_UNIQUE_INDEX_TYPE(MultiplierIndex);

typedef int StageVersion;


class SimTK_SimTKCOMMON_EXPORT State {
public:
    State();
    ~State();

    void clear();

    void setNSubsystems(int i);

    void initializeSubsystem(SubsystemIndex, const String& name, const String& version);

    State(const State&);

    State& operator=(const State&);

    SubsystemIndex addSubsystem(const String& name, const String& version);

    int getNSubsystems() const;
    const String& getSubsystemName   (SubsystemIndex) const;
    const String& getSubsystemVersion(SubsystemIndex) const;
    const Stage&  getSubsystemStage  (SubsystemIndex) const;

    const Stage& getSystemStage() const;

    void invalidateAll(Stage) const;  // cache is mutable

    void advanceSubsystemToStage(SubsystemIndex, Stage) const;
    void advanceSystemToStage(Stage) const;


    QIndex allocateQ(SubsystemIndex, const Vector& qInit); // qdot, qdotdot also allocated in cache
    UIndex allocateU(SubsystemIndex, const Vector& uInit); // udot                    "
    ZIndex allocateZ(SubsystemIndex, const Vector& zInit); // zdot                    "


    QErrIndex    allocateQErr   (SubsystemIndex, int nqerr) const;    // these are cache entries
    UErrIndex    allocateUErr   (SubsystemIndex, int nuerr) const;
    UDotErrIndex allocateUDotErr(SubsystemIndex, int nudoterr) const;

    EventIndex allocateEventIndex(SubsystemIndex, int nevent=1) const;

    EventTriggerByStageIndex allocateEventTrigger(SubsystemIndex, Stage, EventIndex, int nevent=1) const;

    //OBSOLETE
    EventTriggerByStageIndex allocateEventTrigger(SubsystemIndex, Stage, int nevent) const;



    DiscreteVariableIndex allocateDiscreteVariable(SubsystemIndex, Stage invalidates, AbstractValue*);

    DiscreteVariableIndex
        allocateAutoUpdateDiscreteVariable(SubsystemIndex, Stage invalidates, AbstractValue*,
                                           Stage updateDependsOn); 
    CacheEntryIndex getDiscreteVarUpdateIndex(SubsystemIndex, DiscreteVariableIndex) const;
    Stage getDiscreteVarAllocationStage(SubsystemIndex, DiscreteVariableIndex) const;
    Stage getDiscreteVarInvalidatesStage(SubsystemIndex, DiscreteVariableIndex) const;


    const AbstractValue& getDiscreteVariable(SubsystemIndex, DiscreteVariableIndex) const;
    Real getDiscreteVarLastUpdateTime(SubsystemIndex, DiscreteVariableIndex) const;
    const AbstractValue& getDiscreteVarUpdateValue(SubsystemIndex, DiscreteVariableIndex) const;
    AbstractValue& updDiscreteVarUpdateValue(SubsystemIndex, DiscreteVariableIndex) const;
    bool isDiscreteVarUpdateValueRealized(SubsystemIndex, DiscreteVariableIndex) const;
    void markDiscreteVarUpdateValueRealized(SubsystemIndex, DiscreteVariableIndex) const;

    AbstractValue& updDiscreteVariable(SubsystemIndex, DiscreteVariableIndex);
    void setDiscreteVariable(SubsystemIndex, DiscreteVariableIndex, const AbstractValue&);


    CacheEntryIndex allocateCacheEntry(SubsystemIndex, Stage earliest, Stage latest,
                                       AbstractValue*) const;

    CacheEntryIndex allocateCacheEntry(SubsystemIndex sx, Stage g, AbstractValue* v) const
    {   return allocateCacheEntry(sx, g, g, v); }

    CacheEntryIndex allocateLazyCacheEntry(SubsystemIndex sx, Stage earliest, AbstractValue* v) const
    {   return allocateCacheEntry(sx, earliest, Stage::Infinity, v); }

    Stage getCacheEntryAllocationStage(SubsystemIndex, CacheEntryIndex) const;

    const AbstractValue& getCacheEntry(SubsystemIndex, CacheEntryIndex) const;

    AbstractValue& updCacheEntry(SubsystemIndex, CacheEntryIndex) const; // mutable

    bool isCacheValueRealized(SubsystemIndex, CacheEntryIndex) const;

    void markCacheValueRealized(SubsystemIndex, CacheEntryIndex) const;


    int getNY() const;
    int getNQ() const;
    SystemYIndex getQStart() const;
    int getNU() const;
    SystemYIndex getUStart() const;
    int getNZ() const;
    SystemYIndex getZStart() const;
    int getNYErr() const;
    int getNQErr() const;
    SystemYErrIndex getQErrStart() const; 
    int getNUErr() const;
    SystemYErrIndex getUErrStart() const; 
    int getNUDotErr() const;
    int getNMultipliers() const; // =mp+mv+ma, necessarily the same as NUDotErr
    int getNEventTriggers() const;
    int getNEventTriggersByStage(Stage) const;
    SystemEventTriggerIndex getEventTriggerStartByStage(Stage) const; // per-stage



    SystemQIndex getQStart(SubsystemIndex)       const; int getNQ(SubsystemIndex)       const;
    SystemUIndex getUStart(SubsystemIndex)       const; int getNU(SubsystemIndex)       const;
    SystemZIndex getZStart(SubsystemIndex)       const; int getNZ(SubsystemIndex)       const;

    SystemQErrIndex       getQErrStart(SubsystemIndex)    const; int getNQErr(SubsystemIndex)    const;
    SystemUErrIndex       getUErrStart(SubsystemIndex)    const; int getNUErr(SubsystemIndex)    const;
    SystemUDotErrIndex    getUDotErrStart(SubsystemIndex) const; int getNUDotErr(SubsystemIndex) const;
    SystemMultiplierIndex getMultipliersStart(SubsystemIndex) const;
    int getNMultipliers(SubsystemIndex)     const;

    SystemEventTriggerByStageIndex getEventTriggerStartByStage(SubsystemIndex, Stage) const;
    int getNEventTriggersByStage(SubsystemIndex, Stage) const;



    void mapQToSubsystem(SystemQIndex, SubsystemIndex&, QIndex&) const;
    void mapUToSubsystem(SystemUIndex, SubsystemIndex&, UIndex&) const;
    void mapZToSubsystem(SystemZIndex, SubsystemIndex&, ZIndex&) const;
    void mapQErrToSubsystem(SystemQErrIndex, SubsystemIndex&, QErrIndex&) const;
    void mapUErrToSubsystem(SystemUErrIndex, SubsystemIndex&, UErrIndex&) const;
    void mapUDotErrToSubsystem(SystemUDotErrIndex, SubsystemIndex&, UDotErrIndex&) const;
    void mapMultiplierToSubsystem(SystemMultiplierIndex, SubsystemIndex&, MultiplierIndex&) const;
    void mapEventTriggerToSubsystem(SystemEventTriggerIndex, SubsystemIndex&, EventTriggerIndex&) const;
    void mapEventTriggerToStage(SystemEventTriggerIndex, Stage&, SystemEventTriggerByStageIndex&) const;
    void mapSubsystemEventTriggerToStage(EventTriggerIndex, Stage&, EventTriggerByStageIndex&) const;


    const Vector& getEventTriggers() const;
    const Vector& getEventTriggersByStage(Stage) const;
    const Vector& getEventTriggersByStage(SubsystemIndex, Stage) const;

    Vector& updEventTriggers() const; // mutable
    Vector& updEventTriggersByStage(Stage) const;
    Vector& updEventTriggersByStage(SubsystemIndex, Stage) const;

    const Vector& getQ(SubsystemIndex) const;
    const Vector& getU(SubsystemIndex) const;
    const Vector& getZ(SubsystemIndex) const;

    Vector& updQ(SubsystemIndex);
    Vector& updU(SubsystemIndex);
    Vector& updZ(SubsystemIndex);

    const Vector& getQDot(SubsystemIndex) const;
    const Vector& getUDot(SubsystemIndex) const;
    const Vector& getZDot(SubsystemIndex) const;
    const Vector& getQDotDot(SubsystemIndex) const;

    Vector& updQDot(SubsystemIndex) const;    // these are mutable
    Vector& updUDot(SubsystemIndex) const;
    Vector& updZDot(SubsystemIndex) const;
    Vector& updQDotDot(SubsystemIndex) const;

    const Vector& getQErr(SubsystemIndex) const;
    const Vector& getUErr(SubsystemIndex) const;
    const Vector& getUDotErr(SubsystemIndex) const;
    const Vector& getMultipliers(SubsystemIndex) const;
    Vector& updQErr(SubsystemIndex) const;    // these are mutable
    Vector& updUErr(SubsystemIndex) const;
    Vector& updUDotErr(SubsystemIndex) const;
    Vector& updMultipliers(SubsystemIndex) const;

    const Real&   getTime() const;
    const Vector& getY() const; // {Q,U,Z} packed and in that order

    const Vector& getQ() const;
    const Vector& getU() const;
    const Vector& getZ() const;

    Real&   updTime();  // Back up to Stage::Time-1
    Vector& updY();     // Back up to Stage::Dynamics-1

    void setTime(Real t);
    void setY(const Vector& y);

    Vector& updQ();     // Back up to Stage::Position-1
    Vector& updU();     // Back up to Stage::Velocity-1
    Vector& updZ();     // Back up to Stage::Dynamics-1

    void setQ(const Vector& q);
    void setU(const Vector& u);
    void setZ(const Vector& z);

    const Vector& getYDot()    const; // Stage::Acceleration

    const Vector& getQDot()    const; // Stage::Velocity
    const Vector& getZDot()    const; // Stage::Dynamics
    const Vector& getUDot()    const; // Stage::Acceleration

    const Vector& getQDotDot() const; // Stage::Acceleration

    Vector& updYDot() const;    // Stage::Acceleration-1
    Vector& updQDot() const;    // Stage::Velocity-1     (view into YDot)
    Vector& updZDot() const;    // Stage::Dynamics-1            "
    Vector& updUDot() const;    // Stage::Acceleration-1        "

    Vector& updQDotDot() const; // Stage::Acceleration-1

    const Vector& getYErr() const;  // Stage::Velocity

    const Vector& getQErr() const;  // Stage::Position (index 3 constraints)
    const Vector& getUErr() const;  // Stage::Velocity (index 2 constraints)

    const Vector& getUDotErr()     const; // Stage::Acceleration (index 1 constraints)
    const Vector& getMultipliers() const; // Stage::Acceleration

    Vector& updYErr() const; // Stage::Velocity-1
    Vector& updQErr() const; // Stage::Position-1 (view into YErr)
    Vector& updUErr() const; // Stage::Velocity-1        "

    Vector& updUDotErr()     const; // Stage::Acceleration-1 (not a view)
    Vector& updMultipliers() const; // Stage::Acceleration-1 (not a view)

    const Stage& getLowestStageModified() const;

    void resetLowestStageModified() const;

    void autoUpdateDiscreteVariables();
    
    void createRestrictedState(State&                       restrictedState, 
                               EnumerationSet<Stage>        restrictedStages,
                               std::set<SubsystemIndex>     restrictedSubsystems);

    const EnumerationSet<Stage>& getRestrictedStages() const;

    const std::set<SubsystemIndex>& getRestrictedSubsystems() const;

    String toString() const;
    String cacheToString() const;

private:
    // OBSOLETE: This method was misnamed in SimTK 2.0; it has been changed to 
    // isCacheValueRealized() to match markCacheValueRealized(). The old name is 
    // here as an uncallable private method in the hope of getting a helpful
    // error message out of the compiler that will lead you to the correctly-
    // named method. I guess if you're reading this, it worked!
    bool isCacheValueCurrent(SubsystemIndex sx, CacheEntryIndex cx) const
    {   return isCacheValueRealized(sx,cx); }

private:
    class StateRep* rep;
    const StateRep& getRep() const {assert(rep); return *rep;}
    StateRep&       updRep()       {assert(rep); return *rep;}
};

SimTK_SimTKCOMMON_EXPORT std::ostream& 
operator<<(std::ostream& o, const State& s);

} // namespace SimTK

#endif // SimTK_SimTKCOMMON_STATE_H_