1 #ifndef SimTK_SIMMATH_INTEGRATOR_TEST_FRAMEWORK_H_ 2 #define SimTK_SIMMATH_INTEGRATOR_TEST_FRAMEWORK_H_ 3 4 /* -------------------------------------------------------------------------- * 5 * SimTK Core: SimTK Simmath(tm) * 6 * -------------------------------------------------------------------------- * 7 * This is part of the SimTK Core biosimulation toolkit originating from * 8 * Simbios, the NIH National Center for Physics-Based Simulation of * 9 * Biological Structures at Stanford, funded under the NIH Roadmap for * 10 * Medical Research, grant U54 GM072970. See https://simtk.org. * 11 * * 12 * Portions copyright (c) 2006-7 Stanford University and the Authors. * 13 * Authors: Michael Sherman, Peter Eastman * 14 * Contributors: * 15 * * 16 * Permission is hereby granted, free of charge, to any person obtaining a * 17 * copy of this software and associated documentation files (the "Software"), * 18 * to deal in the Software without restriction, including without limitation * 19 * the rights to use, copy, modify, merge, publish, distribute, sublicense, * 20 * and/or sell copies of the Software, and to permit persons to whom the * 21 * Software is furnished to do so, subject to the following conditions: * 22 * * 23 * The above copyright notice and this permission notice shall be included in * 24 * all copies or substantial portions of the Software. * 25 * * 26 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * 27 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * 28 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL * 29 * THE AUTHORS, CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, * 30 * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR * 31 * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE * 32 * USE OR OTHER DEALINGS IN THE SOFTWARE. * 33 * -------------------------------------------------------------------------- */ 34 35 /** 36 * This file contains code which is used for testing various integrators. 37 */ 38 39 #include "SimTKcommon.h" 40 41 #include "SimTKmath.h" 42 #include "simmath/TimeStepper.h" 43 44 #include "PendulumSystem.h" 45 46 #define ASSERT(cond) {SimTK_ASSERT_ALWAYS(cond, "Assertion failed");} 47 48 using namespace SimTK; 49 50 using std::printf; 51 using std::cout; 52 using std::endl; 53 54 class PeriodicHandler : public PeriodicEventHandler { 55 public: 56 static int eventCount; 57 static PeriodicHandler* handler; PeriodicHandler()58 PeriodicHandler() : PeriodicEventHandler(1.0) { 59 } handleEvent(State & state,Real accuracy,bool & shouldTerminate)60 void handleEvent(State& state, Real accuracy, bool& shouldTerminate) const override { 61 62 // This should be triggered every (interval) time units. 63 64 ASSERT(state.getTime() == getNextEventTime(state, true)); 65 eventCount++; 66 } 67 }; 68 69 class ZeroPositionHandler : public TriggeredEventHandler { 70 public: 71 static int eventCount; 72 static Real lastEventTime; 73 static bool hasAccelerated; ZeroPositionHandler(PendulumSystem & pendulum)74 ZeroPositionHandler(PendulumSystem& pendulum) : TriggeredEventHandler(Stage::Velocity), pendulum(pendulum) { 75 } getValue(const State & state)76 Real getValue(const State& state) const override { 77 return state.getQ(pendulum.getGuts().getSubsysIndex())[0]; 78 } handleEvent(State & state,Real accuracy,bool & shouldTerminate)79 void handleEvent(State& state, Real accuracy, bool& shouldTerminate) const override { 80 81 // This should be triggered when the pendulum crosses x == 0. 82 83 Real x = state.getQ(pendulum.getGuts().getSubsysIndex())[0]; 84 ASSERT(std::abs(x) < 0.01); 85 ASSERT(state.getTime() > lastEventTime); 86 eventCount++; 87 lastEventTime = state.getTime(); 88 if (state.getTime() > 7 && !hasAccelerated) { 89 90 // Multiply the pendulum's velocity by sqrt(1.5), which should multiply its total energy by 1.5 (since 91 // at x=0, all of its energy is kinetic). 92 93 hasAccelerated = true; 94 SubsystemIndex subsys = pendulum.getGuts().getSubsysIndex(); 95 state.updU(subsys) *= std::sqrt(1.5); 96 } 97 } 98 private: 99 PendulumSystem& pendulum; 100 }; 101 102 class ZeroVelocityHandler : public TriggeredEventHandler { 103 public: 104 static int eventCount; 105 static Real lastEventTime; ZeroVelocityHandler(PendulumSystem & pendulum)106 ZeroVelocityHandler(PendulumSystem& pendulum) : TriggeredEventHandler(Stage::Velocity), pendulum(pendulum) { 107 getTriggerInfo().setTriggerOnFallingSignTransition(false); 108 } getValue(const State & state)109 Real getValue(const State& state) const override { 110 return state.getU(pendulum.getGuts().getSubsysIndex())[0]; 111 } handleEvent(State & state,Real accuracy,bool & shouldTerminate)112 void handleEvent(State& state, Real accuracy, bool& shouldTerminate) const override { 113 114 // This should be triggered when the pendulum reaches its farthest point in the 115 // negative direction: q[0] == -1, u[0] == 0. 116 117 Vector u = state.getU(pendulum.getGuts().getSubsysIndex()); 118 ASSERT(std::abs(u[0]) < 0.01); 119 ASSERT(state.getTime() > lastEventTime); 120 eventCount++; 121 lastEventTime = state.getTime(); 122 } 123 private: 124 PendulumSystem& pendulum; 125 }; 126 127 class PeriodicReporter : public PeriodicEventReporter { 128 public: 129 static int eventCount; 130 static PeriodicReporter* reporter; PeriodicReporter(PendulumSystem & pendulum)131 PeriodicReporter(PendulumSystem& pendulum) : PeriodicEventReporter(1.0), pendulum(pendulum) { 132 } handleEvent(const State & state)133 void handleEvent(const State& state) const override { 134 135 // This should be triggered every (interval) time units. 136 137 ASSERT(state.getTime() == getNextEventTime(state, true)); 138 eventCount++; 139 140 // Verify conservation of energy. 141 142 const Vector q = state.getQ(pendulum.getGuts().getSubsysIndex()); 143 const Vector u = state.getU(pendulum.getGuts().getSubsysIndex()); 144 Real energy = pendulum.getMass(state)*(0.5*(u[0]*u[0]+u[1]*u[1]) 145 + pendulum.getGravity(state)*(1.0+q[1])); 146 Real expectedEnergy = pendulum.getMass(state) 147 * pendulum.getGravity(state); 148 if (ZeroPositionHandler::hasAccelerated) 149 expectedEnergy *= 1.5; 150 ASSERT(std::abs(1.0-energy/expectedEnergy) < 0.05); 151 } 152 private: 153 PendulumSystem& pendulum; 154 }; 155 156 class OnceOnlyEventReporter : public ScheduledEventReporter { 157 public: 158 static bool hasOccurred; OnceOnlyEventReporter()159 OnceOnlyEventReporter() { 160 } getNextEventTime(const State &,bool includeCurrentTime)161 Real getNextEventTime(const State&, bool includeCurrentTime) const override { 162 return 5.0; 163 } handleEvent(const State & state)164 void handleEvent(const State& state) const override { 165 ASSERT(!hasOccurred); 166 hasOccurred = true; 167 } 168 }; 169 170 class DiscontinuousReporter : public TriggeredEventReporter { 171 public: 172 static int eventCount; DiscontinuousReporter()173 DiscontinuousReporter() : TriggeredEventReporter(Stage::Time) { 174 } getValue(const State & state)175 Real getValue(const State& state) const override { 176 Real step = std::floor(state.getTime()); 177 step = std::fmod(step, 4.0); 178 if (step == 0.0) 179 return 1.0; 180 if (step == 2.0) 181 return -1.0; 182 return 0.0; 183 } handleEvent(const State & state)184 void handleEvent(const State& state) const override { 185 186 // This should be triggered when the value goes to 0, but not when it leaves 0. 187 188 Real t = state.getTime(); 189 Real phase = std::fmod(t, 2.0); 190 ASSERT(std::abs(phase-1.0) < 0.01); 191 eventCount++; 192 } 193 }; 194 195 196 int ZeroVelocityHandler::eventCount = 0; 197 Real ZeroVelocityHandler::lastEventTime = 0.0; 198 int PeriodicHandler::eventCount = 0; 199 PeriodicHandler* PeriodicHandler::handler = 0; 200 int ZeroPositionHandler::eventCount = 0; 201 Real ZeroPositionHandler::lastEventTime = 0.0; 202 bool ZeroPositionHandler::hasAccelerated = false; 203 int PeriodicReporter::eventCount = 0; 204 PeriodicReporter* PeriodicReporter::reporter = 0; 205 bool OnceOnlyEventReporter::hasOccurred = false; 206 int DiscontinuousReporter::eventCount = 0; 207 208 void testIntegrator (Integrator& integ, PendulumSystem& sys, Real accuracy=1e-4) { 209 ZeroVelocityHandler::eventCount = 0; 210 ZeroVelocityHandler::lastEventTime = 0.0; 211 PeriodicHandler::eventCount = 0; 212 ZeroPositionHandler::eventCount = 0; 213 ZeroPositionHandler::lastEventTime = 0.0; 214 ZeroPositionHandler::hasAccelerated = false; 215 PeriodicReporter::eventCount = 0; 216 OnceOnlyEventReporter::hasOccurred = false; 217 DiscontinuousReporter::eventCount = 0; 218 219 const Real t0=0; 220 const Real tFinal = 20.003; 221 const Real qi[] = {1,0}; // (x,y)=(1,0) 222 const Real ui[] = {0,0}; // v=0 223 const Vector q0(2, qi); 224 const Vector u0(2, ui); 225 226 sys.setDefaultMass(10); 227 sys.setDefaultTimeAndState(t0, q0, u0); 228 integ.setAccuracy(accuracy); 229 integ.setConstraintTolerance(1e-4); 230 integ.setFinalTime(tFinal); 231 232 TimeStepper ts(sys); 233 ts.setIntegrator(integ); 234 ts.initialize(sys.getDefaultState()); 235 236 // Try taking a series of steps of constant size. 237 238 ASSERT(ts.getTime() == 0.0); 239 Real time = 1.0; 240 for (; time < 5.0; time += 1.0) { 241 ts.stepTo(time); 242 ASSERT(ts.getTime() == time); 243 } 244 ASSERT(!OnceOnlyEventReporter::hasOccurred); 245 ASSERT(!ZeroPositionHandler::hasAccelerated); 246 247 // Try some steps of random sizes. 248 249 static Random::Uniform random(0.0, 1.0); 250 for (; time < 10.0; time += random.getValue()) { 251 ASSERT(OnceOnlyEventReporter::hasOccurred == (ts.getTime() >= 5.0)); 252 ts.stepTo(time); 253 ASSERT(ts.getTime() == time); 254 } 255 ASSERT(ZeroPositionHandler::hasAccelerated); 256 257 // Try one large step that goes beyond tFinal. 258 259 ts.stepTo(50.0); 260 ASSERT(ts.getTime() == tFinal); 261 ASSERT(integ.getTerminationReason() == Integrator::ReachedFinalTime); 262 ASSERT(ZeroVelocityHandler::eventCount > 10); 263 ASSERT(PeriodicHandler::eventCount == (int) (ts.getTime()/PeriodicHandler::handler->getEventInterval())+1); 264 ASSERT(ZeroPositionHandler::eventCount > 10); 265 ASSERT(PeriodicReporter::eventCount == (int) (ts.getTime()/PeriodicReporter::reporter->getEventInterval())+1); 266 ASSERT(DiscontinuousReporter::eventCount == (int) (ts.getTime()/2.0)); 267 } 268 269 #endif /*SimTK_SIMMATH_INTEGRATOR_TEST_FRAMEWORK_H_*/ 270