1 #include "Simbody.h"
   2 
   3 #include <iostream>     // for std::cout, std::endl
   4 #include <exception>    // for std::exception
   5 
   6 using namespace SimTK;
   7 
   8     // sherm: I wanted to see the total energy so added this reporter. The time
   9     //        and energy go to the console window.
  10     //        Note that energy should be conserved from step to step by about
  11     //        the requested accuracy, i.e. 1e-5 gives about 5 decimal places.
  12     //        Expect dissipation at loose accuracies (good); energy gains at 
  13     //        *too loose* accuracy (means that integrator is unstable at that level).
  14 class EnergyReporter : public PeriodicEventReporter {
  15 public:
  16     EnergyReporter(const MultibodySystem& system, Real interval) :
  17         PeriodicEventReporter(interval), system(system) {
  18     }
  19     void handleEvent(const State& state) const {
  20         // Energy isn't calculated until Dynamics stage even though it could
  21         // be calculated earlier.
  22         system.realize(state, Stage::Dynamics);
  23 
  24         const Real totalEnergy = system.getEnergy(state);
  25         std::cout << state.getTime() <<"\t" << totalEnergy << std::endl;
  26     }
  27 private:
  28     const MultibodySystem& system;
  29 };
  30 
  31 int main() {
  32     // sherm: MOST IMPORTANT -- surround your code with a try/catch block like this
  33     //        to make sure you see error messages.
  34     //        (style note: I don't indent the code within the try/catch because it just
  35     //         ends up shoving the whole program to the right pointlessly)
  36   try { 
  37     //--------------------------------------------------------------------------------------
  38 
  39     // Create the system.
  40     MultibodySystem system;
  41     SimbodyMatterSubsystem matter(system);
  42     GeneralForceSubsystem forces(system);
  43     Force::UniformGravity gravity(forces, matter, Vec3(0, -9.8, 0));
  44 
  45         // sherm: lots of fun options for geometry. I'm making a big, red, translucent sphere here.
  46     Body::Rigid pendulumBody(MassProperties(1.0, Vec3(0), Inertia(1)));
  47     pendulumBody.addDecoration(Transform(), 
  48         DecorativeSphere(0.3).setColor(Red).setOpacity(0.5));
  49 
  50         // sherm: note this simpler way to reference existing MobilizedBodies (saves having to
  51         //        extract their indices)
  52     MobilizedBody last = matter.Ground();
  53     for (int i = 0; i < 10; ++i)
  54         last = MobilizedBody::Ball(last, Transform(Vec3(0)), pendulumBody, Transform(Vec3(0, 1, 0)));
  55 
  56     system.updDefaultSubsystem().addEventReporter(new VTKEventReporter(system, 0.02));
  57     system.updDefaultSubsystem().addEventReporter(new EnergyReporter(system, 0.02));
  58     
  59     // Initialize the system and state.
  60     
  61         // sherm: can realize and get default state in one step
  62     State state = system.realizeTopology();
  63 
  64     Random::Gaussian random;
  65     for (int i = 0; i < state.getNQ(); ++i)
  66         state.updQ()[i] = random.getValue();
  67     
  68     // Simulate it.
  69 
  70         // sherm: use RungeKuttaMerson rather than Verlet for mechanical systems.
  71         //        also, note that you can set accuracy (default is 1e-3; i.e. 0.1%)
  72     RungeKuttaMersonIntegrator integ(system);
  73     integ.setAccuracy(1e-3);    // this is just the default
  74 
  75     TimeStepper ts(system, integ);
  76     ts.initialize(state);
  77     ts.stepTo(100.0);
  78 
  79     //--------------------------------------------------------------------------------------
  80 
  81         // sherm: this catches both system exceptions and SimTK exceptions (which
  82         // derive from the standard exception class)
  83   } catch(std::exception& exc) {
  84     std::cout << "BAD NEWS -- EXCEPTION: " << exc.what() << std::endl;
  85     return 1;   // in case the calling environment is checking
  86   }
  87 
  88         // sherm: normal exit should return 0 from main
  89     return 0;
  90 }