import numpy
import opensim
import matplotlib.pyplot as plt
import os
model = opensim.Model(location_models + model_name)
model.updateMarkerSet(markerset)
model.initSystem()
state = model.initSystem()
#Scale Tool
scale_tool = opensim.ScaleTool(os.path.join(location_XMLs, scale_file))
scale_tool.setSubjectMass(subject_mass)
scale_tool.setPathToSubject('')
scale_tool.getGenericModelMaker().setModelFileName(
os.path.join(location_models, model_name))
scale_tool.getGenericModelMaker().setMarkerSetFileName(
os.path.join(location_markersets, markerset_name))
scale_tool.getGenericModelMaker().processModel()
scale_tool.getModelScaler().setMarkerFileName(marker_file)
# scale_tool.getModelScaler().setOutputModelFileName(os.path.join(data_folder,scaled_model_filename))
scale_tool.getModelScaler().processModel(model, '', subject_mass)
scale_tool.getMarkerPlacer().setMarkerFileName(marker_file)
scale_tool.getMarkerPlacer().processModel(model)
scale_tool.run()
#Save Model
def calculate_spanning_muscle_coordinates(model_file, results_dir):
"""Calculates the coordinates that are spanned by each muscle. Useful for
reducing the required computation of the muscle moment arm matrix.
"""
model = opensim.Model(model_file)
state = model.initSystem()
# construct model tree (parent body - joint - child body)
model_tree = []
for joint in model.getJointSet():
model_tree.append({
'parent':
joint.getParentFrame().getName().replace('_offset', ''),
'joint':
joint.getName(),
'child':
joint.getChildFrame().getName().replace('_offset', '')
})
ordered_body_set = []
for body in model.getBodySet():
ordered_body_set.append(body.getName())
model_coordinates = {} # coordinates in multibody order
for i, coordinate in enumerate(model.getCoordinateSet()):
mbix = coordinate.getBodyIndex()
mqix = coordinate.getMobilizerQIndex()
model_coordinates[coordinate.getName()] = (mbix, mqix)
model_coordinates = dict(
sorted(model_coordinates.items(), key=lambda x: x[1]))
ordered_coordinate_set = []
for key, value in model_coordinates.items():
ordered_coordinate_set.append(key)
# get the coordinates that are spanned by the muscles
muscle_coordinates = {}
for muscle in model.getMuscles():
path = muscle.getGeometryPath().getPathPointSet()
muscle_bodies = []
for point in path:
muscle_bodies.append(point.getBodyName())
# remove duplicate bodies and sort by multibody tree order
muscle_bodies = list(set(muscle_bodies))
muscle_bodies = sorted(muscle_bodies,
key=lambda x: ordered_body_set.index(x))
# find intermediate joints
assert (len(muscle_bodies) > 1)
joints = []
find_intermediate_joints(muscle_bodies[0], muscle_bodies[-1],
model_tree, joints)
# find spanning coordinates
muscle_coordinates[muscle.getName()] = []
for joint in joints:
joint = model.getJointSet().get(joint)
for i in range(0, joint.numCoordinates()):
if joint.get_coordinates(i).isDependent(state):
continue
else:
muscle_coordinates[muscle.getName()].append(
joint.get_coordinates(i).getName())
# sort coordinates by model order
muscle_coordinates[muscle.getName()] = sorted(
muscle_coordinates[muscle.getName()],
key=lambda x: ordered_coordinate_set.index(x))
# write results to file
with open(results_dir + 'muscle_coordinates.csv', 'w') as csv_file:
for key, values in muscle_coordinates.items():
csv_file.write(key)
for value in values:
csv_file.write(';' + value)
csv_file.write('\n')
def create_model_processor(self, root_dir):
# Create base model without reserves.
model = osim.Model(os.path.join(root_dir,
'resources/Rajagopal2016/'
'subject_walk_contact_bounded_80musc.osim'))
forceSet = model.getForceSet()
numContacts = 0
for i in np.arange(forceSet.getSize()):
forceName = forceSet.get(int(i)).getName()
if 'contact' in forceName: numContacts += 1
print('Removing contact force elements from model...')
contactsRemoved = 0
while contactsRemoved < numContacts:
for i in np.arange(forceSet.getSize()):
forceName = forceSet.get(int(i)).getName()
if 'contact' in forceName:
print(' --> removed', forceSet.get(int(i)).getName())
forceSet.remove(int(i))
contactsRemoved += 1
break
modelProcessor = osim.ModelProcessor(model)
modelProcessor.append(osim.ModOpReplaceJointsWithWelds(
['subtalar_r', 'mtp_r', 'subtalar_l', 'mtp_l', 'radius_hand_r',
'radius_hand_l']))
modelProcessor.append(osim.ModOpReplaceMusclesWithDeGrooteFregly2016())
modelProcessor.append(osim.ModOpIgnorePassiveFiberForcesDGF())
modelProcessor.append(osim.ModOpIgnoreTendonCompliance())
modelProcessor.append(osim.ModOpFiberDampingDGF(0))
baseModel = modelProcessor.process()
# Create model for CMC
modelProcessorCMC = osim.ModelProcessor(baseModel)
modelProcessorCMC.append(osim.ModOpAddReserves(1))
cmcModel = modelProcessorCMC.process()
tasks = osim.CMC_TaskSet()
for coord in cmcModel.getCoordinateSet():
cname = coord.getName()
if cname.endswith('_beta'):
continue
task = osim.CMC_Joint()
task.setName(cname)
task.setCoordinateName(cname)
task.setKP(100, 1, 1)
task.setKV(20, 1, 1)
task.setActive(True, False, False)
tasks.cloneAndAppend(task)
tasks.printToXML(
os.path.join(root_dir, 'code',
'motion_prescribed_walking_cmc_tasks.xml'))
cmcModel.printToXML(os.path.join(root_dir, 'resources/Rajagopal2016/'
'subject_walk_for_cmc.osim'))
# Create direct collocation model:
# - TODO reserves
# - implicit tendon compliance mode
# - add external loads
def add_reserve(model, coord, max_control):
actu = osim.CoordinateActuator(coord)
if coord.startswith('lumbar'):
prefix = 'torque_'
elif coord.startswith('pelvis'):
prefix = 'residual_'
else:
prefix = 'reserve_'
actu.setName(prefix + coord)
actu.setMinControl(-1)
actu.setMaxControl(1)
actu.setOptimalForce(max_control)
model.addForce(actu)
add_reserve(baseModel, 'lumbar_extension', 50)
add_reserve(baseModel, 'lumbar_bending', 50)
add_reserve(baseModel, 'lumbar_rotation', 20)
for side in ['_l', '_r']:
add_reserve(baseModel, f'arm_flex{side}', 15)
add_reserve(baseModel, f'arm_add{side}', 15)
add_reserve(baseModel, f'arm_rot{side}', 5)
add_reserve(baseModel, f'elbow_flex{side}', 5)
add_reserve(baseModel, f'pro_sup{side}', 1)
add_reserve(baseModel, f'hip_rotation{side}', 0.5)
add_reserve(baseModel, 'pelvis_tilt', 60)
add_reserve(baseModel, 'pelvis_list', 30)
add_reserve(baseModel, 'pelvis_rotation', 15)
add_reserve(baseModel, 'pelvis_tx', 100)
add_reserve(baseModel, 'pelvis_ty', 200)
add_reserve(baseModel, 'pelvis_tz', 35)
baseModel.initSystem()
muscles = baseModel.updMuscles()
for imusc in np.arange(muscles.getSize()):
muscle = muscles.get(int(imusc))
if 'gas' in muscle.getName() or 'soleus' in muscle.getName():
muscle.set_ignore_tendon_compliance(False)
modelProcessorDC = osim.ModelProcessor(baseModel)
modelProcessorDC.append(osim.ModOpFiberDampingDGF(0.01))
modelProcessorDC.append(osim.ModOpAddReserves(1, 2.5, True))
modelProcessorDC.append(
osim.ModOpTendonComplianceDynamicsModeDGF('implicit'))
ext_loads_xml = os.path.join(root_dir,
"resources/Rajagopal2016/grf_walk.xml")
modelProcessorDC.append(osim.ModOpAddExternalLoads(ext_loads_xml))
return modelProcessorDC
def test_markAdoptedSets(self):
# Set's.
fus = osim.FunctionSet()
fu1 = osim.Constant()
fus.adoptAndAppend(fu1)
del fus
del fu1
s = osim.ScaleSet()
o = osim.Scale()
s.adoptAndAppend(o)
del s
del o
s = osim.ControlSet()
o = osim.ControlLinear()
s.adoptAndAppend(o)
del s
del o
s = osim.BodyScaleSet()
o = osim.BodyScale()
s.adoptAndAppend(o)
del s
del o
s = osim.PathPointSet()
o = osim.PathPoint()
s.adoptAndAppend(o)
del s
del o
s = osim.IKTaskSet()
o = osim.IKMarkerTask()
s.adoptAndAppend(o)
del s
del o
s = osim.MarkerPairSet()
o = osim.MarkerPair()
s.adoptAndAppend(o)
del s
del o
s = osim.MeasurementSet()
o = osim.Measurement()
s.adoptAndAppend(o)
del s
del o
s = osim.FrameSet()
o = osim.Body()
s.adoptAndAppend(o)
del s
del o
s = osim.ForceSet()
o = osim.CoordinateLimitForce()
s.adoptAndAppend(o)
del s
del o
s = osim.ForceSet()
o = osim.SpringGeneralizedForce()
s.append(o)
s = osim.ProbeSet()
o = osim.Umberger2010MuscleMetabolicsProbe()
s.adoptAndAppend(o)
del s
del o
a = osim.Model()
body = osim.Body('body',
1.0,
osim.Vec3(0, 0, 0),
osim.Inertia(0, 0, 0)
)
loc_in_parent = osim.Vec3(0, -0, 0)
orient_in_parent = osim.Vec3(0, 0, 0)
loc_in_body = osim.Vec3(0, 0, 0)
orient_in_body = osim.Vec3(0, 0, 0)
joint = osim.WeldJoint("weld_joint",
a.getGround(),
loc_in_parent, orient_in_parent,
body,
loc_in_body, orient_in_parent)
a.addBody(body)
constr = osim.ConstantDistanceConstraint()
constr.setBody1ByName("ground")
constr.setBody1PointLocation(osim.Vec3(0, 0, 0))
constr.setBody2ByName("body")
constr.setBody2PointLocation(osim.Vec3(1, 0, 0))
constr.setConstantDistance(1)
a.addConstraint(constr)
def test_Joint(self):
a = osim.Model()
body = osim.Body('body',
1.0,
osim.Vec3(0, 0, 0),
osim.Inertia(0, 0, 0)
)
loc_in_parent = osim.Vec3(0, -0, 0)
orient_in_parent = osim.Vec3(0, 0, 0)
loc_in_body = osim.Vec3(0, 0, 0)
orient_in_body = osim.Vec3(0, 0, 0)
joint = osim.FreeJoint("joint",
a.getGround(),
loc_in_parent, orient_in_parent,
body,
loc_in_body, orient_in_parent)
del joint
spatialTransform = osim.SpatialTransform()
joint = osim.CustomJoint("joint",
a.getGround(),
loc_in_parent, orient_in_parent,
body,
loc_in_body, orient_in_parent, spatialTransform)
del joint
joint = osim.EllipsoidJoint("joint",
a.getGround(),
loc_in_parent, orient_in_parent,
body,
loc_in_body, orient_in_parent, osim.Vec3(1, 1, 1))
del joint
joint = osim.BallJoint("joint",
a.getGround(),
loc_in_parent, orient_in_parent,
body,
loc_in_body, orient_in_parent)
del joint
joint = osim.PinJoint("joint",
a.getGround(),
loc_in_parent, orient_in_parent,
body,
loc_in_body, orient_in_parent)
del joint
joint = osim.SliderJoint("joint",
a.getGround(),
loc_in_parent, orient_in_parent,
body,
loc_in_body, orient_in_parent)
del joint
joint = osim.WeldJoint("joint",
a.getGround(),
loc_in_parent, orient_in_parent,
body,
loc_in_body, orient_in_parent)
del joint
joint = osim.GimbalJoint("joint",
a.getGround(),
loc_in_parent, orient_in_parent,
body,
loc_in_body, orient_in_parent)
del joint
joint = osim.UniversalJoint("joint",
a.getGround(),
loc_in_parent, orient_in_parent,
body,
loc_in_body, orient_in_parent)
del joint
joint = osim.PlanarJoint("joint",
a.getGround(),
loc_in_parent, orient_in_parent,
body,
loc_in_body, orient_in_parent)
del joint
def optimMuscleParams(osimModel_ref_filepath, osimModel_targ_filepath, N_eval,
log_folder):
# results file identifier
res_file_id_exp = '_N' + str(N_eval)
# import models
osimModel_ref = opensim.Model(osimModel_ref_filepath)
osimModel_targ = opensim.Model(osimModel_targ_filepath)
# models details
name = Path(osimModel_targ_filepath).stem
ext = Path(osimModel_targ_filepath).suffix
# assigning new name to the model
osimModel_opt_name = name + '_opt' + res_file_id_exp + ext
osimModel_targ.setName(osimModel_opt_name)
# initializing log file
log_folder = Path(log_folder)
log_folder.mkdir(parents=True, exist_ok=True)
logging.basicConfig(filename=str(log_folder) + '/' + name + '_opt' +
res_file_id_exp + '.log',
filemode='w',
format='%(levelname)s:%(message)s',
level=logging.INFO)
# get muscles
muscles = osimModel_ref.getMuscles()
muscles_scaled = osimModel_targ.getMuscles()
# initialize with recognizable values
LmOptLts_opt = -1000 * np.ones((muscles.getSize(), 2))
SimInfo = {}
for n_mus in range(0, muscles.getSize()):
tic = time()
# current muscle name (here so that it is possible to choose a single muscle when developing).
curr_mus_name = muscles.get(n_mus).getName()
print('processing mus ' + str(n_mus + 1) + ': ' + curr_mus_name)
# import muscles
curr_mus = muscles.get(curr_mus_name)
curr_mus_scaled = muscles_scaled.get(curr_mus_name)
# extracting the muscle parameters from reference model
LmOptLts = [
curr_mus.getOptimalFiberLength(),
curr_mus.getTendonSlackLength()
]
PenAngleOpt = curr_mus.getPennationAngleAtOptimalFiberLength()
Mus_ref = sampleMuscleQuantities(osimModel_ref, curr_mus, 'all',
N_eval)
# calculating minimum fiber length before having pennation 90 deg
# acos(0.1) = 1.47 red = 84 degrees, chosen as in OpenSim
limitPenAngle = np.arccos(0.1)
# this is the minimum length the fiber can be for geometrical reasons.
LfibNorm_min = np.sin(PenAngleOpt) / np.sin(limitPenAngle)
# LfibNorm as calculated above can be shorter than the minimum length
# at which the fiber can generate force (taken to be 0.5 Zajac 1989)
if LfibNorm_min < 0.5:
LfibNorm_min = 0.5
# muscle-tendon paramenters value
MTL_ref = [musc_param_iter[0] for musc_param_iter in Mus_ref]
LfibNorm_ref = [musc_param_iter[1] for musc_param_iter in Mus_ref]
LtenNorm_ref = [
musc_param_iter[2] / LmOptLts[1] for musc_param_iter in Mus_ref
]
penAngle_ref = [musc_param_iter[4] for musc_param_iter in Mus_ref]
# LfibNomrOnTen_ref = LfibNorm_ref.*cos(penAngle_ref)
LfibNomrOnTen_ref = [(musc_param_iter[1] * np.cos(musc_param_iter[4]))
for musc_param_iter in Mus_ref]
# checking the muscle configuration that do not respect the condition.
okList = [
pos for pos, value in enumerate(LfibNorm_ref)
if value > LfibNorm_min
]
# keeping only acceptable values
MTL_ref = np.array([MTL_ref[index] for index in okList])
LfibNorm_ref = np.array([LfibNorm_ref[index] for index in okList])
LtenNorm_ref = np.array([LtenNorm_ref[index] for index in okList])
penAngle_ref = np.array([penAngle_ref[index] for index in okList])
LfibNomrOnTen_ref = np.array(
[LfibNomrOnTen_ref[index] for index in okList])
# in the target only MTL is needed for all muscles
MTL_targ = sampleMuscleQuantities(osimModel_targ, curr_mus_scaled,
'MTL', N_eval)
evalTotPoints = len(MTL_targ)
MTL_targ = np.array([MTL_targ[index] for index in okList])
evalOkPoints = len(MTL_targ)
# The problem to be solved is:
# [LmNorm*cos(penAngle) LtNorm]*[Lmopt Lts]' = MTL;
# written as Ax = b or their equivalent (A^T A) x = (A^T b)
A = np.array([LfibNomrOnTen_ref, LtenNorm_ref]).T
b = MTL_targ
# ===== LINSOL =======
# solving the problem to calculate the muscle param
x = linalg.solve(np.dot(A.T, A), np.dot(A.T, b))
LmOptLts_opt[n_mus] = x
# checking the results
if np.min(x) <= 0:
# informing the user
line0 = ' '
line1 = 'Negative value estimated for muscle parameter of muscle ' + curr_mus_name + '\n'
line2 = ' Lm Opt Lts' + '\n'
line3 = 'Template model : ' + str(LmOptLts) + '\n'
line4 = 'Optimized param : ' + str(LmOptLts_opt[n_mus]) + '\n'
# ===== IMPLEMENTING CORRECTIONS IF ESTIMATION IS NOT CORRECT =======
x = optimize.nnls(np.dot(A.T, A), np.dot(A.T, b))
x = x[0]
LmOptLts_opt[n_mus] = x
line5 = 'Opt params (optimize.nnls): ' + str(LmOptLts_opt[n_mus])
logging.info(line0 + line1 + line2 + line3 + line4 + line5 + '\n')
# In our tests, if something goes wrong is generally tendon slack
# length becoming negative or zero because tendon length doesn't change
# throughout the range of motion, so lowering the rank of A.
if np.min(x) <= 0:
# analyzes of Lten behaviour
Lten_ref = [musc_param_iter[2] for musc_param_iter in Mus_ref]
Lten_ref = np.array([Lten_ref[index] for index in okList])
if (np.max(Lten_ref) - np.min(Lten_ref)) < 0.0001:
logging.warning(
' Tendon length not changing throughout range of motion'
)
# calculating proportion of tendon and fiber
Lten_fraction = Lten_ref / MTL_ref
Lten_targ = Lten_fraction * MTL_targ
# first round: optimizing Lopt maintaing the proportion of
# tendon as in the reference model
A1 = np.array([LfibNomrOnTen_ref, LtenNorm_ref * 0]).T
b1 = MTL_targ - Lten_targ
x1 = optimize.nnls(np.dot(A1.T, A1), np.dot(A1.T, b1))
x[0] = x1[0][0]
# second round: using the optimized Lopt to recalculate Lts
A2 = np.array([LfibNomrOnTen_ref * 0, LtenNorm_ref]).T
b2 = MTL_targ - np.dot(A1, x1[0])
x2 = optimize.nnls(np.dot(A2.T, A2), np.dot(A2.T, b2))
x[1] = x2[0][1]
LmOptLts_opt[n_mus] = x
# Here tests about/against optimizers were implemented
# calculating the error (mean squared errors)
fval = mean_squared_error(b, np.dot(A, x), squared=False)
# update muscles from scaled model
curr_mus_scaled.setOptimalFiberLength(LmOptLts_opt[n_mus][0])
curr_mus_scaled.setTendonSlackLength(LmOptLts_opt[n_mus][1])
# PRINT LOGS
toc = time() - tic
line0 = ' '
line1 = 'Calculated optimized muscle parameters for ' + curr_mus.getName(
) + ' in ' + str(toc) + ' seconds.' + '\n'
line2 = ' Lm Opt Lts' + '\n'
line3 = 'Template model : ' + str(LmOptLts) + '\n'
line4 = 'Optimized param : ' + str(LmOptLts_opt[n_mus]) + '\n'
line5 = 'Nr of eval points : ' + str(evalOkPoints) + '/' + str(
evalTotPoints) + ' used' + '\n'
line6 = 'fval : ' + str(fval) + '\n'
line7 = 'var from template [%]: ' + str(
100 *
(np.abs(LmOptLts - LmOptLts_opt[n_mus])) / LmOptLts) + '%' + '\n'
logging.info(line0 + line1 + line2 + line3 + line4 + line5 + line6 +
line7 + '\n')
# SIMULATION INFO AND RESULTS
SimInfo[n_mus] = {}
SimInfo[n_mus]['colheader'] = curr_mus.getName()
SimInfo[n_mus]['LmOptLts_ref'] = LmOptLts
SimInfo[n_mus]['LmOptLts_opt'] = LmOptLts_opt[n_mus]
SimInfo[n_mus]['varPercLmOptLts'] = 100 * (
np.abs(LmOptLts - LmOptLts_opt[n_mus])) / LmOptLts
SimInfo[n_mus]['sampledEvalPoints'] = evalOkPoints
SimInfo[n_mus]['sampledEvalPoints'] = evalTotPoints
SimInfo[n_mus]['fval'] = fval
# assigning optimized model as output
osimModel_opt = osimModel_targ
return osimModel_opt, SimInfo
def optimizer_callBack(x):
model.printToXML("C:/Users/mhmdk/Desktop/Co-op files/co-op semester 1/optimizers_2/friction/copy_hangingHarnessModel_fric_lim.osim")
copy = osim.Model("C:/Users/mhmdk/Desktop/Co-op files/co-op semester 1/optimizers_2/friction/copy_hangingHarnessModel_fric_lim.osim")
frcSet = copy.getForceSet()
ks = 39810
angleRad = x[0]*math.pi/180
copy.initSystem()
reporter = osim.ForceReporter(copy)
copy.addAnalysis(reporter)
fwd_tool = osim.ForwardTool()
fwd_tool.setModel(copy)
file_name = "cont1.xml"
controls_writer(file_name, x[1])
opt_changer(7, 1, frcSet)
opt_changer(8, 1, frcSet)
fwd_tool.setControlsFileName(file_name)
fwd_tool.addControllerSetToModel()
fwd_tool.setFinalTime(3)
shifted_strap_right = angle_shifter(length = length_calc(legR_strap), angle = angleRad, s = "r", tup = legR_strap)
shifted_strap_left = angle_shifter(length = length_calc(legL_strap), angle = angleRad, s = "l", tup = legL_strap)
len_new = length_calc(shifted_strap_right)
right_start = shifted_strap_right[0]
right_end = shifted_strap_right[1]
rs = refactor(arr = right_start, arrRef = hip_pelvis)
rE = refactor(arr = right_end, arrRef = legR_pelvis)
left_start = shifted_strap_left[0]
left_end = shifted_strap_left[1]
ls = refactor(arr = left_start, arrRef = hip_pelvis)
le = refactor(arr = left_end, arrRef = legL_pelvis)
vecRs = osim.Vec3(rs[0], rs[1], rs[2])
vecRe = osim.Vec3(rE[0], rE[1], rE[2])
vecLs = osim.Vec3(ls[0], ls[1], ls[2])
vecLe = osim.Vec3(le[0], le[1], le[2])
baseR = frcSet.get(1)
rightLeg = osim.PathSpring.safeDownCast(baseR)
geoR = rightLeg.upd_GeometryPath()
ppset_r = geoR.updPathPointSet()
point_start_r = ppset_r.get(0)
point_end_r = ppset_r.get(1)
rightLeg.setRestingLength(len_new)
rightLeg.setStiffness(ks)
point_start_r.setLocation(copy.initSystem(), vecRs)
point_end_r.setLocation(copy.initSystem(), vecRe)
baseL = frcSet.get(2)
leftLeg = osim.PathSpring.safeDownCast(baseL)
geoL = leftLeg.upd_GeometryPath()
ppset_l = geoL.updPathPointSet()
point_start_l = ppset_l.get(0)
point_end_l = ppset_l.get(1)
leftLeg.setRestingLength(len_new)
leftLeg.setStiffness(ks)
point_start_l.setLocation(copy.initSystem(), vecLs)
point_end_l.setLocation(copy.initSystem(), vecLe)
copy.printToXML("C:/Users/mhmdk/Desktop/Co-op files/co-op semester 1/optimizers_2/friction/copy_hangingHarnessModel_fric_lim.osim")
fwd_tool.run()
storage = reporter.getForceStorage()
lbls = storage.getColumnLabels()
stateVec = storage.getLastStateVector()
dataSet = stateVec.getData()
os.remove("copy_hangingHarnessModel_fric_lim.osim")
for i in range(1, 11):
arr_ind = i - 1
lbls_ind = i + 1
arr[arr_ind] = dataSet.get(i)
print(lbls.get(lbls_ind) + ": " + str(dataSet.get(i)))
return x[1]**2 + (math.floor(float(arr[2])) - math.floor(float(arr[4])))**2 + dataSet.get(9)**2
XML_generic_IK_path = 'XML\IK.xml' # Path to the generic IK XML file
markerList = np.array([
'RSHO', 'LSHO', 'C7', 'STRN', 'RELB', 'RWRA', 'RWRB', 'LELB', 'LWRA',
'LWRB', 'RASI', 'LASI', 'RPSI', 'LPSI', 'RTHI', 'RKNE', 'INRKNE', 'RTIB',
'RANK', 'RHEE', 'RTOE', 'RPINKY', 'LTHI', 'LKNE', 'INLKNE', 'LTIB', 'LANK',
'LHEE', 'LTOE', 'LPINKY', 'T10', 'CLAV', 'LFIN', 'RFIN'
])
markerWeight = np.array([
1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
1, 1, 1, 1, 1, 1, 1, 1, 1
])
# Launch the musculoskeletal model
osimModel = osim.Model(scaled_MM_Moved_path)
state = osimModel.initSystem()
TRC_file = TRC_files[0]
path, filename = os.path.split(TRC_file)
filename, ext = os.path.splitext(filename)
MOT_file = 'MOT\\' + filename + '.mot' # Path to the output .MOT file that will be created and contain the IK results
XML_IK_file = 'XML\\' + filename + '_IK.xml' # Path to the IK XML file that will be created
# Marker Data
markerData = osim.MarkerData(TRC_file)
initial_time = markerData.getStartFrameTime()
final_time = markerData.getLastFrameTime()
# Create InverseKinematicsTool.
ik_tool = InverseKinematicsTool(ik_setup_file)
ik_tool.model = model
rmse = ik_tool.solve()
# Calculate the mean RMSE
mean_rmse = np.mean(rmse)
print(mean_rmse)
return mean_rmse
########### START #############
# Load model to get intial parameters for optimising
model = osim.Model(
r'C:\Users\llim726\Documents\MASTERS\121719\opensim\infant_model_v4_opt.osim'
)
# Locate the left hip joint and get the initial location parameters
jointset = model.getJointSet()
hip_l = jointset.get('hip_l')
hlx = hip_l.get_frames(0).get_translation()[0]
hly = hip_l.get_frames(0).get_translation()[1]
hlz = hip_l.get_frames(0).get_translation()[2]
hip_l.get_frames(0).set_translation(osim.Vec3(-0.05, -0.05, -0.05))
s = model.initSystem()
model.printToXML(
r"C:\Users\llim726\Documents\MASTERS\121719\opensim\infant_model_v4_opt.osim"
)
def process_files(infile, outdir, jointsonly):
if not os.path.exists(outdir):
os.makedirs(outdir)
model = osim.Model(infile)
s = model.initSystem()
bodies = []
for body in model.getBodySet():
print(body.getName())
# Check if geometry exists (could be just a joint)
check_geom_string = body.getPropertyByName(
'attached_geometry').toString()
mesh_file_name = 'none'
if check_geom_string == '(Mesh)':
geom = body.get_attached_geometry(0)
mesh_file_name = geom.getPropertyByName('mesh_file').toString()
name = body.getName()
p = body.getPositionInGround(s)
r = body.getTransformInGround(s).R()
# Construct Transformation matrix (4x4)
t = np.array([[r.get(0, 0),
r.get(0, 1),
r.get(0, 2),
p.get(0)],
[r.get(1, 0),
r.get(1, 1),
r.get(1, 2),
p.get(1)],
[r.get(2, 0),
r.get(2, 1),
r.get(2, 2),
p.get(2)], [0, 0, 0, 1]])
mesh_geom = Geometry(name, mesh_file_name, t)
bodies.append(mesh_geom)
coords = dict()
cs = model.getCoordinateSet()
for i in range(model.getNumCoordinates()):
coords[cs.get(i).toString()] = cs.get(i).getValue(s)
joints = []
for joint in model.getJointSet():
if joint.getNumProperties() < 6:
continue
if joint.numCoordinates() == 0:
continue
print(joint.getName())
f = joint.getParentFrame()
sock = f.getSocket('parent')
body_name = sock.getConnecteePath()
body_name = body_name.split('/')[-1]
geom = find_geom_by_body(body_name, bodies)
p = f.getPositionInGround(s)
st = joint.getPropertyByName('SpatialTransform')
r_str = []
r_name = []
r = []
R = []
for i in range(3):
t = np.copy(geom.t)
R_body = t[0:3, 0:3]
coord_name = st.getValueAsObject().getPropertyByIndex(
i).getValueAsObject().getPropertyByIndex(0).toString().strip(
'()')
if coord_name == '':
continue
r_name.append(coord_name)
r_str.append(
st.getValueAsObject().getPropertyByIndex(i).getValueAsObject(
).getPropertyByIndex(1).toString().strip('()').split(' '))
r.append([float(j) for j in r_str[-1]])
# arrow (0,1,0)
y_axis = np.array([0, 1, 0])
# rorate around y-axis to coordinate
R_coord = rotation_matrix(coords[coord_name], y_axis)
# rotation to align y-axis to rotational axis
R_rot = rotate_from_to(y_axis, r[-1])
# Original solution (wrong, but gets the job half-done)
# R.append(np.dot(R_body,R_rot)) # align y-axis
R.append(np.dot(R_body, R_rot)) # align y-axi
R[-1] = np.dot(R[-1], R_coord[0:3, 0:3]) # rotate around rot-axis
t[0:3, 0:3] = R[-1]
t[0, 3] = p.get(0) # but point to actual pivot
t[1, 3] = p.get(1)
t[2, 3] = p.get(2)
mesh_geom = Geometry(('joint-' + coord_name), 'joint', t)
joints.append(mesh_geom)
# Get ground object
ground = model.get_ground()
ground.getPropertyByName('attached_geometry').toString()
ground_mesh_name = 'none'
if check_geom_string == '(Mesh)':
geom = ground.get_attached_geometry(0)
ground_mesh_name = geom.getPropertyByName('mesh_file').toString()
bodies = bodies + joints # concat lists
meshes = []
print('Converting ' + str(len(bodies)) + ' bodies...')
for body in bodies:
print('Processing:' + body.name)
if body.body == 'none':
body_mesh = mesh.Mesh.from_file('reference_cube.stl')
elif body.body == 'joint':
body_mesh = mesh.Mesh.from_file('reference_arrow.stl')
else:
if jointsonly:
continue
body_mesh = mesh.Mesh.from_file('Geometry/' + body.body)
body_mesh.transform(body.t)
body_mesh.save(outdir + '/' + body.name + '.stl', mode=stl.Mode.ASCII)
meshes.append(body_mesh)
# Save ground mesh (needs no transform)
if ground_mesh_name != 'none' and not jointsonly:
# Change from vtp to stl too
ground_mesh = mesh.Mesh.from_file('Geometry/' + ground_mesh_name)
else:
# Use cube for joints
ground_mesh = mesh.Mesh.from_file('reference_cube.stl')
ground_mesh.save(outdir + '/ground_mesh.stl', mode=stl.Mode.ASCII)
meshes.append(ground_mesh)
print('Exporting Combined Mesh...')
combined = mesh.Mesh(np.concatenate([combine.data for combine in meshes]))
combined.save(outdir + '/combined_mesh.stl', mode=stl.Mode.ASCII)
def test_addMetabolicProbes(self):
model = osim.Model(
os.path.join(test_dir, "gait10dof18musc_subject01.osim"))
# Twitch ratios for gait1018.
twitchRatios = {
'hamstrings': 0.49,
'bifemsh': 0.53,
'glut_max': 0.55,
'iliopsoas': 0.50,
'rect_fem': 0.39,
'vasti': 0.50,
'gastroc': 0.54,
'soleus': 0.80,
'tib_ant': 0.70
}
# Parameters used for all probes
# ------------------------------
# The following booleans are constructor arguments for the Umberger probe.
# These settings are used for all probes.
activationMaintenanceRateOn = True
shorteningRateOn = True
basalRateOn = False
mechanicalWorkRateOn = True
# The mass of each muscle will be calculated using data from the model:
# muscleMass = (maxIsometricForce / sigma) * rho * optimalFiberLength
# where sigma = 0.25e6 is the specific tension of mammalian muscle (in
# Pascals) and rho = 1059.7 is the density of mammalian muscle (in kg/m^3).
# The slow-twitch ratio used for muscles that either do not appear in the
# file, or appear but whose proportion of slow-twitch fibers is unknown.
defaultTwitchRatio = 0.5
# Whole-body probe
# ----------------
# Define a whole-body probe that will report the total metabolic energy
# consumption over the simulation.
wholeBodyProbe = osim.Umberger2010MuscleMetabolicsProbe(
activationMaintenanceRateOn, shorteningRateOn, basalRateOn,
mechanicalWorkRateOn)
wholeBodyProbe.setOperation("value")
wholeBodyProbe.set_report_total_metabolics_only(False)
# Add the probe to the model and provide a name.
model.addProbe(wholeBodyProbe)
wholeBodyProbe.setName("metabolic_power")
# Loop through all muscles, adding parameters for each into the whole-body
# probe.
for iMuscle in range(model.getMuscles().getSize()):
thisMuscle = model.getMuscles().get(iMuscle)
# Get the slow-twitch ratio from the data we read earlier. Start with
# the default value.
slowTwitchRatio = defaultTwitchRatio
# Set the slow-twitch ratio to the physiological value, if it is known.
for key, val in twitchRatios.items():
if thisMuscle.getName().startswith(key) and val != -1:
slowTwitchRatio = val
# Add this muscle to the whole-body probe. The arguments are muscle
# name, slow-twitch ratio, and muscle mass. Note that the muscle mass
# is ignored unless we set useProvidedMass to True.
wholeBodyProbe.addMuscle(thisMuscle.getName(), slowTwitchRatio)
name = os.path.join(test_dir, 'gait10dof18musc_probed.osim')
model.printToXML(name)
os.remove(name)
def calculate_spanning_muscle_coordinates(model_file, results_dir):
"""Calculates the coordinates that are spanned by each muscle. Useful for
reducing the required computation of the muscle moment arm matrix.
"""
model = opensim.Model(model_file)
state = model.initSystem()
# construct model tree (parent body - joint - child body)
model_tree = []
for i in range(0, model.getJointSet().getSize()):
joint = model.getJointSet().get(i)
model_tree.append({
'parent': joint.getParentName(),
'joint': joint.getName(),
'child': joint.getBody().getName()
})
ordered_body_set = []
for i in range(0, model.getBodySet().getSize()):
ordered_body_set.append(model.getBodySet().get(i).getName())
ordered_coordinate_set = []
for i in range(0, model.getCoordinateSet().getSize()):
ordered_coordinate_set.append(model.getCoordinateSet().get(i).getName())
# get the coordinates that are spanned by the muscles
muscle_coordinates = {}
for i in range(0, model.getMuscles().getSize()):
muscle = model.getMuscles().get(i)
path = muscle.getGeometryPath().getPathPointSet()
muscle_bodies = []
for j in range(0, path.getSize()):
point = path.get(j)
muscle_bodies.append(point.getBodyName())
# remove duplicate bodies and sort by multibody tree order
muscle_bodies = list(set(muscle_bodies))
muscle_bodies = sorted(muscle_bodies,
key=lambda x: ordered_body_set.index(x))
# find intermediate joints
assert(len(muscle_bodies) > 1)
joints = []
find_intermediate_joints(muscle_bodies[0], muscle_bodies[-1],
model_tree, joints)
# find spanning coordinates
muscle_coordinates[muscle.getName()] = []
for joint in joints:
joint = model.getJointSet().get(joint)
for c in range(0, joint.get_CoordinateSet().getSize()):
coordinate = joint.get_CoordinateSet().get(c)
if coordinate.isDependent(state):
continue
else:
muscle_coordinates[muscle.getName()].append(coordinate.getName())
# sort coordinates by model order
muscle_coordinates[muscle.getName()] = sorted(
muscle_coordinates[muscle.getName()],
key=lambda x: ordered_coordinate_set.index(x))
# write results to file
with open(results_dir + 'muscle_coordinates.csv', 'w') as csv_file:
for key, values in muscle_coordinates.items():
csv_file.write(key)
for value in values:
csv_file.write(';' + value)
csv_file.write('\n')
def perform_jra(model_file, ik_file, grf_file, grf_xml, reserve_actuators,
muscle_forces_file, results_dir, prefix, joint_names,
apply_on_bodies, express_in_frame):
"""Performs Static Optimization using OpenSim.
Parameters
----------
model_file: str
OpenSim model (.osim)
ik_file: str
kinematics calculated from Inverse Kinematics
grf_file: str
the ground reaction forces
grf_xml: str
xml description containing how to apply the GRF forces
reserve_actuators: str
path to the reserve actuator .xml file
muscle_forces_file: str
path to the file containing the muscle forces from SO
results_dir: str
directory to store the results
prefix: str
prefix of the resultant joint reaction loads
joint_names: list
joint names of interest
apply_on_bodies: list
apply on child or parent
express_in_frame: list
frame to express results
"""
assert (len(joint_names) == len(apply_on_bodies) == len(express_in_frame))
# model
model = opensim.Model(model_file)
# prepare external forces xml file
name = os.path.basename(grf_file)[:-8]
# external_loads = opensim.ExternalLoads(model, grf_xml)
# external_loads.setExternalLoadsModelKinematicsFileName(ik_file)
# external_loads.setDataFileName(grf_file)
# external_loads.setLowpassCutoffFrequencyForLoadKinematics(6)
# external_loads.printToXML(results_dir + name + '.xml')
# construct joint reaction analysis
motion = opensim.Storage(ik_file)
joint_reaction = opensim.JointReaction(model)
joint_reaction.setName('JointReaction')
joint_reaction.setStartTime(motion.getFirstTime())
joint_reaction.setEndTime(motion.getLastTime())
joint_reaction.setForcesFileName(muscle_forces_file)
joint_names_arr = opensim.ArrayStr()
apply_on_bodies_arr = opensim.ArrayStr()
express_in_frame_arr = opensim.ArrayStr()
for j, b, f in zip(joint_names, apply_on_bodies, express_in_frame):
joint_names_arr.append(j)
print(j)
apply_on_bodies_arr.append(b)
express_in_frame_arr.append(f)
joint_reaction.setJointNames(joint_names_arr)
joint_reaction.setOnBody(apply_on_bodies_arr)
joint_reaction.setInFrame(express_in_frame_arr)
model.addAnalysis(joint_reaction)
model.initSystem()
# analysis
analysis = opensim.AnalyzeTool(model)
analysis.setName(prefix + name)
analysis.setModel(model)
analysis.setModelFilename(model_file)
analysis.setInitialTime(motion.getFirstTime())
analysis.setFinalTime(motion.getLastTime())
analysis.setLowpassCutoffFrequency(6)
analysis.setCoordinatesFileName(ik_file)
analysis.setExternalLoadsFileName(results_dir + name + '.xml')
analysis.setLoadModelAndInput(True)
analysis.setResultsDir(results_dir)
analysis.run()
jra_file = results_dir + name + '_JointReaction_ReactionLoads.sto'
return jra_file
def perform_so(events_file, model_file, ik_file, grf_file, grf_xml,
results_dir, opensimtools_dir, opensimplugin_dir, settings_dir):
"""Performs Static Optimization using OpenSim.
Parameters
----------
model_file: str
OpenSim model (.osim)
ik_file: str
kinematics calculated from Inverse Kinematics
grf_file: str
the ground reaction forces
grf_xml: str
xml description containing how to apply the GRF forces
reserve_actuators: str
path to the reserve actuator .xml file
results_dir: str
directory to store the results
"""
# model
model = opensim.Model(model_file)
# prepare external forces xml file
trial = os.path.basename(grf_file).strip('_GRF.mot')
# external_loads = opensim.ExternalLoads(model, grf_xml)
# external_loads.setExternalLoadsModelKinematicsFileName(ik_file)
# external_loads.setDataFileName(grf_file)
# external_loads.setLowpassCutoffFrequencyForLoadKinematics(6)
# external_loads.printToXML(settings_dir + trial + 'ExternalLoads.xml')
""" Create the Actuators file"""
subj_dir = os.path.dirname(model_file)
data_dir = os.path.dirname(subj_dir)
name = os.path.basename(model_file).strip('.osim')
actuators_file = parse(data_dir + '\Actuators.xml')
"""Get the mass center of the pelvis"""
model = parse(model_file)
massCenter = model.getElementsByTagName('BodySet')[0].getElementsByTagName('objects')[0].\
getElementsByTagName('Body')[1].getElementsByTagName('mass_center')[0]\
.childNodes[0].nodeValue
"""Assign the mass center from the model to the Actuators file and save"""
for i in range(0, 3):
actuators_file.getElementsByTagName('ForceSet')[0].getElementsByTagName('objects')[0].\
getElementsByTagName('PointActuator')[i].getElementsByTagName('point')[0].childNodes[0].nodeValue = massCenter
F = open(settings_dir + 'Actuators.xml', "w")
actuators_file.writexml(F)
F.close()
## parsing the generic setup file and change accordingly
trial = os.path.basename(grf_file).strip('_GRF.mot')
so_set_file = parse(data_dir + '/SO.xml')
so_set_file.getElementsByTagName(
'AnalyzeTool')[0].attributes['name'].value = name + '_' + trial
so_set_file.getElementsByTagName(
'model_file')[0].childNodes[0].nodeValue = model_file
so_set_file.getElementsByTagName('force_set_files')[0].childNodes[
0].nodeValue = settings_dir + 'Actuators.xml'
so_set_file.getElementsByTagName(
'results_directory')[0].childNodes[0].nodeValue = results_dir
#motion = opensim.Storage(ik_file)
# initial_time = motion.getFirstTime()
#final_time = motion.getLastTime()
so_set_file.getElementsByTagName('initial_time')[0].childNodes[
0].nodeValue = events_file.InitialContactTime.iloc[0]
so_set_file.getElementsByTagName('final_time')[0].childNodes[
0].nodeValue = events_file.TimeLowestCOMX.iloc[1]
so_set_file.getElementsByTagName('AnalysisSet')[0].getElementsByTagName('objects')[0].\
getElementsByTagName('StaticOptimization')[0].getElementsByTagName('start_time')[0].\
childNodes[0].nodeValue = events_file.InitialContactTime.iloc[0]
so_set_file.getElementsByTagName('AnalysisSet')[0].getElementsByTagName('objects')[0].\
getElementsByTagName('StaticOptimization')[0].getElementsByTagName('end_time')[0].childNodes[0].nodeValue = events_file.TimeLowestCOMX.iloc[1]
so_set_file.getElementsByTagName('external_loads_file')[0].childNodes[0].nodeValue\
= grf_xml
so_set_file.getElementsByTagName(
'coordinates_file')[0].childNodes[0].nodeValue = ik_file
## save the xml file
so_xml = settings_dir + name + '_' + trial + '_Setup_SO.xml'
F = open(so_xml, "w")
so_set_file.writexml(F)
F.close()
so_force_file = results_dir + name + '_' + trial + '_StaticOptimization_force.sto'
so_activations_file = results_dir +name +'_'+ trial + \
'_StaticOptimization_activation.sto'
#run the so tool through system cmd
so_cmd = opensimtools_dir + '\\analyze -S ' + so_xml+' -L ' \
+ opensimplugin_dir + '\CustomLigament.dll'
print(os.system(so_cmd))
return (so_force_file, so_activations_file)
def modelOrigReturner(iD):
return osim.Model(returnOrigPath(index = iD))
def access_model(params):
hlx = params[0]
hly = params[1]
hlz = params[2]
# print('Params: %f %f %f' %(hlx,hly,hlz))
# Load model for running IK on
model = osim.Model(
r'C:\Users\llim726\Documents\MASTERS\121719\opensim\infant_model_v4_opt.osim'
)
# Add geometry path to ensure the geometry files are found
path = r'C:\Users\llim726\Documents\MASTERS\opensim_model\Geometry'
osim.ModelVisualizer.addDirToGeometrySearchPaths(path)
# Locate the left hip joint and edit the location of the parent (pelvis) frame
jointset = model.getJointSet()
hip_l = jointset.get('hip_l')
# print('Translation before: %f %f %f' % (hip_l.get_frames(0).get_translation()[0],hip_l.get_frames(0).get_translation()[1],hip_l.get_frames(0).get_translation()[2]))
hip_l.get_frames(0).set_translation(osim.Vec3(hlx, hly, hlz))
s = model.initSystem()
#model.printToXML(r"C:\Users\llim726\Documents\infant_analysis\jw\jw_Scaled_changing.osim")
# print('Translation after: %f %f %f' % (hip_l.get_frames(0).get_translation()[0],hip_l.get_frames(0).get_translation()[1],hip_l.get_frames(0).get_translation()[2]))
# Load preconfigured IK settings file - create this using the IKTool GUI and save the necessary parameters
ik_settings = osim.InverseKinematicsTool(
r'C:\Users\llim726\Documents\MASTERS\121719\vicon_121719_5sec_ik.xml')
ik_taskset = ik_settings.getIKTaskSet()
ik_taskset.printToXML(
r"C:\Users\llim726\Documents\MASTERS\121719\opensim\iktaskset_5sec.xml"
)
# Apply IK settings to the loaded model and run
ik_settings.setOutputMotionFileName(
r'C:\Users\llim726\Documents\MASTERS\121719\opensim\opt_ik_121719.mot')
#ik_settings.setName(r'G:\My Drive\infant_analysis\4_opensim\jw\jw_Scaled.osim')
ik_settings.setModel(model)
# ik_settings.run()
# ik_settings.printToXML(r"C:\Users\llim726\Documents\infant_analysis\jw\jw_ik_tools.xml")
# # Open log file to read in the IK RMS errors
# f = open(r'C:\Users\llim726\Documents\infant_analysis\out.log', 'r')
# ik_log = f.readlines()
# # Get the start of the Error output of the most recent IK
# for i in range(len(ik_log)):
# if ik_log[i] == 'Running tool .\n':
# rmse_start = i+1
# if not rmse_start:
# sys.exit('No error output found in out.log.')
#
# for i in range(rmse_start,len(ik_log)):
# ind_start = ik_log[i].find('RMS=')
# ind_end = ik_log[i].find(', max')
# if ind_start == -1:
# check = ik_log[i].find('completed')
# if check: # end of results found, can generate the rms array
# break
# elif i == rmse_start:
# rmse = np.array(float(ik_log[i][ind_start+4:ind_end-1]))
# else:
# rmse = np.vstack((rmse,float(ik_log[i][ind_start+4:ind_end-1])))
ik_setup_file = r'C:\Users\llim726\Documents\MASTERS\121719\vicon_121719_5sec_ik.xml'
# Create InverseKinematicsTool.
ik_tool = InverseKinematicsTool(ik_setup_file)
ik_tool.model = model
rmse = ik_tool.solve()
# Calculate the mean RMSE
mean_rmse = np.mean(rmse)
print(mean_rmse)
return mean_rmse
import numpy as np
import opensim
integrator_accuracy = 3e-3
stepsize = 0.00003074
model = opensim.Model('./gait14dof22musc_pros_20180507.osim')
# initialise model's simbody System object. System is a computational representation of the Model
model.initSystem()
# initialise the ModelVisualizer object
model.setUseVisualizer(True)
'''
Defines controller for controlling actuators.
A controller computes and sets the values of the controls for the actuators under its control
PrescribedController specifies functions that prescribe the control values of its actuators as a function of time.
So we will explicitly provide these functions.
'''
brain = opensim.PrescribedController()
muscleSet = model.getMuscles()
### set constant control on all muscles
for j in range(muscleSet.getSize()):
func = opensim.Constant(1.0)
brain.addActuator(muscleSet.get(j))
brain.prescribeControlForActuator(j, func)
model.addController(brain)
model.initSystem()
########### System initialised
# This script goes through OpenSim funcionalties
# required for OpenSim-RL
import opensim
# Settings
stepsize = 0.01
# Load existing model
model_path = "../osim/models/gait9dof18musc.osim"
model = opensim.Model(model_path)
model.setUseVisualizer(True)
# Build the controller (we will iteratively
# update it at every step of the simulation)
brain = opensim.PrescribedController()
controllers = []
# Add actuators to the controller
state = model.initSystem() # we need to initialize the system (?)
muscleSet = model.getMuscles()
forceSet = model.getForceSet()
for j in range(muscleSet.getSize()):
func = opensim.Constant(1.0)
controllers.append(func)
brain.addActuator(muscleSet.get(j))
brain.prescribeControlForActuator(j, func)
model.addController(brain)
blockos = opensim.Body('blockos', 0.0001, opensim.Vec3(0),
# -*- coding: utf-8 -*-
"""
Created on Mon Apr 15 17:41:48 2019
@author: mhmdk
"""
import opensim as osim
import math
from scipy.optimize import minimize
import os
from controlsWriter import controls_writer
model = osim.Model("C:/Users/mhmdk/Desktop/Co-op files/co-op semester 1/optimizers_2/friction/hangingHarnessModel_fric_lim.osim")
#all the indices representing the individual forces are
#dependent on their location in the osim file above
#MAKE SURE THEY CORRESPOND CORRECTLY
def opt_changer(index, val, frcSet):
frcBase = frcSet.get(index)
frc = osim.PathActuator.safeDownCast(frcBase)
frc.setOptimalForce(val)
def external_tension(num, frcSet):
stiffness = float(num)
tensionBase = frcSet.get(0)
tension = osim.PathSpring.safeDownCast(tensionBase)
def test_bounds(self):
model = osim.Model()
model.setName('sliding_mass')
model.set_gravity(osim.Vec3(0, 0, 0))
body = osim.Body('body', 2.0, osim.Vec3(0), osim.Inertia(0))
model.addComponent(body)
joint = osim.SliderJoint('slider', model.getGround(), body)
coord = joint.updCoordinate()
coord.setName('position')
model.addComponent(joint)
actu = osim.CoordinateActuator()
actu.setCoordinate(coord)
actu.setName('actuator')
actu.setOptimalForce(1)
model.addComponent(actu)
study = osim.MocoStudy()
study.setName('sliding_mass')
mp = study.updProblem()
mp.setModel(model)
ph0 = mp.getPhase()
mp.setTimeBounds(osim.MocoInitialBounds(0.),
osim.MocoFinalBounds(0.1, 5.))
assert ph0.getTimeInitialBounds().getLower() == 0
assert ph0.getTimeInitialBounds().getUpper() == 0
self.assertAlmostEqual(ph0.getTimeFinalBounds().getLower(), 0.1)
self.assertAlmostEqual(ph0.getTimeFinalBounds().getUpper(), 5.0)
mp.setTimeBounds([0.2, 0.3], [3.5])
self.assertAlmostEqual(ph0.getTimeInitialBounds().getLower(), 0.2)
self.assertAlmostEqual(ph0.getTimeInitialBounds().getUpper(), 0.3)
self.assertAlmostEqual(ph0.getTimeFinalBounds().getLower(), 3.5)
self.assertAlmostEqual(ph0.getTimeFinalBounds().getUpper(), 3.5)
# Use setter on MocoPhase.
ph0.setTimeBounds([2.2, 2.3], [4.5])
self.assertAlmostEqual(ph0.getTimeInitialBounds().getLower(), 2.2)
self.assertAlmostEqual(ph0.getTimeInitialBounds().getUpper(), 2.3)
self.assertAlmostEqual(ph0.getTimeFinalBounds().getLower(), 4.5)
self.assertAlmostEqual(ph0.getTimeFinalBounds().getUpper(), 4.5)
mp.setStateInfo('slider/position/value', osim.MocoBounds(-5, 5),
osim.MocoInitialBounds(0))
assert -5 == ph0.getStateInfo(
'slider/position/value').getBounds().getLower()
assert 5 == ph0.getStateInfo(
'slider/position/value').getBounds().getUpper()
assert isnan(
ph0.getStateInfo(
'slider/position/value').getFinalBounds().getLower())
assert isnan(
ph0.getStateInfo(
'slider/position/value').getFinalBounds().getUpper())
mp.setStateInfo('slider/position/speed', [-50, 50], [-3], 1.5)
assert -50 == ph0.getStateInfo(
'slider/position/speed').getBounds().getLower()
assert 50 == ph0.getStateInfo(
'slider/position/speed').getBounds().getUpper()
assert -3 == ph0.getStateInfo(
'slider/position/speed').getInitialBounds().getLower()
assert -3 == ph0.getStateInfo(
'slider/position/speed').getInitialBounds().getUpper()
self.assertAlmostEqual(
1.5,
ph0.getStateInfo(
'slider/position/speed').getFinalBounds().getLower())
self.assertAlmostEqual(
1.5,
ph0.getStateInfo(
'slider/position/speed').getFinalBounds().getUpper())
# Use setter on MocoPhase.
ph0.setStateInfo('slider/position/speed', [-6, 10], [-4, 3], [0])
assert -6 == ph0.getStateInfo(
'slider/position/speed').getBounds().getLower()
assert 10 == ph0.getStateInfo(
'slider/position/speed').getBounds().getUpper()
assert -4 == ph0.getStateInfo(
'slider/position/speed').getInitialBounds().getLower()
assert 3 == ph0.getStateInfo(
'slider/position/speed').getInitialBounds().getUpper()
assert 0 == ph0.getStateInfo(
'slider/position/speed').getFinalBounds().getLower()
assert 0 == ph0.getStateInfo(
'slider/position/speed').getFinalBounds().getUpper()
# Controls.
mp.setControlInfo('actuator', osim.MocoBounds(-50, 50))
assert -50 == ph0.getControlInfo('actuator').getBounds().getLower()
assert 50 == ph0.getControlInfo('actuator').getBounds().getUpper()
mp.setControlInfo('actuator', [18])
assert 18 == ph0.getControlInfo('actuator').getBounds().getLower()
assert 18 == ph0.getControlInfo('actuator').getBounds().getUpper()
def test_component_list(self):
model = osim.Model(os.path.join(test_dir, "arm26.osim"))
num_components = 0
for comp in model.getComponentsList():
num_components += 1
assert num_components > 0
num_bodies = 0
for body in model.getBodyList():
num_bodies += 1
body.getMass()
assert num_bodies == 2
num_joints = 0
for joint in model.getJointList():
num_joints += 1
joint.numCoordinates()
assert num_joints == 2
# Custom filtering.
num_bodies = 0
for frame in model.getFrameList():
body = osim.Body.safeDownCast(frame)
if body != None:
num_bodies += 1
print(body.getName())
body.getInertia()
assert num_bodies == 2
model = osim.Model()
ground = model.getGround()
thelenMuscle = osim.Thelen2003Muscle("Darryl", 1, 0.5, 0.5, 0)
thelenMuscle.addNewPathPoint("muscle1-point1", ground,
osim.Vec3(0.0, 0.0, 0.0))
thelenMuscle.addNewPathPoint("muscle1-point2", ground,
osim.Vec3(1.0, 0.0, 0.0))
millardMuscle = osim.Millard2012EquilibriumMuscle(
"Matt", 1, 0.5, 0.5, 0)
millardMuscle.addNewPathPoint("muscle1-point1", ground,
osim.Vec3(0.0, 0.0, 0.0))
millardMuscle.addNewPathPoint("muscle1-point2", ground,
osim.Vec3(1.0, 0.0, 0.0))
model.addComponent(thelenMuscle)
model.addComponent(millardMuscle)
# Total number of muscles is 2.
assert len(set(model.getMuscleList())) == 2
for muscle in model.getMuscleList():
assert (isinstance(muscle, osim.Thelen2003Muscle)
or isinstance(muscle, osim.Millard2012EquilibriumMuscle))
# There is exactly 1 Thelen2003Muscle.
assert len(set(model.getThelen2003MuscleList())) == 1
for muscle in model.getThelen2003MuscleList():
assert isinstance(muscle, osim.Thelen2003Muscle)
# There is exactly 1 Millard2012EquilibriumMuscle.
assert len(set(model.getMillard2012EquilibriumMuscleList())) == 1
for muscle in model.getMillard2012EquilibriumMuscleList():
assert isinstance(muscle, osim.Millard2012EquilibriumMuscle)
scaleTool.getModelScaler().setTimeRange(timeRange)
#Set output files
scaleTool.getModelScaler().setOutputModelFileName('scaledModel.osim')
scaleTool.getModelScaler().setOutputScaleFileName('scaleSet.xml')
#Set marker adjuster parameters
scaleTool.getMarkerPlacer().setOutputMotionFileName('static_motion.mot')
scaleTool.getMarkerPlacer().setOutputModelFileName('scaledModelAdjusted.osim')
#Save and run scale tool
scaleTool.printToXML('scaleSetup.xml')
scaleTool.run()
#Load in scaled model
scaledModel = osim.Model('scaledModelAdjusted.osim')
#The locations and size of the contact sphere parameters go unchanged with
#standard model scaling, so these need to be edited to ensure they are in
#an appropriate place. This can be done based on the scale set parameters
#for the representative bodies.
#Load in the scale set, parsed from the XML tree
xmlTree = et.parse('scaleSet.xml')
xmlRoot = xmlTree.getroot()
#Create a dictionary with segments and scale factors to access for calculations
scaleFactors = {}
#Loop through segments and place in dictionary
for segment in range(0,len(xmlRoot.findall('./ScaleSet/objects/Scale/segment'))):
def test_markAdopted2(self):
a = osim.Model()
# We just need the following not to cause a segfault.
# Model add*
pa = osim.PathActuator()
pa.setName('pa')
a.addForce(pa)
probe = osim.Umberger2010MuscleMetabolicsProbe()
probe.setName('probe')
a.addProbe(probe)
ma = osim.MuscleAnalysis()
ma.setName('ma')
a.addAnalysis(ma)
pc = osim.PrescribedController()
pc.setName('pc')
a.addController(pc)
body = osim.Body('body1',
1.0,
osim.Vec3(0, 0, 0),
osim.Inertia(0, 0, 0)
)
loc_in_parent = osim.Vec3(0, 0, 0)
orient_in_parent = osim.Vec3(0, 0, 0)
loc_in_body = osim.Vec3(0, 0, 0)
orient_in_body = osim.Vec3(0, 0, 0)
print "creating Weld Joint.."
joint = osim.WeldJoint("weld_joint",
a.getGround(),
loc_in_parent, orient_in_parent,
body,
loc_in_body, orient_in_parent)
print "adding a body .."
a.addBody(body)
print "adding a joint .."
a.addJoint(joint)
print "Creating a ConstantDistanceConstraint.."
constr = osim.ConstantDistanceConstraint()
constr.setBody1ByName("ground")
constr.setBody1PointLocation(osim.Vec3(0, 0, 0))
constr.setBody2ByName("body")
constr.setBody2PointLocation(osim.Vec3(1, 0, 0))
constr.setConstantDistance(1)
a.addConstraint(constr)
f = osim.BushingForce("bushing", "ground", "body",
osim.Vec3(2, 2, 2), osim.Vec3(1, 1, 1),
osim.Vec3(0, 0, 0), osim.Vec3(0, 0, 0))
a.addForce(f)
f2 = osim.BushingForce()
a.addForce(f2)
f3 = osim.SpringGeneralizedForce()
a.addForce(f3)
model = osim.Model(os.path.join(test_dir, "arm26.osim"))
g = osim.CoordinateActuator('r_shoulder_elev')
model.addForce(g)
# Demonstrates the use of PerturbationForce plugin with Python. # # author: Dimitar Stanev [email protected] import opensim # The plugin must be loaded so that the new force is recognized by # OpenSim. However, for the force to be used other than .xml a swig wrapper # should be implemented. lib = opensim.LoadOpenSimLibraryExact('libPerturbationForce.so') # Make sure to run TestPerturbationForce so that the output_model.osim is # generate. You can find the PerturbationForce xml description in the .osim file # under the ForceSet. model = opensim.Model('output_model.osim') # model.setUseVisualizer(True) state = model.initSystem() opensim.simulate(model, state, 1, True)
def calculate_moment_arm_symbolically(model_file, results_dir):
"""Calculate the muscle moment arm matrix symbolically for a
particular OpenSim model.
"""
print('Calculating...')
# parse csv
muscle_coordinates = {}
with open(results_dir + 'muscle_coordinates.csv') as csv_file:
reader = csv.reader(csv_file, delimiter=';')
for row in reader:
muscle_coordinates[row[0]] = row[1:]
# load opensim model
model = opensim.Model(model_file)
state = model.initSystem()
model_coordinates = {} # coordinates in multibody order
for i, coordinate in enumerate(model.getCoordinateSet()):
mbix = coordinate.getBodyIndex()
mqix = coordinate.getMobilizerQIndex()
model_coordinates[coordinate.getName()] = (mbix, mqix)
model_coordinates = dict(
sorted(model_coordinates.items(), key=lambda x: x[1]))
for i, (key, value) in enumerate(model_coordinates.items()):
model_coordinates[key] = i
model_muscles = {}
for i, muscle in enumerate(model.getMuscles()):
model_muscles[muscle.getName()] = i
# calculate moment arm matrix (R) symbolically
R = []
sampling_dict = {}
resolution = {1: 15, 2: 15, 3: 15, 4: 15, 5: 10}
for muscle, k in tqdm(model_muscles.items()):
# get initial state each time
coordinates = muscle_coordinates[muscle]
N = resolution[len(coordinates)]
# calculate moment arms for this muscle and spanning coordinates
sampling_grid = construct_coordinate_grid(model, coordinates, N)
moment_arm = []
for q in sampling_grid:
for i, coordinate in enumerate(coordinates):
model.updCoordinateSet().get(coordinate).setValue(state, q[i])
model.realizePosition(state)
tmp = []
for coordinate in coordinates:
coord = model.getCoordinateSet().get(coordinate)
tmp.append(model.getMuscles().get(muscle).computeMomentArm(
state, coord))
moment_arm.append(tmp)
moment_arm = np.array(moment_arm)
sampling_dict[muscle] = {
'coordinates': coordinates,
'sampling_grid': sampling_grid,
'moment_arm': moment_arm
}
# polynomial regression
degree = 5
muscle_moment_row = [0] * len(model_coordinates)
for i, coordinate in enumerate(coordinates):
coeffs, powers = multipolyfit(sampling_grid,
moment_arm[:, i],
degree,
powers_out=True)
polynomial = mk_sympy_function(coeffs, powers)
polynomial = polynomial.subs({
sp.Symbol('x%d' % i): sp.Symbol(x)
for i, x in enumerate(coordinates)
})
muscle_moment_row[model_coordinates[coordinate]] = polynomial
R.append(muscle_moment_row)
# export data to file because the process is time consuming
R = sp.Matrix(R)
with open(results_dir + 'R.dat', 'wb') as f_R,\
open(results_dir + 'sampling_dict.dat', 'wb') as f_sd,\
open(results_dir + 'model_muscles.dat', 'wb') as f_mm,\
open(results_dir + 'model_coordinates.dat', 'wb') as f_mc:
pickle.dump(R, f_R)
pickle.dump(sampling_dict, f_sd)
pickle.dump(model_muscles, f_mm)
pickle.dump(model_coordinates, f_mc)
def test_printComponentsMatching(self):
model = osim.Model(
os.path.join(test_dir, "gait10dof18musc_subject01.osim"))
num_matches = model.printComponentsMatching("_r")
self.assertEqual(num_matches, 153)
# -*- coding: utf-8 -*-
"""
Created on Tue Apr 23 16:38:44 2019
@author: mhmdk
"""
import opensim as osim
import math
from scipy.optimize import minimize
#TODO: ensure correct path for your files
unloading_path = "C:/Users/mhmdk/Desktop/Co-op files/co-op semester 1/optimizers_2/all_opt/hangingHarnessModel_lim_hip.osim"
unloading_copy_path = "C:/Users/mhmdk/Desktop/Co-op files/co-op semester 1/optimizers_2/all_opt/copy_hangingHarnessModel_lim_hip.osim"
model = osim.Model(unloading_path)
model.printToXML(unloading_copy_path)
copy = osim.Model(unloading_copy_path)
frcSet = copy.getForceSet()
def external_tension(num):
stiffness = float(num)
tensionBase = frcSet.get(0)
tension = osim.PathSpring.safeDownCast(tensionBase)
tension.setStiffness(stiffness)
def test_attachGeometry_memory_management(self):
model = osim.Model()
model.getGround().attachGeometry(osim.Sphere(1.5))
# 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. #
# ----------------------------------------------------------------------- #
# This example shows how to use the StatesTrajectory to analyze a simulation
# (computing joint reaction forces).
import opensim as osim
import math
model = osim.Model()
model.setName('model')
# Create a pendulum model.
# ------------------------
# In the default pose, the system is a mass hanging 1 meter down from a hinge.
body = osim.Body('body', 1.0, osim.Vec3(0), osim.Inertia(1))
model.addComponent(body)
joint = osim.PinJoint('joint', model.getGround(), osim.Vec3(0), osim.Vec3(0),
body, osim.Vec3(0, 1.0, 0), osim.Vec3(0))
joint.updCoordinate().setName('q')
model.addComponent(joint)
# This reporter will collect states throughout the simulation at intervals of
# 0.05 seconds.
#initilization
model_file, ik_data, id_data, u, a, exp_data = import_from_storage(parentDir)
write_to_storage(
'../results/prediction.mot',
labels=[
'time', 'ground_force_vx', 'ground_force_vy', 'ground_force_vz',
'ground_force_px', 'ground_force_py', 'ground_force_pz',
'1_ground_force_vx', '1_ground_force_vy', '1_ground_force_vz',
'1_ground_force_px', '1_ground_force_py', '1_ground_force_pz',
'ground_torque_x', 'ground_torque_y', 'ground_torque_z',
'1_ground_torque_x', '1_ground_torque_y', '1_ground_torque_z'
],
prepare=True)
model = opensim.Model(model_file)
weight = 72.6 #kg
height = 1.8 #m
state = model.initSystem()
coordinate_set = model.updCoordinateSet()
pelvis = model.updBodySet().get('pelvis')
calcn_r = model.updBodySet().get('calcn_r')
calcn_l = model.updBodySet().get('calcn_l')
toes_r = model.updBodySet().get('toes_r')
toes_l = model.updBodySet().get('toes_l')
bodies_left = [calcn_l] * 6 + [toes_l]
bodies_right = [calcn_r] * 6 + [toes_r]
points_r = [
opensim.Vec3(0, -0.005, 0), # Heel
opensim.Vec3(0.05, -0.005, -0.025), # Heel
