#!C:/Python26/python.exe
# Based on TestPendulum.cpp SimTK example program

from molmodel.simtk import *

# Create the system. 
 
system = MultibodySystem()
matter = SimbodyMatterSubsystem(system) 
forces = GeneralForceSubsystem(system) 
gravity = Force.UniformGravity(forces, matter, Vec3(0, -9.8, 0)) 
pendulumBody = Body.Rigid(MassProperties(1.0, Vec3(0), Inertia(1))) 
pendulumBody.addDecoration(Transform(), DecorativeSphere(0.1)) 
pendulum1 = MobilizedBody.Pin(matter.Ground(), Transform(Vec3(0)), 
        pendulumBody, Transform(Vec3(0, 1, 0))) 
pendulum2 = MobilizedBody.Pin(pendulum1, Transform(Vec3(0)), 
        pendulumBody, Transform(Vec3(0, 1, 0))) 
system.defaultSubsystem().addEventReporter(VTKEventReporter(system, 0.01)) 
 
# Initialize the system and state. 
 
system.realizeTopology()   
state = system.defaultState() 
pendulum2.setOneU(state, 0, 5.0)   
 
# Simulate it. 
 
integ = VerletIntegrator(system) 
ts = TimeStepper(system, integ) 
ts.initialize(state) 

print "About to simulate..."
ts.stepTo(3.0) 
print "Simulation complete"