from simbios.simtk import * import math import sys Deg2Rad = math.pi / 180.0 Rad2Deg = 180.0 / math.pi GroundFrame = Transform() m = 1.0 # kg g = 9.8 # meters/s**2 apply in -y direction d = 0.5 # meters def get_input(prompt): "raw_input() doesn't flush!?!?" sys.stdout.write(prompt) sys.stdout.flush() return raw_input() def main(): # CREATE MULTIBODY SYSTEM AND ITS SUBSYSTEMS mbs = MultibodySystem() crankRocker = SimbodyMatterSubsystem(mbs) forces = GeneralForceSubsystem(mbs) viz = DecorationSubsystem(mbs) gravity = Force.UniformGravity(forces, crankRocker, Vec3(0, -g, 0)) # ADD BODIES AND THEIR MOBILIZERS crankBody = Body.Rigid(MassProperties(.1, Vec3(0), 0.1*Gyration.brick(1,3,.5)) ).addDecoration(Transform(), DecorativeEllipsoid(0.1*Vec3(1,3,.4) ).setResolution(10).setOpacity(.2)) sliderBody = Body.Rigid(MassProperties(.2, Vec3(0), 0.2*Gyration.brick(1,5,.5)) ).addDecoration(Transform(), DecorativeEllipsoid(0.2*Vec3(1,5,.4) ).setColor(Blue).setResolution(10).setOpacity(.2)) longBar = Body.Rigid(MassProperties(0.01, Vec3(0), 0.01*Gyration.cylinderAlongX(.1, 5)) ).addDecoration(Rotation(math.pi/2,ZAxis), DecorativeCylinder(.01, 1)) crank = MobilizedBody.Pin( crankRocker.Ground(), Transform(), crankBody, Transform(Vec3(0, .15, 0))) slider = MobilizedBody.Pin( crankRocker.Ground(), Transform(Vec3(2.5,0,0)), sliderBody, Transform(Vec3(0, 1, 0))) rightConn = MobilizedBody.Universal( crank, Transform(Rotation(-math.pi/2,YAxis),Vec3(0,-.3,0)), longBar, Transform(Rotation(-math.pi/2,YAxis),Vec3(-1,0,0))) ball = Constraint.Ball(slider, Vec3(0,-1, 0), rightConn, Vec3(1,0,0)) ball.setDefaultRadius(0.01) #Constraint.Rod rodl(leftConn, Vec3(0), rightConn, Vec3(-2.5,0,0), 2.5) #Constraint.Rod rodr(leftConn, Vec3(2.5,-1,0), rightConn, Vec3(-1.5,0,0), std.sqrt(2.)) #Constraint.Rod rodo(leftConn, Vec3(1.5,0,0), rightConn, Vec3(-2.5,1,0), std.sqrt(2.)) #Constraint.Rod rodl(leftConn, Vec3(-2.5,0,0), rightConn, Vec3(-2.5,0,0), 5) #Constraint.Rod rodr(leftConn, Vec3(2.5,0,0), rightConn, Vec3(2.5,0,0), 5) #Constraint.Rod rodo(leftConn, Vec3(2.5,-1,0), rightConn, Vec3(-2.5,1,0), 2) # Add visualization line (orange=spring, black=constraint) #viz.addRubberBandLine(crank, Vec3(0,-3,0), # slider, Vec3(0,-5,0), # DecorativeLine().setColor(Black).setLineThickness(4)) #forces.addMobilityConstantForce(leftPendulum, 0, 20) #forces.addCustomForce(ShermsForce(leftPendulum,rightPendulum)) #forces.addGlobalEnergyDrain(1) Force.MobilityConstantForce(forces, crank, 0, 1) Force.MobilityLinearDamper(forces, crank, 0, 1.0) #Motion.Linear(crank, Vec3(a,b,c)) # crank(t)=at^2+bt+c #Motion.Linear lmot(rightConn, Vec3(a,b,c)) # both axes follow #lmot.setAxis(1, Vec3(d,e,f)) #Motion.Orientation(someBall, orientFuncOfT) #someBall.prescribeOrientation(orientFunc) #Motion.Relax(crank) # acc=vel=0, pos determined by some default relaxation solver # Like this, or should this be an Instance-stage mode of every mobilizer? #Motion.Lock(crank) # acc=vel=0 pos is discrete or fast #Motion.Steady(crank, Vec3(1,2,3)) # acc=0 vel=constant pos integrated #Motion.Steady crankRate(crank, 1) # acc=0 vel=constant, same for each axis pos integrated # or ... #crank.lock(state) #crank.setMotionType(state, Regular|Discrete|Fast|Prescribed, stage) # need a way to register a mobilizer with a particular relaxation solver, # switch between dynamic, continuous relaxed, end-of-step relaxed, discrete. # what about a "local" (explicit) relaxation, like q=(q1+q2)/2 ? s = mbs.realizeTopology() # returns a reference to the the default state mbs.realizeModel(s) # define appropriate states for this System #crankRate.setRate(s, 3) crank.setAngle(s, 5) #q crank.setRate(s, 3) #u display = VTKVisualizer(mbs) mbs.realize(s, Stage.Position) display.report(s) print "q=", s.getQ() print "qErr=", s.getQErr() crank.setAngle(s, -30*Deg2Rad) #crank.setQToFitRotation (s, Rotation.aboutZ(-.9*math.pi/2)) #TODO #rightPendulum.setUToFitLinearVelocity(s, Vec3(1.1,0,1.2)) #crank.setUToFitAngularVelocity(s, 10*Vec3(.1,.2,.3)) mbs.realize(s, Stage.Velocity) display.report(s) print "q=", s.getQ() print "qErr=", s.getQErr() # These are the SimTK Simmath integrators: myStudy = RungeKuttaMersonIntegrator(mbs) #CPodesIntegrator myStudy(mbs, CPodes.BDF, CPodes.Newton) #myStudy.setMaximumStepSize(0.001) myStudy.setAccuracy(1e-3) #myStudy.setProjectEveryStep(True) #myStudy.setAllowInterpolation(False) #myStudy.setMaximumStepSize(.1) dt = .02 # output intervals finalTime = 10 myStudy.setFinalTime(finalTime) get_input("press enter >") display.report(s) get_input("press enter >") # Peforms assembly if constraints are violated. myStudy.initialize(s) myStudy.setProjectEveryStep(False) myStudy.setConstraintTolerance(.001) myStudy.initialize(myStudy.getState()) print "Using Integrator ", std.string(myStudy.getMethodName()), ":\n" print "ACCURACY IN USE=", myStudy.getAccuracyInUse() print "CTOL IN USE=", myStudy.getConstraintToleranceInUse() print "TIMESCALE=", myStudy.getTimeScaleInUse() print "Y WEIGHTS=", myStudy.getStateWeightsInUse() print "1/CTOLS=", myStudy.getConstraintWeightsInUse() s = myStudy.getState() display.report(s) print "q=", s.getQ() print "qErr=", s.getQErr() nextReport = 0 status = myStudy.stepTo(nextReport*dt, float('inf')) while status != Integrator.EndOfSimulation: s = myStudy.getState() mbs.realize(s) crankAngle = crank.getBodyRotation(s).convertRotationToAngleAxis()[0] * Rad2Deg interpolStr = "" if myStudy.isStateInterpolated(): interpolStr = " (INTERP)" print "%5g %10.4g E=%10.8g h%3d=%g %s%s\n" % ( s.getTime(), crankAngle, mbs.calcEnergy(s), myStudy.getNumStepsTaken(), myStudy.getPreviousStepSizeTaken(), Integrator.successfulStepStatusString(status).c_str(), interpolStr ) print " qerr=%10.8g uerr=%10.8g uderr=%10.8g\n" % ( crankRocker.getQErr(s).normRMS(), crankRocker.getUErr(s).normRMS(), crankRocker.getUDotErr(s).normRMS()) display.report(s) if status == Integrator.ReachedReportTime: nextReport += 1 status = myStudy.stepTo(nextReport*dt, float('inf')) for i in range(100): display.report(myStudy.getState()) print "Using Integrator %s:\n" % myStudy.getMethodName() print "# STEPS/ATTEMPTS = %d/%d\n" % (myStudy.getNumStepsTaken(), myStudy.getNumStepsAttempted()) print "# ERR TEST FAILS = %d\n" % myStudy.getNumErrorTestFailures() print "# REALIZE/PROJECT = %d/%d\n" % (myStudy.getNumRealizations(), myStudy.getNumProjections()) if __name__ == '__main__': main()