#~ Passive Dynamic Walker Simulation
#~ Eric Lew
#~ Neuromuscular Biomechanica Lab
#~ Stanford University
#~ Stanford, CA, 94305
#~ Copyright 2008

#~ Model geometry, inertial parameters, and initial conditions taken from a Working Model 
#~ simulation of a passive dynamic walker by:

#~ Dan Jung
#~ Mario Gomez
#~ Art Kuo

#~ Human-Power Lab
#~ Cornell University
#~ Ithaca, NY14850

from simbios.simtk import *
import sys
from math import pi as Pi
from math import sin, cos

#**********************************************************************
#Define Parameters
#**********************************************************************

#Scalars
hFloor = .24219 #Checked 7/22
qFloor = -.0456 #Checked 7/22
mThigh = .1 #mass thighin kg #Checked 7/22
mShankFoot = 2*.031 #mass shank in kg #Checked 7/22
mPelvis = .667 #mass pelvis in kg #Checked 7/22
wThigh = .46 #length of thigh in m #Checked 7/22
hThigh = .04#Checked 7/22
wShank = .33322#Checked 7/22
hShankFoot = .04#Checked 7/22
rFoot = .2#Checked 7/22
rPelvis = .1#Checked 7/22
wPelvis = .01	#Checked 7/22

#Position vectors
rfo_soS = Vec3(.1666,.0676,0) #Checked 7/23
rscm_soS = Vec3(.0706,.0379,0) #Checked 7/22
rsfcm_soSF = rscm_soS #Shank foot center of mass is in same position as shank com
rtcm_toT = Vec3(-.03,0,0)

#Inertiasf
Ip_poP = Inertia(1e-10)
nonZfactor = 1.0
Isf_sfoSF = Inertia(5.2485e-4*nonZfactor,3.1e-4*nonZfactor,.0025)#.002146) 
It_toT = Inertia(1e-6*nonZfactor,.0019*nonZfactor,.0019)

#**********************************************************************
#Roll Constraint On Event Handler 
#**********************************************************************

class RollConstraintOn(TriggeredEventHandler):
    def __init__(self, system, foot, noSlipX, noSlipZ, title):
        super(RollConstraintOn, self).__init__(Stage.Position)
        self.system = system
        self.foot = foot
        self.noSlipX = noSlipX
        self.noSlipZ = noSlipZ
        self.title = title
        self.getTriggerInfo().setTriggerOnRisingSignTransition(False)
    
    def getValue(self, state):
        #print "Called getValue in TEH"
        rsfo_go = self.foot.getBodyOriginLocation(state)
        gRs = self.foot.getBodyRotation(state)
        rfo_go = rsfo_go+gRs*rfo_soS
        rcontact_go = rfo_go+Vec3(0,-rFoot,0)
        planeNormal = UnitVec3(0,1,0)
        fudge = +1e-5# 
        #I use this to make the foot go a little below the ground before turning on the constraint.  When the constraint turns on, it
        #will force the foot to move up into the plane, so when the constraint is turned off, there's no way it will be below the heightd
        #at which it would normally turn on.  this basically prevents the foot from falling through the ground. 

        hc = ~(planeNormal)*rcontact_go+fudge
        #print "height of bottom of ", self.title, " = ", hc
        return hc
    
    def handleEvent(self, state, accuracy, yWeights, ooConstraintTols, lowestModified, shouldTerminate):
        self.system.realize(state, Stage.Instance)
        if(self.noSlipX.isDisabled(state)):
            print "Roll Constraint ", self.title, " has been enabled."
            self.noSlipX.enable(state)
            self.noSlipZ.enable(state)
            lowestModified = Stage.Instance
        return lowestModified

#**********************************************************************
#Roll Constraint Off Event Handler
#**********************************************************************

