#!/cygdrive/c/Python26/python.exe
# Based on TestPendulum.cpp SimTK example program

import sys
import simtk.molmodel as ss

# Create the system
system = ss.MultibodySystem()
matter = ss.SimbodyMatterSubsystem(system) 
forces = ss.GeneralForceSubsystem(system) 
gravity = ss.Force.UniformGravity(forces, matter, (0, -9.8, 0))
pendulumBody = ss.Body.Rigid(ss.MassProperties(1.0, (0,0,0), ss.Inertia(1))) 
pendulumBody.addDecoration(ss.Transform(), ss.DecorativeSphere(0.1)) 
pendulum1 = ss.MobilizedBody.Pin(matter.Ground(), ss.Transform((0, 0, 0)), 
        pendulumBody, ss.Transform((0, 1, 0)))
pendulum2 = ss.MobilizedBody.Pin(pendulum1, ss.Transform((0, 0, 0)), 
        pendulumBody, ss.Transform((0, 1, 0)))
vtkReporter = ss.VTKEventReporter(system, 0.01)
system.updDefaultSubsystem().addEventReporter(vtkReporter) 

# Initialize the system and state.
 
system.realizeTopology()
state = system.defaultState
pendulum2.setOneU(state, 0, 5.0)
 
# Simulate it. 

integ = ss.VerletIntegrator(system) 
ts = ss.TimeStepper(system, integ) 
ts.initialize(state) 

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