#~ /* KneeJointExample - #~ * Portions copyright (c) 2008-9 Stanford University #~ * Contributors: Ajay Seth #~ * #~ * Permission is hereby granted, free of charge, to any person obtaining #~ * a copy of this software and associated documentation files (the #~ * "Software"), to deal in the Software without restriction, including #~ * without limitation the rights to use, copy, modify, merge, publish, #~ * distribute, sublicense, and/or sell copies of the Software, and to #~ * permit persons to whom the Software is furnished to do so, subject #~ * to the following conditions: #~ * #~ * The above copyright notice and this permission notice shall be included #~ * in all copies or substantial portions of the Software. #~ * #~ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS #~ * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF #~ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. #~ * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY #~ * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, #~ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE #~ * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. #~ */ #~ /**@file #~ * A 2D knee joint example that deomstrates using custom mobilizers (FunctioBased) #~ * to simulate the effects of joint geometry that leads to the translation of tibia #~ * (shank) with respect to the femur (thigh) during flexion and extension of the knee. #~ */ import simbios.simtk as simtk from simbios.simtk import * import math import sys m = 1 # kg g = 9.8 # meters/s**2 apply in -y direction d = 0.4 # meters Deg2Rad = math.pi/180 def main(argv = None): if argv is None: argv = sys.argv i = 0 #------------------------------------------------------------------------------------------------- # Experimental data points (x,y) of tibia origin (tibial plateau) measured w.r.t. to # origin of the femur (hip joint center) in the femur frame as a function of knee joint angle. # From Yamaguchi and Zajac, 1989. #------------------------------------------------------------------------------------------------- # Tibia X: npx = 12 angX = (-2.094395102393, -1.745329251994, -1.396263401595, -1.047197551197, -0.698131700798, -0.349065850399, -0.174532925199, 0.197344221443, 0.337394955864, 0.490177570472, 1.521460267071, 2.094395102393) kneeX = (-0.003200000000, 0.001790000000, 0.004110000000, 0.004100000000, 0.002120000000, -0.001000000000, -0.003100000000, -0.005227000000, -0.005435000000, -0.005574000000, -0.005435000000, -0.005250000000) # Tibia Y npy = 7 angY = (-2.094395102393, -1.221730476396, -0.523598775598, -0.349065850399, -0.174532925199, 0.159148563428, 2.094395102393) kneeY = (-0.422600000000, -0.408200000000, -0.399000000000, -0.397600000000, -0.396600000000, -0.395264000000, -0.396000000000) # Create SimTK Vectors to hold data points ka_x = Vector_[float](npx); ka_x[:] = angX[:] # Form a vector of Real's tib_x = Vector_[float](npx); tib_x[:] = kneeX[:] # Generate splines as function of a given knee angle from a vector # of tibia x-translation data points. Spline returns a vec<1> fitterX = SplineFitter[float].fitFromGCV(degree=3, x=ka_x, y=tib_x) fx = fitterX.getSpline() #------------------------------------------------------------------------------------------------- # Define the 6-spatial functions that specify the motion of the tibia as a mobilized (shank) as # a FunctionBased MobilizedBody (shank) w.r.t. to its parent (the thigh). #------------------------------------------------------------------------------------------------- # Each function has to return 1 Real functions = [None] * 6 # as a function of coordIndices (more than one per function) coordIndices = Array[Array[int]](6) # about a body-fixed axis for rotations or in parent translations axes = Array[Vec3](6) # Number of mobilities (dof) nm = 1 # Set the 1st and 2nd spatial rotation about the orthogonal (X then Y) axes as # constant values. That is they don't contribute to motion nor do they have # any coordinates in the equations of motion. # |--------------------------------| # | 1st function: rotation about X | # |--------------------------------| functions[0] = Function.Constant(0, 0) noIndex = Array[int](0) coordIndices[0] = noIndex # |--------------------------------| # | 2nd function: rotation about Y | # |--------------------------------| functions[1] = (Function.Constant(0, 0)) coordIndices[1] = noIndex # Set the spatial rotation about third axis to be the knee-extension # angle (the one q) about the Z-axis of the tibia at the femoral condyles # Define the coefficients of the linear function of the knee-angle with the # spatial rotation about Z. coeff = Vector(2) # Linear function x3 = coeff[0]*q + coeff[1] coeff[0] = 1 coeff[1] = 0 # |--------------------------------| # | 3rd function: rotation about Z | # |--------------------------------| functions[2] = Function.Linear(coeff) # function of coordinate 0 (knee extension angle) qIndex = Array[int](1,0) coordIndices[2] = qIndex # Set the spatial translations as a function (splines) along the parent X and Y axes # |-----------------------------------| # | 4th function: translation along X | # |-----------------------------------| functions[3] = Spline(fx) coordIndices[3] =(qIndex) # |-----------------------------------| # | 5th function: translation along Y | # |-----------------------------------| functions[4] = (Function.Constant(0, 0)) coordIndices[4] = noIndex # |-----------------------------------| # | 6th function: translation along Z | # |-----------------------------------| functions[5] = Function.Constant(0, 0) coordIndices[5] = noIndex # Assemble the multibody system grav = -9.8065 system = MultibodySystem() matter = SimbodyMatterSubsystem(system) forces = GeneralForceSubsystem(system) gravity = Force.UniformGravity(forces, matter, Vec3(0, grav, 0)) matter.setShowDefaultGeometry(True) #------------------------------------------------------------------------------------------------- # Define the model's physical (body) properties #------------------------------------------------------------------------------------------------- #Thigh femur = Body.Rigid(MassProperties(8.806, Vec3(0), Inertia(Vec3(0.1268, 0.0332, 0.1337)))) femur.addDecoration(Transform(Vec3(0, -0.21+0.1715, 0)), (DecorativeCylinder(0.04, 0.21)).setColor(Vec3(1.0,0,0))) #Shank tibia = Body.Rigid(MassProperties(3.510, Vec3(0), Inertia(Vec3(0.0477, 0.0048, 0.0484)))) tibia.addDecoration(Transform(Vec3(0, -0.235+0.1862, 0)), (DecorativeCylinder(0.035, 0.235)).setColor(Vec3(1.0,0.2,0.2))) #------------------------------------------------------------------------------------------------- # Build the multibody system by adding mobilzed bodies to the matter subsystem #------------------------------------------------------------------------------------------------- # Add the thigh via hip joint thigh = MobilizedBody.Pin(matter.Ground(), Transform(Vec3(0)), femur, Transform(Vec3(0.0020, 0.1715, 0))) # add shank via right knee joint as a pin (initially) shank = MobilizedBody.Pin(thigh, Transform(Vec3(0.0033, -0.2294, 0)), tibia, Transform(Vec3(0.0, 0.1862, 0.0))) # --------------------- Replace the above pin knee joint with a function-based mobilizer ------------------------- # NOTE: function of Y-translation data was defined int the femur frame according to Yamaguchi and Zajac, which had # the orgin at the hip joint center and the Y along the long-axis of the femur and Z out of the page #MobilizedBody.FunctionBased shank(thigh, ... # Setup reporters # VTK Animation system.updDefaultSubsystem().addEventReporter(VTKEventReporter(system, 0.01)) # Define the model and all necessary caches system.realizeTopology() state = system.getDefaultState() matter.setUseEulerAngles(state, True) system.realizeModel(state) nq = state.getNQ() nu = state.getNU() # Hip and knee coordinates and speeds similar to early swing hip_angle = -45*math.pi/180 knee_angle = 0*math.pi/180 hip_vel = 1.0 knee_vel = -5.0 # Set initial states (Q's and U's) # Position thigh.setOneQ(state, 0, hip_angle) shank.setOneQ(state, 0, knee_angle) # Speed thigh.setOneU(state, 0, hip_vel) shank.setOneU(state, 0, knee_vel) system.realize(state, Stage.Acceleration) # Simulate it. integ = RungeKuttaMersonIntegrator(system) integ.setAccuracy(1e-3) ts = TimeStepper(system, integ) ts.initialize(state) ts.stepTo(5.0) if __name__ == "__main__": sys.exit(main())