State.h

Go to the documentation of this file.

#ifndef SimTK_SimTKCOMMON_STATE_H_
#define SimTK_SimTKCOMMON_STATE_H_

/* -------------------------------------------------------------------------- *
 *                       Simbody(tm): SimTKcommon                             *
 * -------------------------------------------------------------------------- *
 * 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.               *
 *                                                                            *
 * Portions copyright (c) 2005-12 Stanford University and the Authors.        *
 * Authors: Michael Sherman                                                   *
 * Contributors: Peter Eastman                                                *
 *                                                                            *
 * 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 "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 setNumSubsystems(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 getNumSubsystems() const;
const String& getSubsystemName   (SubsystemIndex) const;
const String& getSubsystemVersion(SubsystemIndex) const;
const Stage&  getSubsystemStage  (SubsystemIndex) const;

const Stage& getSystemStage() const;

void invalidateAll(Stage);

void invalidateAllCacheAtOrAbove(Stage) const;

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

StageVersion getSystemTopologyStageVersion() const;

QIndex allocateQ(SubsystemIndex, const Vector& qInit);
UIndex allocateU(SubsystemIndex, const Vector& uInit); 
ZIndex allocateZ(SubsystemIndex, const Vector& zInit);
QErrIndex    allocateQErr   (SubsystemIndex, int nqerr) const;   
UErrIndex    allocateUErr   (SubsystemIndex, int nuerr) const;
UDotErrIndex allocateUDotErr(SubsystemIndex, int nudoterr) const;
EventTriggerByStageIndex 
allocateEventTrigger(SubsystemIndex, Stage 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;

void markCacheValueNotRealized(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;

const Vector& getUWeights(SubsystemIndex) const;
const Vector& getZWeights(SubsystemIndex) const;

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

Vector& updUWeights(SubsystemIndex);
Vector& updZWeights(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;

const Vector& getQErrWeights(SubsystemIndex) const;
const Vector& getUErrWeights(SubsystemIndex) const;

Vector& updQErr(SubsystemIndex) const;    // these are mutable
Vector& updUErr(SubsystemIndex) const;
Vector& updUDotErr(SubsystemIndex) const;
Vector& updMultipliers(SubsystemIndex) const;

Vector& updQErrWeights(SubsystemIndex);
Vector& updUErrWeights(SubsystemIndex);

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;


const Vector& getUWeights() const;    // diag(Wu)

const Vector& getZWeights() const;

Vector& updUWeights();

Vector& updZWeights();

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

const Vector& getQErrWeights() const;

const Vector& getUErrWeights() const;

Vector& updQErrWeights();

Vector& updUErrWeights();

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)

void getSystemStageVersions(Array_<StageVersion>& versions) const;

Stage getLowestSystemStageDifference
   (const Array_<StageVersion>& prevVersions) const;

void setSystemTopologyStageVersion(StageVersion topoVersion);

void autoUpdateDiscreteVariables();

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

//------------------------------------------------------------------------------
                                private:
class StateImpl* impl;
const StateImpl& getImpl() const {assert(impl); return *impl;}
StateImpl&       updImpl()       {assert(impl); return *impl;}
};

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

} // namespace SimTK

#endif // SimTK_SimTKCOMMON_STATE_H_