# Author: Anton Kaniewksi and Santiago Arroyave # Universite Aix-Marseille # Date : 15/06/2021 # # This example script runs multiple inverse kinematics for the ant model. To see the results # load the model and ik output. # # --------------------------------------------------------------------------- # # OpenSim: Script_Monte_Move_Axis.py # # --------------------------------------------------------------------------- # # OpenSim is a toolkit for musculoskeletal modeling and simulation, # # developed as an open source project by a worldwide community. Development # # and support is coordinated from Stanford University, with funding from the # # U.S. NIH and DARPA. See http://opensim.stanford.edu and the README file # # for more information including specific grant numbers. # # # # Copyright (c) 2005-2017 Stanford University and the Authors # # Author(s): Ayman Habib, Kevin Xu, Edith Arnold # # # # Licensed under the Apache License, Version 2.0 (the "License"); you may # # not use this file except in compliance with the License. You may obtain a # # copy of the License at http://www.apache.org/licenses/LICENSE-2.0 # # # # Unless required by applicable law or agreed to in writing, software # # distributed under the License is distributed on an "AS IS" BASIS, # # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # # See the License for the specific language governing permissions and # # limitations under the License. # # --------------------------------------------------------------------------- # # Import needed packages # File input and output with native python os package import os import random import re import time import org.opensim.modeling import math # Utils contains tools to browse for files and folders import org.opensim.utils as utils #Start the timer start_time = time.time() # Browse for Directory containing trc files to process print "Acquiring folders with marker data" # Get the path to a model trcDataFolder = "C:\OpenSim4\Resources\Models\Barbarus" filename = "8_media1_gaitcycle3.trc" # # Select folder to store output results # print "Acquiring folder to store the IK results output files" resultsFolder = os.path.join(trcDataFolder, 'IK_Results_MC_MoveAxis'); # Load the setup file print "Acquiring setup file" # Prompts user to select file if above does not exist setupFile = os.path.join(trcDataFolder,"16_setup_IK_ant_model_axis.xml"); # Initialize InverseKinematicsTool from setup file print "Creating InverseKinematicsTool" ikTool = modeling.InverseKinematicsTool(setupFile) # Set the results folder in the ikTool ikTool.setResultsDir(resultsFolder) ## Modify the model # Initialize of data NbIter = 100; # Number of iteration thetaDef = 10 # deg thetaDefRad = (thetaDef *math.pi)/180 #rad lCyl = 0.2 # Lenght of the cylinder rCyl = math.tan(thetaDefRad)*(lCyl/2) # Radius of the cylinder frequenceTimeStep = 0.01 counter = 0 crossProduct = [[0],[0],[0]] prodScalaire = 0 Xp = 1 Yp = 2 Zp = 3 Xc = 4 Yc = 5 Zc = 6 xAxis = 7 yAxis = 8 zAxis = 9 # Create vector readable for opensim nParentFrame = modeling.Vec3() nChildFrame = modeling.Vec3() naxisZ = modeling.Vec3() ## Load the model to be used and and initialize print "Acquiring model" modelFile = os.path.join(trcDataFolder, "Barbarus_model_05_2021.osim"); # if not os.path.exists(modelFile): # modelFile = utils.FileUtils.getInstance().browseForFilename(".osim", "Select the Model", 1) print "Creating and initializing model" # Create copy of the model model = modeling.Model(modelFile) fullPathName = model.getInputFileName() # Change the name of the modified model newName = fullPathName.replace('.osim','_MC_Axis.osim') setupFile = os.path.join(resultsFolder,'17_media1_gaitcycle3_IK_Setup_MC_test.xml'); # Use the trc file to get the start and end times markerData = modeling.MarkerData(os.path.join(trcDataFolder,filename)) ikTool.setStartTime(markerData.getStartFrameTime()) ikTool.setEndTime(markerData.getLastFrameTime()) # Compute the number of time step in the IK nbTimeStep = (markerData.getLastFrameTime() - markerData.getStartFrameTime()) / frequenceTimeStep + 1 # print nbTimeStep # Compute the number of joints in the model nbJoint = model.getJointSet().getSize() # print nbJoint # Compute the number of coordinates in the model nbCoordinates = model.getCoordinateSet().getSize() # print nbCoordinates # Define matrix use later in the program dxCir = [0,0] dyCir = [0,0] dzCir = [0,0] OmcfMatrixC = [[0 for p in range(0,1)] for k in range(0,4)] OmcfMatrixP = [[0 for p in range(0,1)] for k in range(0,4)] OmcoMatrix = [[0 for p in range(0,1)] for k in range(0,4)] passageMatrix = [[0 for p in range(0,4)] for k in range(0,4)] passageMatrix[3][3] = 1 OmcoMatrix[3][0] = 1 colsCoordData = 10 # coord name, Xp, Yp, Zp, Xc, Yc, Zc, xAxis, yAxis, zAxis # Get the original position and orientation axis of the model OriCoorMatrix = [[0 for p in range(0,colsCoordData)] for k in range(0,nbCoordinates)] for k in range (0,nbCoordinates): OriCoorMatrix [k][0] = model.getCoordinateSet().get(k) # get the name of the coordinate nameCoor = str(OriCoorMatrix [k][0]) joint = model.getCoordinateSet().get(k).getJoint() parentFrame = joint.get_frames(0) # get the parent frame of the joint link to the coordiante OriCoorMatrix[k][Xp] = parentFrame.get_translation().get(0) # get the x position of the parent frame OriCoorMatrix[k][Yp] = parentFrame.get_translation().get(1) # get the y position of the parent frame OriCoorMatrix[k][Zp] = parentFrame.get_translation().get(2) # get the z position of the parent frame childFrame = joint.get_frames(1)# get the child frame of the joint link to the coordiante OriCoorMatrix[k][Xc] = childFrame.get_translation().get(0) # get the x position of the child frame OriCoorMatrix[k][Yc] = childFrame.get_translation().get(1) # get the y position of the child frame OriCoorMatrix[k][Zc] = childFrame.get_translation().get(2) # get the z position of the child frame # Go to the customjjoint to have the SpatialTransform customJoint = modeling.CustomJoint.safeDownCast(joint) if ("xR" in nameCoor) == True: axis = customJoint.get_SpatialTransform().getTransformAxis(0).getAxis() # get XAxis OriCoorMatrix[k][xAxis] = axis.get(0) # 0 = rotation 1 so x OriCoorMatrix[k][yAxis] = axis.get(1) # 0 = rotation 2 so y OriCoorMatrix[k][zAxis] = axis.get(2) # 0 = rotation 3 so z elif ("yR" in nameCoor) == True: axis = customJoint.get_SpatialTransform().getTransformAxis(1).getAxis() # get YAxis OriCoorMatrix[k][xAxis] = axis.get(0) # 0 = rotation 1 so x OriCoorMatrix[k][yAxis] = axis.get(1) # 0 = rotation 2 so y OriCoorMatrix[k][zAxis] = axis.get(2) # 0 = rotation 3 so z elif ("zR" in nameCoor) == True: axis = customJoint.get_SpatialTransform().getTransformAxis(2).getAxis() # get ZAxis OriCoorMatrix[k][xAxis] = axis.get(0) # 0 = rotation 1 so x OriCoorMatrix[k][yAxis] = axis.get(1) # 0 = rotation 2 so y OriCoorMatrix[k][zAxis] = axis.get(2) # 0 = rotation 3 so z # print OriCoorMatrix # Creation of the Coordinate matrix to collect all the modification through all the different iterations CoorMatrix = [[[0 for p in range(colsCoordData)] for k in range(0,nbCoordinates)] for h in range(0,NbIter)] #CoorMatrix [h][k][p] for h in range (0,NbIter): for k in range (0,nbCoordinates): CoorMatrix[h][k][0] = model.getCoordinateSet().get(k) # print CoorMatrix ## Head of the results file for the angles # Open the saving text file for the angles results anglesFile = open(os.path.join(resultsFolder,'Results_angles.txt'), "w") anglesFile.write("Angles file \n") # Read the head of intermediates angles results angles = open(os.path.join(resultsFolder,'8_media1_gaitcycle3_ik.mot'),'r') lines_before_12 = angles.readlines()[0:11] angles.close() # Write the head of intermediates angles results for k in range(0,11): line = re.split(r'\t+', lines_before_12[k]) anglesFile.write('%s\t' % line) anglesFile.write('\n') anglesFile.write('\n') anglesMatrix = [[[0 for p in range(nbCoordinates+1)] for k in range(0,nbTimeStep)] for h in range(0,NbIter)] # Write the head part of the angles results file anglesResultsFile = open(os.path.join(resultsFolder,'Final_results_angles.txt'), "w") anglesResultsFile.write(" Angles for time step \n \n") anglesResultsFile.write("1er_paragraphe=mean,_2eme=variance_and_3eme_=_standard_deviation \n \n") anglesResultsFile.write("time ") for i in range(0,nbCoordinates): anglesResultsFile.write(model.getCoordinateSet().get(i).getName() +" ") anglesResultsFile.write("\n") for h in range (0,NbIter): model_loop = modeling.Model(modelFile) fullPathName = model_loop.getInputFileName() # Change the name of the modified model newName = fullPathName.replace('.osim','_MC_Axis.osim') for nb in range (0,6): # The 6 first coordinates correspond to the thorax to the ground # so don't touch it because it's not part of the model # Get the parent frame CoorMatrix[h][nb][Xp] = OriCoorMatrix[nb][Xp] CoorMatrix[h][nb][Yp] = OriCoorMatrix[nb][Yp] CoorMatrix[h][nb][Zp] = OriCoorMatrix[nb][Zp] # Get the child frame CoorMatrix[h][nb][Xc] = OriCoorMatrix[nb][Xc] CoorMatrix[h][nb][Yc] = OriCoorMatrix[nb][Yc] CoorMatrix[h][nb][Zc] = OriCoorMatrix[nb][Zc] # Get the axis CoorMatrix[h][nb][xAxis] = OriCoorMatrix[nb][xAxis] CoorMatrix[h][nb][yAxis] = OriCoorMatrix[nb][yAxis] CoorMatrix[h][nb][zAxis] = OriCoorMatrix[nb][zAxis] for nb in range (6,nbCoordinates): ## Translation change # Reset the matrix result OmcfMatrixC[0][0] = OmcfMatrixC[1][0] = OmcfMatrixC[2][0] = OmcfMatrixC[3][0] = 0 OmcfMatrixP[0][0] = OmcfMatrixP[1][0] = OmcfMatrixP[2][0] = OmcfMatrixP[3][0] = 0 # Get the joint, parent and child frame of the current coordinate joint = model_loop.getCoordinateSet().get(nb).getJoint() parentFrame = joint.get_frames(0) childFrame = joint.get_frames(1) # Modification of the z to create the both point in a circle dzCir[1] = (lCyl / 2) # lCyl / 2 = 0.5 dzCir[0] = - (lCyl / 2) # Creation of the random position for a circle in the xy plan # The both circle are shift to the original place as describe before with the dzCir for i in range (0,2): # Random circle of the cylinder position alpha = 2 * math.pi * random.random() r = rCyl * math.sqrt(random.random()) dxCir[i] = r * math.cos(alpha) dyCir[i] = r * math.sin(alpha) # Compute the real movement of the original point not still apply dx = (dxCir[0] + dxCir[1]) /2 dy = (dyCir[0] + dyCir[1]) /2 # Get the translation of the child frame at the current coordinate Tx = Ty = Tz = 0 Tx = childFrame.get_translation().get(0) # Txc Ty = childFrame.get_translation().get(1) # Tyc Tz = childFrame.get_translation().get(2) # Tzc # Get orientation of the child and parent frame at the current coordinate # The both orientation are the same at each coordinate psi = childFrame.get_orientation().get(0) theta = childFrame.get_orientation().get(1) phi = childFrame.get_orientation().get(2) # Euler rotation matrix (x,y,z sequence) A = math.cos(theta)*math.cos(phi); B = -math.cos(theta)*math.sin(phi); C = math.sin(theta); D = math.sin(psi)*math.sin(theta)*math.sin(phi)+math.cos(psi)*math.sin(phi); E = -math.sin(psi)*math.sin(theta)*math.sin(phi)+math.cos(psi)*math.cos(phi); F = -math.sin(psi)*math.cos(theta); G = -math.cos(psi)*math.sin(theta)*math.cos(phi)+math.sin(psi)*math.sin(phi); H = math.cos(psi)*math.sin(theta)*math.sin(phi)+math.sin(psi)*math.cos(phi); I = math.cos(psi)*math.cos(theta); # Form of the transfert matrix passageMatrix[0][0] = A #[ A B C Tx] passageMatrix[0][1] = B #[ D E F Ty] passageMatrix[0][2] = C #[ G H I Tz] passageMatrix[1][0] = D #[ 0 0 0 1 ] passageMatrix[1][1] = E passageMatrix[1][2] = F passageMatrix[2][0] = G passageMatrix[2][1] = H passageMatrix[2][2] = I passageMatrix[0][3] = Tx passageMatrix[1][3] = Ty passageMatrix[2][3] = Tz # Definition of the base matrix OmcoMatrix[0][0] = dx OmcoMatrix[1][0] = dy OmcoMatrix[2][0] = 0 # Product of the both matrice # iterate through rows of X for mi in range(0,len(passageMatrix)): # iterate through columns of Y for mj in range(0,len(OmcoMatrix[0])): # iterate through rows of Y for mk in range(0,len(OmcoMatrix)): OmcfMatrixC[mi][mj] += passageMatrix[mi][mk] * OmcoMatrix[mk][mj] # print passageMatrix # print OmcoMatrix # print "OmcfMatrixC" # print OmcfMatrixC # Define the new point for the child frame position CoorMatrix[h][nb][Xc] = OmcfMatrixC[0][0] nXc = OmcfMatrixC[0][0] CoorMatrix[h][nb][Yc] = OmcfMatrixC[1][0] nYc = OmcfMatrixC[1][0] CoorMatrix[h][nb][Zc] = OmcfMatrixC[2][0] nZc = OmcfMatrixC[2][0] Tx = Ty = Tz = 0 Tx = parentFrame.get_translation().get(0) # Txp Ty = parentFrame.get_translation().get(1) # Typ Tz = parentFrame.get_translation().get(2) # Tzp # print "Txp, Typ, Tzp" # print Tx, Ty, Tz # Set the translation part of the transfert matrix passageMatrix[0][3] = Tx passageMatrix[1][3] = Ty passageMatrix[2][3] = Tz # Product of the both matrice # iterate through rows of X for mi in range(0,len(passageMatrix)): # iterate through columns of Y for mj in range(0,len(OmcoMatrix[0])): # iterate through rows of Y for mk in range(0,len(OmcoMatrix)): OmcfMatrixP[mi][mj] += passageMatrix[mi][mk] * OmcoMatrix[mk][mj] # print passageMatrix # print OmcoMatrix # print "OmcfMatrixP" # print OmcfMatrixP # Define the new point for the parent frame position CoorMatrix[h][nb][Xp] = OmcfMatrixP[0][0] nXp = OmcfMatrixP[0][0] CoorMatrix[h][nb][Yp] = OmcfMatrixP[1][0] nYp = OmcfMatrixP[1][0] CoorMatrix[h][nb][Zp] = OmcfMatrixP[2][0] nZp = OmcfMatrixP[2][0] # Set the parent frame depending of our computation nParentFrame.set(0,nXp) nParentFrame.set(1,nYp) nParentFrame.set(2,nZp) # Set the child frame depending of our computation nChildFrame.set(0,nXc) nChildFrame.set(1,nYc) nChildFrame.set(2,nZc) # Set the new vector instead of the old one parentFrame.set_translation(nParentFrame) childFrame.set_translation(nChildFrame) # Go to the customjjoint to have the SpatialTransform customJoint = modeling.CustomJoint.safeDownCast(joint) # Get the name of the current and previous coordinate # To check if the coordinate is a z axis of a pin joint nameCoor = str(CoorMatrix[h][nb][0]) nameCoorBefore = str(CoorMatrix[h][nb-1][0]) if ("zR" in nameCoor) == True and ("yR" in nameCoorBefore) == False: # Position of the point final for the vector to put instead of the z axis nXvector = dxCir[1] - dxCir[0] nYvector = dyCir[1] - dyCir[0] nZvector = dzCir[1] - dzCir[0] nVectorNorme = math.sqrt(pow(nXvector,2) + pow(nYvector,2) + pow(nZvector,2)) CoorMatrix[h][nb][xAxis] = nXvector/nVectorNorme nZx = nXvector/nVectorNorme CoorMatrix[h][nb][yAxis] = nYvector/nVectorNorme nZy = nYvector/nVectorNorme CoorMatrix[h][nb][zAxis] = nZvector/nVectorNorme nZz = nZvector/nVectorNorme # Create the new vector to be readable by opensim naxisZ.set(0,nZx) naxisZ.set(1,nZy) naxisZ.set(2,nZz) # Set in the model the new axis customJoint.get_SpatialTransform().getTransformAxis(2).setAxis(naxisZ) # print "axisZ" # print axisZ else: CoorMatrix[h][nb][xAxis] = OriCoorMatrix[nb][xAxis] CoorMatrix[h][nb][yAxis] = OriCoorMatrix[nb][yAxis] CoorMatrix[h][nb][zAxis] = OriCoorMatrix[nb][zAxis] # Print the model model_loop.print(newName) ## Set ik Tool for running ikTool = modeling.InverseKinematicsTool(setupFile) # Set name of input trc file and output motion in tool ikTool.setMarkerDataFileName(os.path.join(trcDataFolder,filename)) outFileName = filename.replace('.trc', '_ik.mot') ikTool.setOutputMotionFileName(os.path.join(resultsFolder,outFileName)) # Run the tool # print "Running trial file "+ikTool.getMarkerDataFileName()+" Output:"+ikTool.getOutputMotionFileName() ikTool.run() # print "Finished processing trial "+filename # Load the results angles = open(os.path.join(resultsFolder,'8_media1_gaitcycle3_ik.mot'),'r') lines_after_12 = angles.readlines()[11:] angles.close() # Fullfill of the angles matrix with output of the kinematics simulation for k in range(0,len(lines_after_12)): line = re.split(r'\t+', lines_after_12[k]) anglesFile.write("%s\t" % line) anglesFile.write("\n") for p in range(0,nbCoordinates+1): # line[0] = actual time anglesMatrix [h][k][p] = line[p] anglesFile.write("\n") anglesFile.close() # Show times characteristics of the code print("--- %s seconds for all iterations ---" % (time.time() - start_time)) time_by_iterations = (time.time() - start_time)/NbIter print("--- %s seconds for by iterations ---" % (time_by_iterations))