Simbody
3.5
|
00001 #ifndef SimTK_SimTKCOMMON_SYSTEM_H_ 00002 #define SimTK_SimTKCOMMON_SYSTEM_H_ 00003 00004 /* -------------------------------------------------------------------------- * 00005 * Simbody(tm): SimTKcommon * 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) 2006-13 Stanford University and the Authors. * 00013 * Authors: Michael Sherman * 00014 * Contributors: Peter Eastman * 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 "SimTKcommon/basics.h" 00028 #include "SimTKcommon/Simmatrix.h" 00029 #include "SimTKcommon/internal/State.h" 00030 #include "SimTKcommon/internal/Subsystem.h" 00031 #include "SimTKcommon/internal/SubsystemGuts.h" 00032 00033 #include <cassert> 00034 00035 namespace SimTK { 00036 00037 class DecorativeGeometry; 00038 class DefaultSystemSubsystem; 00039 class ScheduledEventHandler; 00040 class ScheduledEventReporter; 00041 class TriggeredEventHandler; 00042 class TriggeredEventReporter; 00043 class RealizeOptions; 00044 class RealizeResults; 00045 class ProjectOptions; 00046 class ProjectResults; 00047 00048 //============================================================================== 00049 // SYSTEM 00050 //============================================================================== 00097 class SimTK_SimTKCOMMON_EXPORT System { 00098 public: 00099 class Guts; // local; name is System::Guts 00100 00101 00102 //------------------------------------------------------------------------------ 00125 System& setUpDirection(const CoordinateDirection& up); 00126 00134 System& setUseUniformBackground(bool useUniformBackground); 00135 00149 System& setDefaultTimeScale(Real tc); 00150 00158 System& setDefaultLengthScale(Real lc); 00159 00165 void setHasTimeAdvancedEvents(bool); // default=false 00166 00168 CoordinateDirection getUpDirection() const; 00171 bool getUseUniformBackground() const; 00174 Real getDefaultTimeScale() const; 00177 Real getDefaultLengthScale() const; 00180 bool hasTimeAdvancedEvents() const; 00184 //------------------------------------------------------------------------------ 00193 inline void addEventHandler(ScheduledEventHandler* handler); 00196 inline void addEventHandler(TriggeredEventHandler* handler); 00199 inline void addEventReporter(ScheduledEventReporter* handler) const; 00202 inline void addEventReporter(TriggeredEventReporter* handler) const; 00206 //------------------------------------------------------------------------------ 00235 const State& realizeTopology() const; 00236 00251 const State& getDefaultState() const; 00254 State& updDefaultState(); 00255 00274 void realizeModel(State& state) const; 00275 00283 void realize(const State& state, Stage stage = Stage::HighestRuntime) const; 00287 //------------------------------------------------------------------------------ 00368 void project(State& state, Real accuracy=-1) const; 00369 00385 void projectQ(State& state, Real accuracy=-1) const; 00386 00404 void projectU(State& state, Real accuracy=-1) const; 00405 00441 void projectQ(State& state, Vector& qErrEst, 00442 const ProjectOptions& options, ProjectResults& results) const; 00443 00467 void projectU(State& state, Vector& uErrEst, 00468 const ProjectOptions& options, ProjectResults& results) const; 00469 00470 00488 void prescribe(State& state) const { 00489 realize(state, Stage::Time); 00490 prescribeQ(state); 00491 realize(state, Stage::Position); 00492 prescribeU(state); 00493 } 00494 00513 bool prescribeQ(State& state) const; 00514 00530 bool prescribeU(State& state) const; 00531 00537 void getFreeQIndex(const State& state, Array_<SystemQIndex>& freeQs) const; 00538 00544 void getFreeUIndex(const State& state, Array_<SystemUIndex>& freeUs) const; 00548 //------------------------------------------------------------------------------ 00587 void handleEvents(State& state, 00588 Event::Cause cause, 00589 const Array_<EventId>& eventIds, 00590 const HandleEventsOptions& options, 00591 HandleEventsResults& results) const; 00592 00596 void reportEvents(const State& state, 00597 Event::Cause cause, 00598 const Array_<EventId>& eventIds) const; 00599 00608 void calcEventTriggerInfo(const State& state, 00609 Array_<EventTriggerInfo>& triggerInfo) const; 00610 00616 void calcTimeOfNextScheduledEvent(const State& state, 00617 Real& tNextEvent, 00618 Array_<EventId>& eventIds, 00619 bool includeCurrentTime) const; 00620 00624 void calcTimeOfNextScheduledReport(const State& state, 00625 Real& tNextEvent, 00626 Array_<EventId>& eventIds, 00627 bool includeCurrentTime) const; 00631 //------------------------------------------------------------------------------ 00660 void relax(State& state, Stage stage, Real accuracy=-1) const; 00664 //------------------------------------------------------------------------------ 00685 void multiplyByN(const State& state, const Vector& u, 00686 Vector& dq) const; 00688 void multiplyByNTranspose(const State& state, const Vector& fq, 00689 Vector& fu) const; 00691 void multiplyByNPInv(const State& state, const Vector& dq, 00692 Vector& u) const; 00694 void multiplyByNPInvTranspose(const State& state, const Vector& fu, 00695 Vector& fq) const; 00699 //------------------------------------------------------------------------------ 00709 void resetAllCountersToZero(); 00710 00711 // Realization 00712 00716 int getNumRealizationsOfThisStage(Stage) const; 00717 00721 int getNumRealizeCalls() const; 00722 00723 // Prescribed motion 00724 00726 int getNumPrescribeQCalls() const; 00728 int getNumPrescribeUCalls() const; 00729 00730 // Projection 00731 00734 int getNumProjectQCalls() const; 00736 int getNumFailedProjectQCalls() const; 00739 int getNumQProjections() const; 00742 int getNumQErrorEstimateProjections() const; 00743 00746 int getNumProjectUCalls() const; 00748 int getNumFailedProjectUCalls() const; 00751 int getNumUProjections() const; 00754 int getNumUErrorEstimateProjections() const; 00755 00756 // Event handling and reporting 00757 00761 int getNumHandlerCallsThatChangedStage(Stage) const; 00762 00765 int getNumHandleEventCalls() const; 00766 00769 int getNumReportEventCalls() const; 00773 //------------------------------------------------------------------------------ 00780 System() : guts(0) { } 00782 System(const System&); 00784 System& operator=(const System&); 00787 ~System(); 00788 00790 const String& getName() const; 00792 const String& getVersion() const; 00793 00796 SubsystemIndex adoptSubsystem(Subsystem& child); 00797 00799 int getNumSubsystems() const; 00801 const Subsystem& getSubsystem(SubsystemIndex) const; 00803 Subsystem& updSubsystem(SubsystemIndex); 00806 const DefaultSystemSubsystem& getDefaultSubsystem() const; 00809 DefaultSystemSubsystem& updDefaultSubsystem(); 00810 00814 inline operator const Subsystem&() const; // implemented below 00818 inline operator Subsystem&(); 00819 00824 bool systemTopologyHasBeenRealized() const; 00825 00833 StageVersion getSystemTopologyCacheVersion() const; 00834 00840 void setSystemTopologyCacheVersion(StageVersion topoVersion) const; 00841 00851 void invalidateSystemTopologyCache() const; 00852 00860 void calcDecorativeGeometryAndAppend(const State&, Stage, 00861 Array_<DecorativeGeometry>&) const; 00862 00863 00866 bool isSameSystem(const System& otherSystem) const; 00867 00868 00872 const Guts& getSystemGuts() const {assert(guts); return *guts;} 00876 Guts& updSystemGuts() {assert(guts); return *guts;} 00877 00881 void adoptSystemGuts(System::Guts* g); 00882 00884 explicit System(System::Guts* g) : guts(g) { } 00886 bool hasGuts() const {return guts!=0;} 00887 00889 bool isOwnerHandle() const; 00891 bool isEmptyHandle() const; 00894 private: 00895 friend class Guts; 00896 // This is the only data member in this class. Also, any class derived from 00897 // System must have *NO* data members at all (data goes in the Guts class). 00898 Guts* guts; 00899 }; 00900 00901 00910 class SimTK_SimTKCOMMON_EXPORT DefaultSystemSubsystem : public Subsystem { 00911 public: 00912 explicit DefaultSystemSubsystem(System& sys); 00913 void addEventHandler(ScheduledEventHandler* handler); 00914 void addEventHandler(TriggeredEventHandler* handler); 00915 void addEventReporter(ScheduledEventReporter* handler) const; 00916 void addEventReporter(TriggeredEventReporter* handler) const; 00917 EventId createEventId(SubsystemIndex subsys, const State& state) const; 00918 void findSubsystemEventIds 00919 (SubsystemIndex subsys, const State& state, 00920 const Array_<EventId>& allEvents, 00921 Array_<EventId>& eventsForSubsystem) const; 00922 // don't let doxygen see this private class 00924 class Guts; 00926 private: 00927 const Guts& getGuts() const; 00928 Guts& updGuts(); 00929 }; 00930 00931 inline void System::addEventHandler(ScheduledEventHandler* handler) 00932 { updDefaultSubsystem().addEventHandler(handler); } 00933 inline void System::addEventHandler(TriggeredEventHandler* handler) 00934 { updDefaultSubsystem().addEventHandler(handler); } 00935 inline void System::addEventReporter(ScheduledEventReporter* handler) const 00936 { getDefaultSubsystem().addEventReporter(handler); } 00937 inline void System::addEventReporter(TriggeredEventReporter* handler) const 00938 { getDefaultSubsystem().addEventReporter(handler); } 00939 00940 inline System::operator const Subsystem&() const {return getDefaultSubsystem();} 00941 inline System::operator Subsystem&() {return updDefaultSubsystem();} 00942 00943 00944 //============================================================================== 00945 // PROJECT OPTIONS and PROJECT RESULTS 00946 //============================================================================== 00950 class ProjectOptions { 00951 public: 00952 enum Option { 00954 None = 0x0000, 00959 LocalOnly = 0x0001, 00963 DontThrow = 0x0002, 00966 UseInfinityNorm = 0x0004, 00970 ForceProjection = 0x0008, 00974 ForceFullNewton = 0x0010 00975 }; 00976 00978 ProjectOptions() {clear();} 00982 explicit ProjectOptions(Real accuracy) 00983 { clear(); setRequiredAccuracy(accuracy); } 00986 explicit ProjectOptions(Option opt) 00987 { clear(); setOption(opt); } 00988 00992 ProjectOptions& clear() 00993 { optionSet=0; setAccuracyDefaults(); return *this; } 00994 00999 ProjectOptions& setRequiredAccuracy(Real accuracy) { 01000 requiredAccuracy = accuracy > 0 ? accuracy 01001 : getDefaultRequiredAccuracy(); 01002 return *this; 01003 } 01004 01007 ProjectOptions& setOvershootFactor(Real overshoot) { 01008 assert(0 < overshoot && overshoot <= 1); 01009 desiredOvershoot = overshoot; 01010 return *this; 01011 } 01012 01015 ProjectOptions& setProjectionLimit(Real limit) { 01016 assert(limit > 0); 01017 projectionLimit = limit; 01018 return *this; 01019 } 01020 01023 ProjectOptions& clearOption(Option opt) 01024 { optionSet &= ~(unsigned)opt; return *this; } 01026 ProjectOptions& setOption (Option opt) 01027 { optionSet |= (unsigned)opt; return *this; } 01028 01030 Real getRequiredAccuracy() const {return requiredAccuracy;} 01033 Real getOvershootFactor() const {return desiredOvershoot;} 01035 Real getProjectionLimit() const {return projectionLimit;} 01036 01037 bool isOptionSet(Option opt) const {return (optionSet&(unsigned)opt) != 0;} 01038 01039 static Real getDefaultRequiredAccuracy() {return Real(1e-4);} 01040 static Real getDefaultOvershootFactor() {return Real(0.1);} //i.e., 1e-5 01041 01042 // Set operators: not, or, and, set difference 01043 ProjectOptions& operator|=(const ProjectOptions& opts) 01044 { optionSet |= opts.optionSet; return *this; } 01045 ProjectOptions& operator&=(const ProjectOptions& opts) 01046 { optionSet &= opts.optionSet; return *this; } 01047 ProjectOptions& operator-=(const ProjectOptions& opts) 01048 { optionSet &= ~opts.optionSet; return *this; } 01049 01050 ProjectOptions& operator|=(Option opt) {setOption(opt); return *this;} 01051 ProjectOptions& operator-=(Option opt) {clearOption(opt); return *this;} 01052 01053 private: 01054 Real requiredAccuracy; 01055 Real desiredOvershoot; // try for accuracy*overshoot 01056 Real projectionLimit; // abort if initial norm is worse than this 01057 unsigned optionSet; 01058 01059 void setAccuracyDefaults() { 01060 requiredAccuracy = getDefaultRequiredAccuracy(); 01061 desiredOvershoot = getDefaultOvershootFactor(); 01062 projectionLimit = Infinity; // we'll try from however far away 01063 } 01064 }; 01065 01067 class ProjectResults { 01068 public: 01069 ProjectResults() {clear();} 01070 01071 enum Status { 01073 Invalid = -1, 01076 Succeeded = 0, 01079 FailedToAchieveAccuracy = 1, 01084 FailedToConverge = 2 01085 }; 01086 01089 ProjectResults& clear() { 01090 m_exitStatus = Invalid; 01091 m_anyChangeMade = m_projectionLimitExceeded = false; 01092 m_numIterations = 0; 01093 m_worstError = -1; 01094 m_normOnEntrance = m_normOnExit = NaN; 01095 return *this; 01096 } 01097 bool isValid() const {return m_exitStatus != Invalid;} 01098 Status getExitStatus() const {return m_exitStatus;} 01099 01100 bool getAnyChangeMade() const {assert(isValid());return m_anyChangeMade;} 01101 int getNumIterations() const {assert(isValid());return m_numIterations;} 01102 Real getNormOnEntrance() const {assert(isValid());return m_normOnEntrance;} 01103 Real getNormOnExit() const {assert(isValid());return m_normOnExit;} 01104 int getWorstErrorOnEntrance() const 01105 { assert(isValid());return m_worstError; } 01106 bool getProjectionLimitExceeded() const 01107 { assert(isValid());return m_projectionLimitExceeded; } 01108 01109 ProjectResults& setExitStatus(Status status) 01110 { m_exitStatus=status; return *this; } 01111 ProjectResults& setAnyChangeMade(bool changeMade) 01112 { m_anyChangeMade=changeMade; return *this; } 01113 ProjectResults& setProjectionLimitExceeded(bool limitExceeded) 01114 { m_projectionLimitExceeded=limitExceeded; return *this; } 01115 ProjectResults& setNumIterations(int numIterations) 01116 { m_numIterations=numIterations; return *this; } 01117 ProjectResults& setNormOnEntrance(Real norm, int worstError) 01118 { m_normOnEntrance=norm; m_worstError=worstError; return *this; } 01119 ProjectResults& setNormOnExit(Real norm) 01120 { m_normOnExit=norm; return *this; } 01121 private: 01122 Status m_exitStatus; 01123 bool m_anyChangeMade; 01124 bool m_projectionLimitExceeded; 01125 int m_numIterations; 01126 int m_worstError; // index of worst error on entrance 01127 Real m_normOnEntrance; // in selected rms or infinity norm 01128 Real m_normOnExit; 01129 }; 01130 01131 01132 01133 //============================================================================== 01134 // REALIZE OPTIONS and REALIZE RESULTS 01135 //============================================================================== 01137 class RealizeOptions { 01138 unsigned int optionSet; 01139 explicit RealizeOptions(unsigned o) : optionSet(o) { } 01140 public: 01141 01142 enum Option { 01143 None = 0x00, 01144 DontThrow = 0x01 01145 }; 01146 01147 01148 RealizeOptions() : optionSet(0) { } 01149 01150 // This is an implicit conversion 01151 RealizeOptions(Option opt) : optionSet((unsigned)opt) { } 01152 01153 // Implicit conversion to bool when needed 01154 operator bool() const {return optionSet != 0;} 01155 bool isEmpty() const {return optionSet==0;} 01156 01157 bool isOptionSet(Option opt) const {return (optionSet&(unsigned)opt) != 0;} 01158 void clear() {optionSet=0;} 01159 void clearOption(Option opt) {optionSet &= ~(unsigned)opt;} 01160 void setOption (Option opt) {optionSet |= (unsigned)opt;} 01161 01162 // Set operators: or, and 01163 RealizeOptions& operator|=(RealizeOptions opts) {optionSet |= opts.optionSet; return *this;} 01164 RealizeOptions& operator&=(RealizeOptions opts) {optionSet &= opts.optionSet; return *this;} 01165 01166 RealizeOptions& operator|=(Option opt) {setOption(opt); return *this;} 01167 RealizeOptions& operator-=(Option opt) {clearOption(opt); return *this;} 01168 }; 01169 01171 class RealizeResults { 01172 }; 01173 01174 01175 01176 01177 } // namespace SimTK 01178 01179 #endif // SimTK_SimTKCOMMON_SYSTEM_H_