class RollConstraintOff(TriggeredEventHandler):
    def __init__(self, system, foot, noSlipX, noSlipZ, title) : 
        super(RollConstraintOff, self).__init__(Stage.Position)
        self.system = system
        self.foot = foot
        self.noSlipX = noSlipX
        self.noSlipZ = noSlipZ
        self.title = title
        self.getTriggerInfo().setTriggerOnFallingSignTransition(False)
    
    def getValue(self, state):
        #print "Called getValue in TEH"
        rsfo_go = self.foot.getBodyOriginLocation(state)
        gRs = self.foot.getBodyRotation(state)
        rfo_go = rsfo_go+gRs*rfo_soS
        rcontact_go = rfo_go+Vec3(0,-rFoot,0)
        planeNormal = UnitVec3(0,1,0)
        fudge = -1e-2 
        #I use this to make the foot go a little above the ground before turning off the constraint.  When the constraint 
        #turns off, the foot will already be above the floor

        hc = ~(planeNormal)*rcontact_go-(hFloor/2-.12109)+fudge
        #print "height of bottom of ", self.title, " = ", hc
        return hc
    
    def handleEvent(self, state, accuracy, yWeights, ooConstraintTols, lowestModified, shouldTerminate):
        self.system.realize(state, Stage.Instance)
        if(not self.noSlipX.isDisabled(state)):
            print "Roll Constraint ", self.title, " has been disabled."
            self.noSlipX.disable(state)
            self.noSlipZ.disable(state)
            lowestModified = Stage.Instance
        return lowestModified


#**********************************************************************
#Knee Constraint On Event Handler - turn on constraint when q>=0
#**********************************************************************
class KneeConstraintOn(TriggeredEventHandler):
    def __init__(self, system, shankFoot, thigh, pelvis, constraint, title):
        super(KneeConstraintOn, self).__init__(Stage.Position)
        self.system = system
        self.shankFoot = shankFoot
        self.thigh = thigh
        self.pelvis = pelvis
        self.constraint = constraint
        self.title = title
        self.getTriggerInfo().setTriggerOnFallingSignTransition(False)

    def getValue(self, state):
        if(self.thigh.getOneQ(state,0) > -Pi/2.0):
            q = self.shankFoot.getOneQ(state,0)
            #print "q", self.title, " = ", q
            return q
        return -1.0
    
    def handleEvent(self, state, accuracy, yWeights, ooConstraintTols, lowestModified, shouldTerminate):
        self.system.realize(state, Stage.Velocity)
        if(self.constraint.isDisabled(state)):
            #Calculate center of mass of thgih
            gRt = self.thigh.getBodyRotation(state)
            rto_go = self.thigh.getBodyOriginLocation(state)
            #print "rto_go = ", rto_go
            rtcm_go = rto_go + gRt*rtcm_toT
            #print "rtcm_go = ", rtcm_go
            #Calculate center of mass of shankfoot
            rsfo_go = self.shankFoot.getBodyOriginLocation(state)
            gRs = self.shankFoot.getBodyRotation(state)
            #print "gRs = ", gRs
            rsfcm_go = rsfo_go+gRs*rsfcm_soSF 
            #print "rsfcm_go = ", rsfcm_go

            #Calculate center of mass of shankfootthigh combination
            rtsfcm_go = (mShankFoot*rsfcm_go+mThigh*rtcm_go)/(mShankFoot+mThigh)
            #print "rtsfcm_go = ", rtsfcm_go
            #Calculate com of tsf wrt pelvis
            vpcm_go = self.pelvis.getBodyOriginVelocity(state)
            rpcm_go = self.pelvis.getBodyOriginLocation(state)
            rtsfcm_p = rtsfcm_go-rpcm_go
            #print "rtsfcm_p = ", rtsfcm_p
            
            #Calculate angular momenta of thigh and shankfoot prior to constraint enabling:
            LHt_cm = self.thigh.calcBodyMomentumAboutBodyMassCenterInGround(state)
            Ht_cm = LHt_cm[0] #Angular momentum
            #print "Ht_cm = " <<Ht_cm
            Lt_cm = LHt_cm[1] #Linear momentum
            #print "Lt_cm = " <<Lt_cm
            Ht_g = Ht_cm+rtcm_go % Lt_cm #Angular momentum about ground
            #print "Ht_g = ", Ht_g

            LHsf_cm = self.shankFoot.calcBodyMomentumAboutBodyMassCenterInGround(state)
            Hsf_cm = LHsf_cm[0] #Angular momentum
            #print " Hsf_cm= " <<Hsf_cm
            Lsf_cm = LHsf_cm[1] #Linear momentum
            #print "Lsf_cm = " <<Lsf_cm
            Hsf_g = Hsf_cm+rsfcm_go % Lsf_cm #Angular momentum about ground
            #print "Hsf_g = ", Hsf_g

            Htotal_g = Hsf_g+Ht_g
            #print "Htotal_g = ", Htotal_g

            #Calculate mass of tsf
            mtsf = mThigh+mShankFoot

            #Calculate moment of inertias

            #Shift inertias to	COMs
            It_tcmT = It_toT.shiftToMassCenter(rtcm_toT,mThigh)
            Isf_sfcmSF = Isf_sfoSF.shiftToMassCenter(rsfcm_soSF,mShankFoot)
            
            #reexpress inertias in global frame
            It_tcmG = It_tcmT.reexpress(~gRt)
            Isf_sfcmG = Isf_sfcmSF.reexpress(~gRs)
            
            #Shift inertias to TSF COM
            rtsfcm_tcm = rtsfcm_go-rtcm_go
            rtsfcm_sfcm = rtsfcm_go-rsfcm_go

            It_tsfcmG = It_tcmG.shiftFromMassCenter(rtsfcm_tcm,mThigh)
            Isf_tsfcmG = Isf_sfcmG.shiftFromMassCenter(rtsfcm_sfcm,mShankFoot)
            
            #Final inertia is sum of shankfoot and thigh inertias about tsf COM in global frame
            Itsf_tsfcmG = It_tsfcmG + Isf_tsfcmG

            Itsf_tsfcmGMat = Itsf_tsfcmG.toMat33()
            #print "Itsf_tsfcmGMat = " <<Itsf_tsfcmGMat
        
            #Calculate part 1 of expression for angular velocity of TSF
            mat1 = Itsf_tsfcmG.toMat33() + Inertia(mtsf*~rtsfcm_go*rtsfcm_p).toMat33()
            

            #Calculate angular velocity of TSF
            omegatsf = mat1.invert()*(Ht_g+Hsf_g-mtsf*rtsfcm_go%vpcm_go)
            
            #Set joint angle to be slightly negative so when constraint is disabled it won't be positive
            self.shankFoot.setOneQ(state,0,-1e-2)

            #Enable Constraint
            self.constraint.enable(state)
            
            #print "Calculated final omegaTSF = ", omegatsf,  endl
            #print "Previous omegaThigh = ", thigh.getBodyAngularVelocity(state)
            #Update TSF velocity
            self.thigh.setOneU(state,0,omegatsf[2])
            self.shankFoot.setOneU(state,0,0)
             
            print "Knee Constraint ", self.title, " has been enabled"
            lowestModified = Stage.Instance
        else:
            lowestModified = Stage.Report
        return lowestModified
