1 /* -------------------------------------------------------------------------- *
2  *                        Simbody(tm): SimTKmath                              *
3  * -------------------------------------------------------------------------- *
4  * This is part of the SimTK biosimulation toolkit originating from           *
5  * Simbios, the NIH National Center for Physics-Based Simulation of           *
6  * Biological Structures at Stanford, funded under the NIH Roadmap for        *
7  * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody.  *
8  *                                                                            *
9  * Portions copyright (c) 2006-12 Stanford University and the Authors.        *
10  * Authors: Michael Sherman, Peter Eastman                                    *
11  * Contributors:                                                              *
12  *                                                                            *
13  * Licensed under the Apache License, Version 2.0 (the "License"); you may    *
14  * not use this file except in compliance with the License. You may obtain a  *
15  * copy of the License at http://www.apache.org/licenses/LICENSE-2.0.         *
16  *                                                                            *
17  * Unless required by applicable law or agreed to in writing, software        *
18  * distributed under the License is distributed on an "AS IS" BASIS,          *
19  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *
20  * See the License for the specific language governing permissions and        *
21  * limitations under the License.                                             *
22  * -------------------------------------------------------------------------- */
23 
24 #include "SimTKmath.h"
25 
26 #include "PendulumSystem.h"
27 
28 #define ASSERT(cond) {SimTK_ASSERT_ALWAYS(cond, "Assertion failed");}
29 
30 using namespace SimTK;
31 
32 using std::printf;
33 using std::cout;
34 using std::endl;
35 
36 class ZeroVelocityHandler : public TriggeredEventHandler {
37 public:
38     static int eventCount;
39     static Real lastEventTime;
ZeroVelocityHandler(PendulumSystem & pendulum)40     ZeroVelocityHandler(PendulumSystem& pendulum) : TriggeredEventHandler(Stage::Velocity), pendulum(pendulum) {
41         getTriggerInfo().setTriggerOnFallingSignTransition(false);
42     }
getValue(const State & state) const43     Real getValue(const State& state) const override {
44         return state.getU(pendulum.getGuts().getSubsysIndex())[0];
45     }
handleEvent(State & state,Real accuracy,bool & shouldTerminate) const46     void handleEvent(State& state, Real accuracy, bool& shouldTerminate) const override {
47 
48         // This should be triggered when the pendulum reaches its farthest point in the
49         // negative direction: q == -1, u == 0.
50 
51         Real q = state.getQ(pendulum.getGuts().getSubsysIndex())[0];
52         Real u = state.getU(pendulum.getGuts().getSubsysIndex())[0];
53         ASSERT(std::abs(q+1.0) < 0.05);
54         ASSERT(std::abs(u) < 0.01);
55         ASSERT(state.getTime() > lastEventTime);
56         eventCount++;
57         lastEventTime = state.getTime();
58     }
59 private:
60     PendulumSystem& pendulum;
61 };
62 
63 class PeriodicHandler : public ScheduledEventHandler {
64 public:
65     static int eventCount;
66     static Real lastEventTime;
getNextEventTime(const State &,bool includeCurrentTime) const67     Real getNextEventTime(const State&, bool includeCurrentTime) const override {
68         return lastEventTime+1.5;
69     }
handleEvent(State & state,Real accuracy,bool & shouldTerminate) const70     void handleEvent(State& state, Real accuracy, bool& shouldTerminate) const override {
71 
72         // This should be triggered every 1.5 time units.
73 
74         ASSERT(state.getTime() == lastEventTime+1.5);
75         eventCount++;
76         lastEventTime = state.getTime();
77     }
78 };
79 
80 class ZeroPositionReporter : public TriggeredEventReporter {
81 public:
82     static int eventCount;
83     static Real lastEventTime;
ZeroPositionReporter(PendulumSystem & pendulum)84     ZeroPositionReporter(PendulumSystem& pendulum) : TriggeredEventReporter(Stage::Velocity), pendulum(pendulum) {
85     }
getValue(const State & state) const86     Real getValue(const State& state) const override {
87         return state.getQ(pendulum.getGuts().getSubsysIndex())[0];
88     }
handleEvent(const State & state) const89     void handleEvent(const State& state) const override {
90 
91         // This should be triggered when the pendulum crosses q == 0.
92 
93         Real q = state.getQ(pendulum.getGuts().getSubsysIndex())[0];
94         ASSERT(std::abs(q) < 0.01);
95         ASSERT(state.getTime() > lastEventTime);
96         eventCount++;
97         lastEventTime = state.getTime();
98     }
99 private:
100     PendulumSystem& pendulum;
101 };
102 
103 class PeriodicReporter : public ScheduledEventReporter {
104 public:
105     static int eventCount;
106     static Real lastEventTime;
getNextEventTime(const State &,bool includeCurrentTime) const107     Real getNextEventTime(const State&, bool includeCurrentTime) const override {
108         return lastEventTime*2;
109     }
handleEvent(const State & state) const110     void handleEvent(const State& state) const override {
111 
112         // This should be triggered every 1.5 time units.
113 
114         ASSERT(state.getTime() == lastEventTime*2);
115         eventCount++;
116         lastEventTime = state.getTime();
117     }
118 };
119 
120 int ZeroVelocityHandler::eventCount = 0;
121 Real ZeroVelocityHandler::lastEventTime = 0.0;
122 int PeriodicHandler::eventCount = 0;
123 Real PeriodicHandler::lastEventTime = 0.0;
124 int ZeroPositionReporter::eventCount = 0;
125 Real ZeroPositionReporter::lastEventTime = 0.0;
126 int PeriodicReporter::eventCount = 0;
127 Real PeriodicReporter::lastEventTime = 0.5;
128 
main()129 int main () {
130   try {
131     PendulumSystem sys;
132     sys.addEventHandler(new ZeroVelocityHandler(sys));
133     sys.addEventHandler(new PeriodicHandler());
134     sys.addEventReporter(new ZeroPositionReporter(sys));
135     sys.addEventReporter(new PeriodicReporter());
136     sys.realizeTopology();
137 
138     RungeKuttaMersonIntegrator integ(sys);
139 
140     const Real t0=0;
141     const Real qi[] = {1,0}; // (x,y)=(1,0)
142     const Real ui[] = {0,0}; // v=0
143     const Vector q0(2, qi);
144     const Vector u0(2, ui);
145 
146     sys.setDefaultMass(10);
147     sys.setDefaultTimeAndState(t0, q0, u0);
148 
149     integ.setAccuracy(1e-2);
150     integ.setConstraintTolerance(1e-4);
151 
152     const Real tFinal = 20.003;
153     const Real hReport = 1.;
154 
155     integ.setFinalTime(tFinal);
156 
157     TimeStepper ts(sys);
158     ts.setIntegrator(integ);
159     ts.initialize(sys.getDefaultState());
160     ASSERT(ts.getTime() == 0.0);
161     ts.stepTo(10.0);
162     ASSERT(ts.getTime() == 10.0);
163     ts.stepTo(50.0);
164     ASSERT(ts.getTime() == tFinal);
165     ASSERT(integ.getTerminationReason() == Integrator::ReachedFinalTime);
166     ASSERT(ZeroVelocityHandler::eventCount >= 10);
167     ASSERT(PeriodicHandler::eventCount == (int) (ts.getTime()/1.5));
168     ASSERT(ZeroPositionReporter::eventCount > 10);
169     ASSERT(PeriodicReporter::eventCount == (int) (std::log(ts.getTime())/std::log(2.0))+1);
170     cout << "Done" << endl;
171     return 0;
172   }
173   catch (std::exception& e) {
174     std::printf("FAILED: %s\n", e.what());
175     return 1;
176   }
177 }
178