# --------------------------------------------------------------------------- # # OpenSim: Script_Monte_Move_Markers.py # # --------------------------------------------------------------------------- # # Authors: 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. # # Import needed packages # File input and output with native python os package # 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 os import math import random import re import time import org.opensim.modeling # 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 resultsFolder = os.path.join(trcDataFolder, 'IK_Results_MC_Markers'); if not os.path.exists(resultsFolder): os.mkdir(resultsFolder) # Prompts user to select directory if above does not exist if not os.path.exists(trcDataFolder): trcDataFolder = utils.FileUtils.getInstance().browseForFolder("Select the folder that contains trc files"); print trcDataFolder # Load the setup file print "Acquiring setup file" # Prompts user to select file if above does not exist setupFile = os.path.join(trcDataFolder,"14_setup_IK_ant_model.xml"); if not os.path.exists(setupFile): setupFile = utils.FileUtils.getInstance().browseForFilename(".xml", "Select the setup file for the IK tool", 1) print setupFile # 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 # Basic data h = 0 COUNTER = 0 NbIter = 100 nbAxis = 3 #x,y,z frequenceTimeStep = 0.01 # Calibration data x = 0 y = 1 z = 2 xCalib = 0.45; # milimeter yCalib = 1.3382; # milimeter zCalib = 0.346; # milimeter W = 0.4 # mm (diameter of the sphere of calibration) # don't forget to add .0 to have a double # Definition of the ellipse size if needed xEllipse = xCalib / W yEllipse = yCalib / W zEllipse = zCalib / W # Create readable vector for opensim nv = modeling.Vec3() print nv ## 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') ## Load the model to be used and and initialize print "Acquiring model" modelFile = os.path.join(trcDataFolder, "Barbarus_model_05_2021.osim"); print "Creating and initializing model" # Create copy of the model model = modeling.Model(modelFile) # Get full path name of original.old model fullPathName = model.getInputFileName() # Change the name of the modified model newName = fullPathName.replace('.osim','_MC_Markers.osim') setupFile = os.path.join(resultsFolder,'15_media1_gaitcycle3_IK_Setup_MC_test.xml'); # Get the location of the markers currentMarkerSet = model.getMarkerSet() nbMarkers = currentMarkerSet.getSize() bestMarkerPosition = [[0] * nbAxis for i in range(nbMarkers)] markerMatrix = [[0] * nbAxis for i in range(nbMarkers)] for i in range (0,nbMarkers): # Get one by one information marker nameMarker = currentMarkerSet.get(i).getName() # print nameMarker markerLocation = currentMarkerSet.get(i).get_location() # print markerLocation # Create matrix with the original data markerMatrix[i][x] = markerLocation.get(x) markerMatrix[i][y] = markerLocation.get(y) markerMatrix[i][z] = markerLocation.get(z) print markerMatrix # Set name of output motion print "Processing " + filename; # 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 # Creation of the results matrix for the angles nbCoordinates = model.getCoordinateSet().getSize() print nbCoordinates anglesMatrix = [[[0 for p in range(nbCoordinates+1)] for k in range(0,nbTimeStep)] for h in range(0,NbIter)] ## Loop to modify the marker, run the IK and save the results 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_Markers.osim') currentMarkerSet_loop = model_loop.getMarkerSet() # model_loop.print(newName) # Modify the marker position for the current marker depending to the original data for i in range (0,nbMarkers): p = W/2*pow(random.random(),(1/3)) phi = math.acos((1-2*random.random())) omega = 2 * math.pi * random.random() # print "location of current marker" # print currentMarkerSet.get(i).get_location() # uncomment the ellipse par to add it nx = markerMatrix[i][x] + p * math.sin(omega) * math.cos(phi) # * xEllipse ny = markerMatrix[i][y] + p * math.sin(phi) # * yEllipse nz = markerMatrix[i][z] + p * math.cos(omega) * math.cos(phi) # * zEllipse # Set the nv vector nv.set(0,nx) nv.set(1,ny) nv.set(2,nz) # Set the new location of the current marker currentMarkerSet_loop.get(i).set_location(nv) # 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 ikTool.run() # 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") COUNTER = COUNTER + 1 # Close the results files 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))