#*/

#**********************************************************************
#Knee Constraint Off Event Handler
#**********************************************************************

class KneeConstraintOff(TriggeredEventHandler):
    def __init__(self, system, shankFootP, shankFootS, constraintP, constraintS, title):
        super(KneeConstraintOff, self).__init__(Stage.Acceleration)
        self.system = system
        self.shankFootS = shankFootS
        self.shankFootP = shankFootP
        self.constraintP = constraintP 
        self.constraintS = constraintS 
        self.title = title
        self.getTriggerInfo().setTriggerOnRisingSignTransition(False)
        
    def getValue(self, state):
        if(not (self.constraintP.isDisabled(state))):
            rsfo_go = self.shankFootS.getBodyOriginLocation(state)
            gRs = self.shankFootS.getBodyRotation(state)
            rfo_go = rsfo_go+gRs*rfo_soS
            rcontact_go = rfo_go+Vec3(0,-rFoot,0)
            planeNormal = UnitVec3(0,1,0)
            fudge = -1e-3 
            #I use this to make the foot go a little below the ground before turning on the constraint.  When the constraint turns on, it
            #will force the foot to move up into the plane, so when the constraint is turned off, there's no way it will be below the heightd
            #at which it would normally turn on.  this basically prevents the foot from falling through the ground. 
            hc = ~(planeNormal)*rcontact_go+fudge	
            return hc
        return 1.0
    
    def handleEvent(self, state, accuracy, yWeights, ooConstraintTols, lowestModified, shouldTerminate):
        self.system.realize(state,Stage.Position)
        rsfo_go = self.shankFootP.getBodyOriginLocation(state)
        gRs = self.shankFootP.getBodyRotation(state)
        rfo_go = rsfo_go+gRs*rfo_soS
        rcontact_go = rfo_go+Vec3(0,-rFoot,0)
        planeNormal = UnitVec3(0,1,0)
        fudge = -1e-3 
        hc = ~(planeNormal)*rcontact_go+fudge
        if( not (self.constraintP.isDisabled(state)) and (hc<0) ): #If the primary knee constraint is enabled and the primary foot is touching the ground
            #Printout constraint force
            self.shankFootP.setOneQ(state,0,-1e-5)
            self.constraintP.disable(state)
            print "Knee Constraint ", self.title, " has been disabled"
            lowestModified = Stage.Instance
        else:
            lowestModified = Stage.Report
        return lowestModified
