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