#*/

def enterParam(title):
    sys.stdout.flush()
    param = float(raw_input(title + " = "))
    return param

def main():

    #**********************************************************************
    #Create the system and basic subsystems
    #**********************************************************************
    
    system = MultibodySystem()
    matter = SimbodyMatterSubsystem(system)
    matter.setShowDefaultGeometry(False)
    forces = GeneralForceSubsystem(system)

    #**********************************************************************
    #Define bodies
    #**********************************************************************
    #The center of mass is a vector from B's origin, expressed in the B frame. 
    #The inertia is taken about the B origin, and expressed in B.

    pelvisBody = Body.Rigid(MassProperties(mPelvis,Vec3(0),Ip_poP))
    thighBody = Body.Rigid(MassProperties(mThigh, rtcm_toT, It_toT))
    shankFootBody = Body.Rigid(MassProperties(mShankFoot, rscm_soS, Isf_sfoSF))
    ballBody = Body.Rigid(MassProperties(mShankFoot, Vec3(0), mShankFoot*Inertia.sphere(rFoot)))
    axleBody = Body.Rigid(MassProperties(1e-10, Vec3(0), 1e-10*Inertia.sphere(rFoot/5.)))
    
    #*********************************************************************
    #Add decorations
    #**********************************************************************
    
    pelvisBody.addDecoration(Transform(Rotation(Pi/2,XAxis)), 
        DecorativeCylinder(rPelvis,wPelvis/2).setColor(Orange).setOpacity(0.5))
    matter.updGround().addBodyDecoration(Transform(Vec3(0,-.12109,0)), 
        DecorativeBrick(Vec3(3.0,hFloor/2,0.01)).setColor(Green).setOpacity(1))
    thighBody.addDecoration(Transform(), 
        DecorativeBrick(Vec3(wThigh/2,hThigh/2,1e-10)).setColor(Blue).setOpacity(0.5))
    shankFootBody.addDecoration(Transform(), 
        DecorativeBrick(Vec3(wShank/2,hShankFoot/2,1e-10)).setColor(Red).setOpacity(0.5))
    shankFootBody.addDecoration(Transform(Rotation(Pi/2,XAxis),rfo_soS),
        DecorativeCylinder(rFoot,.01).setColor(Gray).setOpacity(.3))
    ballBody.addDecoration(Transform(Vec3(0)), 
        DecorativeSphere(rFoot).setColor(Blue).setOpacity(.3))
    axleBody.addDecoration(Transform(), DecorativeBrick(Vec3(.2, .03, .03))
        .setColor(Yellow).setOpacity(1))

    #**********************************************************************
    #Mobilize Bodies 
    #**********************************************************************
    
    pelvis = MobilizedBody.Free(matter.updGround(), Transform(Vec3(0)), pelvisBody, Transform())
    thigh1 = MobilizedBody.Pin(pelvis, Transform(Vec3(0,0,0)),
        thighBody, Transform(Vec3(-wThigh/2.,0,0)))
    thigh2 = MobilizedBody.Pin(pelvis, Transform(Vec3(0,0,0)),
        thighBody, Transform(Vec3(-wThigh/2.0,0, 0)))
    shankFoot1 = MobilizedBody.Pin(thigh1, Transform(Vec3(wThigh/2,0,0)),
        shankFootBody, Transform(Vec3(-wShank/2.,0,0)))
    shankFoot2 = MobilizedBody.Pin(thigh2, Transform(Vec3(wThigh/2,0,0)),
        shankFootBody, Transform(Vec3(-wShank/2.,0,0)))
    axle1 = MobilizedBody.Free(matter.updGround(),axleBody)
    axle2 = MobilizedBody.Free(matter.updGround(),axleBody)

    #**********************************************************************
    # Forces
    #**********************************************************************
    
    gravity = Force.UniformGravity(forces, matter, Vec3(-9.8*sin(qFloor), -9.8*cos(qFloor), 0))
    
    #**********************************************************************
    # Hunt Crossley Model
    #**********************************************************************
    
    #Hunt Crossley Parameters
    factorYoung = 1.0
    factorDissipation = 1e4
    # factorDissipation = float(raw_input(Please enter dissipation factor: "))
    
    #wood (density taken from canadian spruce http:#www.simetric.co.uk/si_wood.htm
    wood_density = 450.  # kg/m^3
    wood_young   = factorYoung*11.0e9  # pascals (N/m)
    wood_poisson = 0.3    # ratio
    wood_planestrain = wood_young/(1.-wood_poisson*wood_poisson)
    wood_dissipation = factorDissipation*0.001

    #rubber (density taken from canadian spruce http:#www.simetric.co.uk/si_rubber.htm
    rubber_density = 945  # kg/m^3
    rubber_young   = factorYoung*1e8  # pascals (N/m)
    rubber_poisson = 0.5    # ratio
    rubber_planestrain = rubber_young/(1.-rubber_poisson*rubber_poisson)
    rubber_dissipation = factorDissipation*0.001


    #Steel
    steel_density = 8000.  # kg/m^3
    steel_young   = factorYoung*200.0e9  # pascals (N/m)
    steel_poisson = 0.3    # ratio
    steel_planestrain = steel_young/(1.-steel_poisson*steel_poisson)
    steel_dissipation = factorDissipation*0.001

    #Concrete
    concrete_density = 2300.  # kg/m^3
    concrete_young   = factorYoung*25e9  # pascals (N/m)
    concrete_poisson = 0.15    # ratio
    concrete_planestrain = concrete_young/(1.-concrete_poisson*concrete_poisson)
    concrete_dissipation = factorDissipation*0.005
    
    contact = HuntCrossleyContact()
    system.addForceSubsystem(contact)
    contact.addSphere(shankFoot1, rfo_soS+Vec3(0,0,rFoot), rFoot, rubber_planestrain, rubber_dissipation)
    contact.addSphere(shankFoot2, rfo_soS+Vec3(0,0,-rFoot), rFoot, rubber_planestrain, rubber_dissipation)
    
    contact.addHalfSpace(matter.updGround(), UnitVec3(0,1,0), 0, wood_planestrain, wood_dissipation)
    

    #**********************************************************************
    # Constraints
    #**********************************************************************
    title1 = "shankFoot1"
    title2 = "shankFoot2"

    #Constrain axle to be connected to each shankFoot
    fixAxlePosition1 = Constraint.Ball(shankFoot1,rfo_soS,axle1,Vec3(0))
    fixAxlePosition2 = Constraint.Ball(shankFoot2,rfo_soS,axle2,Vec3(0))
    
    #Constrain shank to be welded to thigh (turn this one on and off)
    constantAngleKnee1 = Constraint.ConstantAngle(thigh1,UnitVec3(1,0,0),shankFoot1,UnitVec3(0,1,0),Pi/2)  
    constantAngleKnee2 = Constraint.ConstantAngle(thigh2,UnitVec3(1,0,0),shankFoot2,UnitVec3(0,1,0),Pi/2)  
    constantAngleKnee2.setDisabledByDefault(True)

    #Constrain feet to roll on floor without slipping
    noSlipX1 = Constraint.NoSlip1D(axle1,Vec3(0,-rFoot,0),UnitVec3(1,0,0),shankFoot1,matter.updGround())
    noSlipZ1 = Constraint.NoSlip1D(axle1,Vec3(0,-rFoot,0),UnitVec3(0,0,1),shankFoot1,matter.updGround())
    
    noSlipX2 = Constraint.NoSlip1D(axle2,Vec3(0,-rFoot,0),UnitVec3(1,0,0),shankFoot2,matter.updGround())
    noSlipX2.setDisabledByDefault(True)
    noSlipZ2 = Constraint.NoSlip1D(axle2,Vec3(0,-rFoot,0),UnitVec3(0,0,1),shankFoot2,matter.updGround())
    noSlipZ2.setDisabledByDefault(True)

    #Constraint pelvis and thigh1 to remain in a plane
    pelvisPlane = Constraint.PointInPlane(matter.updGround(),UnitVec3(0,0,1),0,pelvis,Vec3(0))
    thigh1Plane = Constraint.PointInPlane(matter.updGround(),UnitVec3(0,0,1),0,thigh1,Vec3(0))
    thigh2Plane = Constraint.PointInPlane(matter.updGround(),UnitVec3(0,0,1),0,thigh2,Vec3(0))
    
    #**********************************************************************
    #Add event reporter
    #**********************************************************************
    
    system.updDefaultSubsystem().addEventReporter(VTKEventReporter(system, .01))
    
    #**********************************************************************
    #Add event handler
    #**********************************************************************

    # For some reason it is important to construct the event handler outside of the addEventHandler() call
    kneeConstraintOn1 = KneeConstraintOn(system, shankFoot1, thigh1, pelvis, constantAngleKnee1, title1)
    system.updDefaultSubsystem().addEventHandler(kneeConstraintOn1)
    kneeConstraintOn2 = KneeConstraintOn(system, shankFoot2, thigh2, pelvis, constantAngleKnee2, title2)    
    system.updDefaultSubsystem().addEventHandler(kneeConstraintOn2)
    
    kneeConstraintOff1 = KneeConstraintOff(system, shankFoot1, shankFoot2, constantAngleKnee1, constantAngleKnee2, title1)
    system.updDefaultSubsystem().addEventHandler(kneeConstraintOff1)
    kneeConstraintOff2 = KneeConstraintOff(system, shankFoot2, shankFoot1, constantAngleKnee2, constantAngleKnee1, title2)
    system.updDefaultSubsystem().addEventHandler(kneeConstraintOff2)
    
    rollConstraintOn1 = RollConstraintOn(system, shankFoot1, noSlipX1, noSlipZ1, title1)
    system.updDefaultSubsystem().addEventHandler(rollConstraintOn1)
    rollConstraintOff1 = RollConstraintOff(system, shankFoot1, noSlipX1, noSlipZ1, title1)
    system.updDefaultSubsystem().addEventHandler(rollConstraintOff1)
    
    rollConstraintOn2 = RollConstraintOn(system, shankFoot2, noSlipX2, noSlipZ2, title2)
    system.updDefaultSubsystem().addEventHandler(rollConstraintOn2)
    rollConstraintOff2 = RollConstraintOff(system, shankFoot2, noSlipX2, noSlipZ2, title2)
    system.updDefaultSubsystem().addEventHandler(rollConstraintOff2)
    

    #**********************************************************************
    #Initialize the system and state.
    #**********************************************************************
    while(True):
        print "Welcome to the Passive Dynamic Walker Simbody Simulator!"
        print "Please press 1 if you would like to use default initial conditions"
        print "Please press 2 if you would like to modify the pelvis and swing leg initial conditions"
        print "Please press 3 if you would like to modify all initial conditions"
        print "Please press 4 if you would like to exit the program"
        sys.stdout.flush()
        choice = int(raw_input())
        thigh2U = 0.0
        shank2U = 0.0
        pelvisXU = 0.0
        if(choice == 2):
            thigh2U = enterParam("thigh2U")
            shank2U = enterParam("shank2U")
            pelvisXU = enterParam("pelvisXU")
        state = system.realizeTopology()
        # state = system.getDefaultState()
        heightFudge = 1e-4
        #*******Start with setting pelvis position
        pelvis.setQToFitTranslation(state, Vec3(-2.0206,.99189+heightFudge,0)) #Checked 7/22
        #******Start with setting axle positions
        axle1.setQToFitTranslation(state,Vec3(-1.9426,.19962,0))
        axle2.setQToFitTranslation(state,Vec3(-2.1841,.2665,0))
        #******Start with setting shankFoot positions
        #********Set positions via joint angles*********
        thigh1.setQToFitRotation(state, Rotation(-1.5576,ZAxis)) #Checked 7/22
        shankFoot1.setQToFitRotation(state, Rotation(-1e-5,ZAxis)) #Checked 7/22
        thigh2.setQToFitRotation(state, Rotation(-1.4703,ZAxis)) #Checked 7/22
        shankFoot2.setQToFitRotation(state, Rotation(.70656-Pi+1.4703,ZAxis))#Checked 7/22
        #*******Set Velocities********
        pelvis.setUToFitLinearVelocity(state, Vec3(0.63369+pelvisXU,4.812e-2,0)) #Checked 7/22
        thigh1.setUToFitAngularVelocity (state, Vec3(0,0,-.63863)) #Checked 7/22
        shankFoot1.setUToFitAngularVelocity (state, Vec3(0,0,-.63863))#Checked 7/22
        thigh2.setUToFitAngularVelocity (state, Vec3(0,0,1.858+thigh2U))#Checked 7/22
        shankFoot2.setUToFitAngularVelocity (state, Vec3(0,0,1.4823-1.858+shank2U))#Checked 7/22
        
        # Program crashes at runtime at "system.realize(state, Stage.Velocity)"
        # File "ExampleDynamicWalker.py", line 507, in main
        # system.realize(state, Stage.Velocity)
        # TypeError: 'NoneType' object is not callable
        system.realize(state, Stage.Velocity)
        
        shankFoot1Vel = shankFoot1.getBodyOriginVelocity(state)
        shankFoot1Omega = shankFoot1.getBodyAngularVelocity(state)
        gRs1 = shankFoot1.getBodyRotation(state)
        axle1Vel = shankFoot1Vel+shankFoot1Omega%(gRs1*rfo_soS)
        shankFoot2Vel = shankFoot2.getBodyOriginVelocity(state)
        shankFoot2Omega = shankFoot2.getBodyAngularVelocity(state)
        gRs2 = shankFoot2.getBodyRotation(state)
        axle2Vel = shankFoot2Vel+shankFoot2Omega%(gRs2*rfo_soS)
        axle1.setUToFitLinearVelocity(state, axle1Vel)
        axle2.setUToFitLinearVelocity(state, axle2Vel) 
        # axle2.setUToFitLinearVelocity(state, Vec3(1.755,-.095396,0)) #Checked 7/30
        # axle1.setUToFitLinearVelocity(state, Vec3(.18879,1.9753e-2,0)) #Checked 7/30
        # 
        #**********************************************************************
        # Simulate it.
        #**********************************************************************
        #VerletIntegrator integ(system)
        integ = RungeKuttaMersonIntegrator(system)
        integ.setAccuracy(1e-3)
        ts = TimeStepper(system, integ)
        ts.initialize(state)
        tfinal = 5.0
        # tfinal = float(raw_input(Please enter tfinal: "))
        # ts.stepTo(tfinal)
        integ.setReturnEveryInternalStep(True)
        ts.setReportAllSignificantStates(True)
        numSteps = 0
        while(integ.getTime() < tfinal):
            numSteps += 1
            ts.stepTo(tfinal)
            # print "Current time step: ", integ.getPreviousStepSizeTaken()
        print "error test failures", integ.getNumErrorTestFailures()
        print "projection failures", integ.getNumProjectionFailures()
        print "realization failures", integ.getNumRealizationFailures()
        print "Projections", integ.getNumProjections()
        print "steps attempted", integ.getNumStepsAttempted()
        print "steps taken", integ.getNumStepsTaken()
        print "numSteps = ", numSteps
        sys.stdout.flush()
        numSteps = int(raw_input())

if __name__ == "__main__":
    main()