def test_STOFileAdapter(self):
table = osim.TimeSeriesTable(
os.path.join(test_dir, 'subject02_grf_HiFreq.mot'))
assert table.getNumRows() == 439
assert table.getNumColumns() == 18
table = osim.TimeSeriesTable(
os.path.join(test_dir, 'std_subject01_walk1_ik.mot'))
assert table.getNumRows() == 73
assert table.getNumColumns() == 23
def test_TimeSeriesTable(self):
print
table = osim.TimeSeriesTable()
table.setColumnLabels(('col1', 'col2', 'col3', 'col4'))
assert(table.getColumnLabels() == ('col1', 'col2', 'col3', 'col4'))
# Append a row to the table.
row = osim.RowVector([1, 2, 3, 4])
table.appendRow(0.1, row)
assert table.getNumRows() == 1
assert table.getNumColumns() == 4
row0 = table.getRowAtIndex(0)
assert (row0[0] == row[0] and
row0[1] == row[1] and
row0[2] == row[2] and
row0[3] == row[3])
# Append another row to the table.
row[0] *= 2
row[1] *= 2
row[2] *= 2
row[3] *= 2
table.appendRow(0.2, row)
assert table.getNumRows() == 2
assert table.getNumColumns() == 4
row1 = table.getRow(0.2)
assert (row1[0] == row[0] and
row1[1] == row[1] and
row1[2] == row[2] and
row1[3] == row[3])
# Append another row to the table with a timestamp
# less than the previous one. Exception expected.
try:
table.appendRow(0.15, row)
assert False
except RuntimeError:
pass
def test_GCVSplineSet(self):
splineset = osim.GCVSplineSet(
os.path.join(test_dir, 'std_subject01_walk1_ik.mot'))
splineset = osim.GCVSplineSet(
osim.TimeSeriesTable(
os.path.join(test_dir, 'std_subject01_walk1_ik.mot')), [], 5,
0)
def test_connecting_and_iterate_inputs(self):
m = osim.Model()
b = osim.Body('b1', 2.0, osim.Vec3(1, 0, 0), osim.Inertia(1))
j = osim.PinJoint('pin', m.getGround(), b)
# Source.
source = osim.TableSource()
source.setName("source")
table = osim.TimeSeriesTable()
table.setColumnLabels(('col1', 'col2', 'col3', 'col4'))
row = osim.RowVector([1, 2, 3, 4])
table.appendRow(0.0, row)
row = osim.RowVector([2, 3, 4, 5])
table.appendRow(1.0, row)
source.setTable(table)
# Reporter.
rep = osim.ConsoleReporter()
rep.setName("rep")
m.addBody(b)
m.addJoint(j)
m.addComponent(source)
m.addComponent(rep)
# Connect.
# There are multiple ways to perform the connection, especially
# for reporters.
coord = j.get_coordinates(0)
rep.updInput('inputs').connect(coord.getOutput('value'))
rep.connectInput_inputs(coord.getOutput('speed'), 'spd')
rep.connectInput_inputs(source.getOutput('column').getChannel('col1'))
rep.addToReport(
source.getOutput('column').getChannel('col2'), 'second_col')
s = m.initSystem()
# Access and iterate through AbstractInputs, using names.
expectedLabels = [
'/model/jointset/pin/pin_coord_0|value', 'spd',
'/model/source|column:col1', 'second_col'
]
i = 0
for name in rep.getInputNames():
# Actually, there is only one input, which we connected to 4
# channels.
assert rep.getInput(name).getNumConnectees() == 4
for j in range(4):
assert (rep.getInput(name).getLabel(j) == expectedLabels[j])
i += 1
# Access concrete Input.
# Input value is column 2 at time 0.
assert (osim.InputDouble.safeDownCast(rep.getInput('inputs')).getValue(
s, 3) == 2.0)
def test_vectorview_typemaps(self):
# Use a TimeSeriesTable to obtain VectorViews.
table = osim.TimeSeriesTable()
table.setColumnLabels(['a'])
table.appendRow(0.0, osim.RowVector([1.5]))
table.appendRow(1.0, osim.RowVector([2.5]))
column = table.getDependentColumn('a').to_numpy()
assert len(column) == 2
assert column[0] == 1.5
assert column[1] == 2.5
row = table.getRowAtIndex(0).to_numpy()
assert len(row) == 1
assert row[0] == 1.5
row = table.getRowAtIndex(1).to_numpy()
assert len(row) == 1
assert row[0] == 2.5
def create_ground_reaction_file(self, model, solution, filepath):
reaction = osim.analyzeSpatialVec(model, solution,
['/jointset/foot_ground_r\|reaction_on_child'])
reaction = reaction.flatten(['_MX', '_MY', '_MZ', '_FX', '_FY', '_FZ'])
prefix = '/jointset/foot_ground_r|reaction_on_child'
FX = reaction.getDependentColumn(prefix + '_FX')
FY = reaction.getDependentColumn(prefix + '_FY')
FZ = reaction.getDependentColumn(prefix + '_FZ')
MX = reaction.getDependentColumn(prefix + '_MX')
MY = reaction.getDependentColumn(prefix + '_MY')
MZ = reaction.getDependentColumn(prefix + '_MZ')
# X coordinate of the center of pressure.
x_cop = np.empty(reaction.getNumRows())
z_cop = np.empty(reaction.getNumRows())
TY = np.empty(reaction.getNumRows())
state = model.initSystem()
mass = model.getTotalMass(state)
gravity_accel = model.get_gravity()
mass_center = model.calcMassCenterPosition(state)
print(f'com = {mass_center[0], mass_center[1], mass_center[2]}')
print(f'weight = {mass * abs(gravity_accel[1])}')
offset = model.getBodySet().get('calcn_r').getPositionInGround(state)
x_offset = offset.get(0)
z_offset = offset.get(2)
print(f'x_offset = {x_offset}; z_offset = {z_offset}')
for i in range(reaction.getNumRows()):
x = MZ[i] / FY[i] + x_offset
z = -MX[i] / FY[i] + z_offset
x_cop[i] = x
z_cop[i] = z
TY[i] = MY[i] - (x - x_offset) * FZ[i] + (z - z_offset) * FX[i]
zeroMatrix = osim.Matrix(reaction.getNumRows(), 1, 0)
zero = osim.Vector(reaction.getNumRows(), 0)
external_loads = osim.TimeSeriesTable(reaction.getIndependentColumn(),
zeroMatrix, ['zero'])
external_loads.appendColumn('ground_force_vx', FX)
external_loads.appendColumn('ground_force_vy', FY)
external_loads.appendColumn('ground_force_vz', FZ)
external_loads.appendColumn('ground_force_px', osim.Vector(x_cop))
external_loads.appendColumn('ground_force_py', zero)
external_loads.appendColumn('ground_force_pz', osim.Vector(z_cop))
external_loads.appendColumn('ground_torque_x', zero)
external_loads.appendColumn('ground_torque_y', osim.Vector(TY))
external_loads.appendColumn('ground_torque_z', zero)
osim.STOFileAdapter.write(external_loads, filepath)
def test_TableSourceReporter(self):
m = osim.Model()
# Source.
source = osim.TableSource()
source.setName("source")
table = osim.TimeSeriesTable()
table.setColumnLabels(('col1', 'col2', 'col3', 'col4'))
row = osim.RowVector([1, 2, 3, 4])
table.appendRow(0.0, row)
row = osim.RowVector([2, 3, 4, 5])
table.appendRow(1.0, row)
source.setTable(table)
# Reporter.
c_rep = osim.ConsoleReporter()
c_rep.setName("c_rep")
t_rep = osim.TableReporter()
t_rep.setName("t_rep")
# Add.
m.addComponent(source)
m.addComponent(c_rep)
m.addComponent(t_rep)
# Connect.
c_rep.updInput("inputs").connect(
source.getOutput("column").getChannel("col1"))
t_rep.updInput("inputs").connect(
source.getOutput("column").getChannel("col2"))
# Realize.
s = m.initSystem()
s.setTime(0.5)
m.realizeReport(s)
# Test.
# This value is the average of col2 (2.0 and 3.0).
assert t_rep.getReport().getRowAtIndex(0)[0] == 2.5
def calc_net_generalized_forces(model, motion):
model.initSystem()
net_joint_moments = None
import tempfile
with tempfile.TemporaryDirectory() as tmpdirname:
id_tool = osim.InverseDynamicsTool()
modelID = osim.Model(model)
id_tool.setModel(modelID)
if type(motion) == osim.Storage:
id_tool.setLowpassCutoffFrequency(6)
storage = motion
else:
table = motion.exportToStatesTable()
labels = list(table.getColumnLabels())
import re
for ilabel in range(len(labels)):
labels[ilabel] = labels[ilabel].replace('/value', '')
labels[ilabel] = re.sub('/jointset/(.*?)/', '', labels[ilabel])
table.setColumnLabels(labels)
storage = osim.convertTableToStorage(table)
# TODO: There's a bug in converting column labels in
# convertTableToStorage().
stolabels = osim.ArrayStr()
stolabels.append('time')
for label in labels:
stolabels.append(label)
storage.setColumnLabels(stolabels)
id_tool.setCoordinateValues(storage)
# id_tool.setExternalLoadsFileName(extloads_fpath)
excludedForces = osim.ArrayStr()
excludedForces.append('ACTUATORS')
id_tool.setExcludedForces(excludedForces)
id_result = 'joint_moment_breakdown_residuals.sto'
id_tool.setResultsDir(tmpdirname)
id_tool.setOutputGenForceFileName(id_result)
# TODO: Remove muscles from the model?
id_tool.run()
net_joint_moments = osim.TimeSeriesTable(
os.path.join(tmpdirname, id_result))
return net_joint_moments
def to_sto(self, filename, metadata=None):
"""
Write a sto file from a Analogs3dOsim
Parameters
----------
filename : string
path of the file to write
metadata : dict, optional
dict with optional metadata to add in the output file
"""
filename = Path(filename)
# Make sure the directory exists, otherwise create it
if not filename.parents[0].is_dir():
filename.parents[0].mkdir()
table = osim.TimeSeriesTable()
# set metadata
table.setColumnLabels(self.get_labels)
if metadata:
for key, value in metadata.items():
table.addTableMetaDataString(key, str(value))
if 'nColumns' not in metadata:
table.addTableMetaDataString('nColumns', str(self.shape[1]))
else:
table.addTableMetaDataString('nColumns', str(self.shape[1]))
table.addTableMetaDataString('nRows', str(self.shape[-1]))
time_vector = np.arange(start=0,
stop=1 / self.get_rate * self.shape[2],
step=1 / self.get_rate)
for iframe in range(self.shape[-1]):
a = self.get_frame(iframe)
row = osim.RowVector(a.ravel().tolist())
table.appendRow(time_vector[iframe], row)
adapter = osim.STOFileAdapter()
adapter.write(table, str(filename))
def get_convergence_results(root_dir, result):
metadata = result.convergence_metadata()
x = list()
costs = list()
for md in metadata:
solution_fpath = os.path.join(root_dir, 'results', 'convergence',
md['solution_file'])
if not os.path.exists(solution_fpath):
print(f"Warning: solution {solution_fpath} does not exist.")
continue
table = osim.TimeSeriesTable(solution_fpath)
num_mesh_intervals = md['num_mesh_intervals']
# All problems use Hermite-Simpson transcription.
if table.getNumRows() != 2 * num_mesh_intervals + 1:
print("Warning: inconsistent number of mesh intervals "
f"({(table.getNumRows() - 1) / 2} vs {num_mesh_intervals}).")
num_mesh_intervals = (table.getNumRows() - 1) / 2
x.append(num_mesh_intervals)
costs.append(float(table.getTableMetaDataAsString('objective')))
# Normalize costs.
if costs:
costs = np.array(costs) / costs[-1]
return x, costs
def solveMocoInverseWithEMG():
# This initial block of code is identical to the code above.
inverse = osim.MocoInverse()
modelProcessor = osim.ModelProcessor('subject_walk_armless.osim')
modelProcessor.append(osim.ModOpAddExternalLoads('grf_walk.xml'))
modelProcessor.append(osim.ModOpIgnoreTendonCompliance())
modelProcessor.append(osim.ModOpReplaceMusclesWithDeGrooteFregly2016())
modelProcessor.append(osim.ModOpIgnorePassiveFiberForcesDGF())
modelProcessor.append(osim.ModOpScaleActiveFiberForceCurveWidthDGF(1.5))
modelProcessor.append(osim.ModOpAddReserves(1.0))
inverse.setModel(modelProcessor)
inverse.setKinematics(osim.TableProcessor('coordinates.sto'))
inverse.set_initial_time(0.81)
inverse.set_final_time(1.79)
inverse.set_mesh_interval(0.02)
inverse.set_kinematics_allow_extra_columns(True)
study = inverse.initialize()
problem = study.updProblem()
# Add electromyography tracking.
emgTracking = osim.MocoControlTrackingGoal('emg_tracking')
emgTracking.setWeight(50.0)
# Each column in electromyography.sto is normalized so the maximum value in
# each column is 1.0.
controlsRef = osim.TimeSeriesTable('electromyography.sto')
# Scale the tracked muscle activity based on peak levels from
# "Gait Analysis: Normal and Pathological Function" by
# Perry and Burnfield, 2010 (digitized by Carmichael Ong).
soleus = controlsRef.updDependentColumn('soleus')
gasmed = controlsRef.updDependentColumn('gastrocnemius')
tibant = controlsRef.updDependentColumn('tibialis_anterior')
for t in range(0, controlsRef.getNumRows()):
soleus[t] = 0.77 * soleus[t]
gasmed[t] = 0.87 * gasmed[t]
tibant[t] = 0.37 * tibant[t]
emgTracking.setReference(osim.TableProcessor(controlsRef))
# Associate actuators in the model with columns in electromyography.sto.
emgTracking.setReferenceLabel('/forceset/soleus_r', 'soleus')
emgTracking.setReferenceLabel('/forceset/gasmed_r', 'gastrocnemius')
emgTracking.setReferenceLabel('/forceset/gaslat_r', 'gastrocnemius')
emgTracking.setReferenceLabel('/forceset/tibant_r', 'tibialis_anterior')
problem.addGoal(emgTracking)
# Solve the problem and write the solution to a Storage file.
solution = study.solve()
solution.write('example3DWalking_MocoInverseWithEMG_solution.sto')
# Write the reference data in a way that's easy to compare to the solution.
controlsRef.removeColumn('medial_hamstrings')
controlsRef.removeColumn('biceps_femoris')
controlsRef.removeColumn('vastus_lateralis')
controlsRef.removeColumn('vastus_medius')
controlsRef.removeColumn('rectus_femoris')
controlsRef.removeColumn('gluteus_maximus')
controlsRef.removeColumn('gluteus_medius')
controlsRef.setColumnLabels(
['/forceset/soleus_r', '/forceset/gasmed_r', '/forceset/tibant_r'])
controlsRef.appendColumn('/forceset/gaslat_r', gasmed)
osim.STOFileAdapter.write(controlsRef, 'controls_reference.sto')
# Generate a report comparing MocoInverse solutions without and with EMG
# tracking.
model = modelProcessor.process()
output = 'example3DWalking_MocoInverseWithEMG_report.pdf'
ref_files = [
'example3DWalking_MocoInverseWithEMG_solution.sto',
'controls_reference.sto'
]
report = osim.report.Report(model,
'example3DWalking_MocoInverse_solution.sto',
output=output,
bilateral=True,
ref_files=ref_files)
# The PDF is saved to the working directory.
report.generate()
def plot_passive_joint_moments(model, coord_table,
coord_paths, muscle_paths=None):
model.initSystem()
# for muscle in model.getMuscleList():
# muscle.set_ignore_tendon_compliance(True)
# model.initSystem()
num_coords = len(coord_paths)
if not muscle_paths:
muscle_paths = list()
for muscle in model.getMuscleList():
muscle_paths.append(muscle.getAbsolutePathString())
num_muscles = len(muscle_paths)
labels = list(coord_table.getColumnLabels())
import re
for ilabel in range(len(labels)):
labels[ilabel] = labels[ilabel].replace('/value', '')
labels[ilabel] = re.sub('/jointset/(.*?)/', '', labels[ilabel])
coord_table.setColumnLabels(labels)
storage = osim.convertTableToStorage(coord_table)
# TODO: There's a bug in converting column labels in
# convertTableToStorage().
stolabels = osim.ArrayStr()
stolabels.append('time')
for label in labels:
stolabels.append(label)
storage.setColumnLabels(stolabels)
net_joint_moments = None
import tempfile
with tempfile.TemporaryDirectory() as tmpdirname:
id_tool = osim.InverseDynamicsTool()
modelID = osim.Model(model)
id_tool.setModel(modelID)
id_tool.setCoordinateValues(storage)
id_tool.setLowpassCutoffFrequency(6)
# id_tool.setExternalLoadsFileName(extloads_fpath)
excludedForces = osim.ArrayStr()
excludedForces.append('ACTUATORS')
id_tool.setExcludedForces(excludedForces)
id_result = 'joint_moment_breakdown_residuals.sto'
id_tool.setResultsDir(tmpdirname)
id_tool.setOutputGenForceFileName(id_result)
# TODO: Remove muscles from the model?
id_tool.run()
net_joint_moments = osim.TimeSeriesTable(
os.path.join(tmpdirname, id_result))
time = coord_table.getIndependentColumn()
states_traj = osim.StatesTrajectory.createFromStatesStorage(model, storage,
True, True,
True)
# TODO for models without activation dynamics, we must prescribeControlsToModel().
fig = pl.figure(figsize=(8.5, 11))
tendon_forces = np.empty((len(time), num_muscles))
for imusc, muscle_path in enumerate(muscle_paths):
muscle = model.getComponent(muscle_path)
for itime in range(len(time)):
state = states_traj.get(itime)
state.updU().setToZero()
muscle.setActivation(state, 0.01)
model.realizeVelocity(state)
muscle.computeEquilibrium(state)
model.realizeDynamics(state)
tendon_forces[itime, imusc] = muscle.getTendonForce(state)
for icoord, coord_path in enumerate(coord_paths):
coord = model.getComponent(coord_path)
label = os.path.split(coord_path)[-1] + '_moment'
net_moment = toarray(net_joint_moments.getDependentColumn(label))
moment_arms = np.empty((len(time), num_muscles))
for imusc, muscle_path in enumerate(muscle_paths):
muscle = model.getComponent(muscle_path)
for itime in range(len(time)):
state = states_traj.get(itime)
moment_arms[itime, imusc] = \
muscle.computeMomentArm(state, coord)
ax = fig.add_subplot(num_coords, 1, icoord + 1)
# net_integ = np.trapz(np.abs(net_moment), x=time)
sum_actuators_shown = np.zeros_like(time)
for imusc, muscle_path in enumerate(muscle_paths):
if np.any(moment_arms[:, imusc]) > 0.00001:
this_moment = tendon_forces[:, imusc] * moment_arms[:, imusc]
mom_integ = np.trapz(np.abs(this_moment), time)
if mom_integ > 0.5:
ax.plot(time, this_moment, label=muscle_path)
sum_actuators_shown += this_moment
ax.plot(time, sum_actuators_shown,
label='sum actuators shown', color='gray', linewidth=2)
time_net = np.array(net_joint_moments.getIndependentColumn())
filter = (time_net > time[0]) & (time_net < time[-1])
# ax.plot(time_net[filter], net_moment[filter],
# label='net', color='black', linewidth=2)
ax.set_title(coord_path)
ax.set_ylabel('moment (N-m)')
ax.legend(frameon=False, bbox_to_anchor=(1, 1),
loc='upper left', ncol=2)
ax.tick_params(axis='both')
ax.set_xlim([time[0], time[-1]])
ax.set_xlabel('time (% gait cycle)')
fig.tight_layout()
return fig
# Conduct an analysis using MuscleAnalysis and ProbeReporter.
# Create an AnalyzeTool setup file.
analyze = osim.AnalyzeTool()
analyze.setName("analyze")
analyze.setModelFilename("hanging_muscle.osim")
analyze.setStatesFileName("exampleHangingMuscle_states.sto")
analyze.updAnalysisSet().cloneAndAppend(osim.MuscleAnalysis())
analyze.updAnalysisSet().cloneAndAppend(osim.ProbeReporter())
analyze.updControllerSet().cloneAndAppend(
osim.PrescribedController("exampleHangingMuscle_controls.sto"))
analyze.printToXML("exampleHangingMuscle_AnalyzeTool_setup.xml")
# Run the analysis.
analyze = osim.AnalyzeTool("exampleHangingMuscle_AnalyzeTool_setup.xml")
analyze.run()
table_force = osim.TimeSeriesTable(
"analyze_MuscleAnalysis_ActiveFiberForce.sto")
table_velocity = osim.TimeSeriesTable(
"analyze_MuscleAnalysis_FiberVelocity.sto")
time = table_force.getIndependentColumn()
force = table_force.getDependentColumn("muscle").to_numpy()
velocity = table_velocity.getDependentColumn("muscle").to_numpy()
# Plot the terms of the metabolics model, and compare the metabolics model's
# mechanical work rate to the mechanical work rate computed using the
# MuscleAnalysis.
plot = False
# The following environment variable is set during automated testing.
if os.getenv('OPENSIM_USE_VISUALIZER') != '0':
try:
import pylab as pl
plot = True
def __init__(
self,
model,
trajectory_filepath,
bilateral=True,
ref_files=None,
colormap=None,
output=None,
):
self.model = model
self.model.initSystem()
self.trajectory_filepath = trajectory_filepath
self.trajectory = osim.MocoTrajectory(self.trajectory_filepath)
self.bilateral = bilateral
self.ref_files = ref_files
self.colormap = colormap
trajectory_fname = ntpath.basename(self.trajectory_filepath)
trajectory_fname = trajectory_fname.replace('.sto', '')
trajectory_fname = trajectory_fname.replace('.mot', '')
self.trajectory_fname = trajectory_fname
if output:
self.output = output
else:
self.output = trajectory_fname + '_report.pdf'
# Get any reference files provided by the user and create a list of NumPy
# arrays to use in plotting.
self.refs = list()
if ref_files != None:
for ref_file in ref_files:
filename, file_ext = os.path.splitext(ref_file)
# If the reference is a file with metadata indicating that
# rotational data is in degrees, convert to radians and write to
# a new file to be used for plotting.
# TODO: don't write the extra file permanently
if file_ext == '.sto' or file_ext == '.mot':
ref = osim.TimeSeriesTable(ref_file)
if (ref.hasTableMetaDataKey('inDegrees')
and ref.getTableMetaDataAsString('inDegrees')
== 'yes'):
simbodyEngine = self.model.getSimbodyEngine()
simbodyEngine.convertDegreesToRadians(ref)
ref_file = ref_file.replace(file_ext,
'_radians' + file_ext)
sto_adapter = osim.STOFileAdapter()
sto_adapter.write(ref, ref_file)
num_header_rows = 1
with open(ref_file) as f:
for line in f:
if not line.startswith('endheader'):
num_header_rows += 1
else:
break
this_ref = np.genfromtxt(ref_file,
names=True,
delimiter='\t',
skip_header=num_header_rows)
self.refs.append(this_ref)
# Load the colormap provided by the user. Use a default colormap ('jet')
# if not provided. Uniformly sample the colormap based on the number of
# reference data sets, plus one for the MocoTrajectory.
import matplotlib.cm as cm
if colormap is None: colormap = 'jet'
self.cmap_samples = np.linspace(0.1, 0.9, len(self.refs) + 1)
self.cmap = cm.get_cmap(colormap)
## Legend handles and labels.
# ===========================
# Create legend handles and labels that can be used to create a figure
# legend that is applicable all figures.
self.legend_handles = list()
self.legend_labels = list()
all_files = list()
if ref_files != None: all_files += ref_files
all_files.append(trajectory_filepath)
lw = 8 / len(self.cmap_samples)
if lw < 0.5: lw = 0.5
if lw > 2: lw = 2
for sample, file in zip(self.cmap_samples, all_files):
color = self.cmap(sample)
import matplotlib.lines as mlines
if bilateral:
r = mlines.Line2D([], [], ls='-', color=color, linewidth=lw)
self.legend_handles.append(r)
self.legend_labels.append(file + ' (right leg)')
l = mlines.Line2D([], [], ls='--', color=color, linewidth=lw)
self.legend_handles.append(l)
self.legend_labels.append(file + ' (left leg)')
else:
h = mlines.Line2D([], [], ls='-', color=color, linewidth=lw)
self.legend_handles.append(h)
self.legend_labels.append(file)
# Time
# -----
# Convert trajectory time vector to a plotting-friendly NumPy array.
self.time = convert(self.trajectory.getTime())
# Create a conservative set of x-tick values based on the time vector.
nexttime = math.ceil(self.time[0] * 10) / 10
nexttolast = math.floor(self.time[-1] * 10) / 10
if (nexttolast - nexttime) < 1.5:
self.timeticks = np.arange(nexttime, nexttolast, 0.2)
else:
self.timeticks = None
self.plots_per_page = 15.0
self.num_cols = 3
# Add an extra row to hold the legend and other infromation.
self.num_rows = (self.plots_per_page / 3) + 1
def test_TimeSeriesTable(self):
print
table = osim.TimeSeriesTable()
table.setColumnLabels(('col1', 'col2', 'col3', 'col4'))
assert (table.getColumnLabels() == ('col1', 'col2', 'col3', 'col4'))
# Append a row to the table.
row = osim.RowVector([1, 2, 3, 4])
table.appendRow(0.1, row)
assert table.getNumRows() == 1
assert table.getNumColumns() == 4
row0 = table.getRowAtIndex(0)
assert (row0[0] == row[0] and row0[1] == row[1] and row0[2] == row[2]
and row0[3] == row[3])
# Append another row to the table.
row[0] *= 2
row[1] *= 2
row[2] *= 2
row[3] *= 2
table.appendRow(0.2, row)
assert table.getNumRows() == 2
assert table.getNumColumns() == 4
row1 = table.getRow(0.2)
assert (row1[0] == row[0] and row1[1] == row[1] and row1[2] == row[2]
and row1[3] == row[3])
# Append another row to the table with a timestamp
# less than the previous one. Exception expected.
try:
table.appendRow(0.15, row)
assert False
except RuntimeError:
pass
# Test pack-ing of columns of TimeSeriesTable.
table = osim.TimeSeriesTable()
table.setColumnLabels(
('col0_x', 'col0_y', 'col0_z', 'col1_x', 'col1_y', 'col1_z',
'col2_x', 'col2_y', 'col2_z', 'col3_x', 'col3_y', 'col3_z'))
row = osim.RowVector([1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1])
table.appendRow(1, row)
row = osim.RowVector([2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2])
table.appendRow(2, row)
row = osim.RowVector([3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3])
table.appendRow(3, row)
assert len(table.getColumnLabels()) == 12
assert table.getNumRows() == 3
assert table.getNumColumns() == 12
print table
avgRow = table.averageRow(1, 3)
assert avgRow.ncol() == 12
assert abs(avgRow[0] - 2) < 1e-8 #epsilon
assert abs(avgRow[11] - 2) < 1e-8 #epsilon
nearRow = table.getNearestRow(1.1)
assert nearRow.ncol() == 12
assert nearRow[0] == 1
assert nearRow[11] == 1
tableVec3 = table.packVec3(('_x', '_y', '_z'))
tableVec3.getColumnLabels() == ('col0', 'col1', 'col2', 'col3')
tableVec3.getNumRows() == 3
tableVec3.getNumColumns() == 4
print tableVec3
tableVec3 = table.packVec3()
tableVec3.getColumnLabels() == ('col0', 'col1', 'col2', 'col3')
tableVec3.getNumRows() == 3
tableVec3.getNumColumns() == 4
print tableVec3
avgRow = tableVec3.averageRow(1, 2)
assert avgRow.ncol() == 4
assert abs(avgRow[0][0] - 1.5) < 1e-8 #epsilon
assert abs(avgRow[3][2] - 1.5) < 1e-8 #epsilon
nearRow = tableVec3.getNearestRow(1.1)
assert nearRow.ncol() == 4
assert nearRow[0][0] == 1
assert nearRow[3][2] == 1
tableUnitVec3 = table.packUnitVec3()
tableUnitVec3.getColumnLabels() == ('col0', 'col1', 'col2', 'col3')
tableUnitVec3.getNumRows() == 3
tableUnitVec3.getNumColumns() == 4
print tableUnitVec3
table.setColumnLabels(
('col0.0', 'col0.1', 'col0.2', 'col0.3', 'col1.0', 'col1.1',
'col1.2', 'col1.3', 'col2.0', 'col2.1', 'col2.2', 'col2.3'))
tableQuat = table.packQuaternion()
tableQuat.getColumnLabels() == ('col0', 'col1', 'col2')
tableQuat.getNumRows() == 3
tableQuat.getNumColumns() == 3
print tableQuat
table.setColumnLabels(
('col0_0', 'col0_1', 'col0_2', 'col0_3', 'col0_4', 'col0_5',
'col1_0', 'col1_1', 'col1_2', 'col1_3', 'col1_4', 'col1_5'))
tableSVec = table.packSpatialVec()
tableSVec.getColumnLabels() == ('col0', 'col1')
tableSVec.getNumRows() == 3
tableSVec.getNumColumns() == 2
print tableSVec
def report_results(self, root_dir, args):
self.parse_args(args)
fig = plt.figure(figsize=(7.5, 3))
values = [
'/jointset/hip_r/hip_extension_r/value',
'/jointset/knee_r/knee_extension_r/value',
'/jointset/ankle_r/ankle_plantarflexion_r/value',
]
coord_signs = {
'/jointset/hip_r/hip_extension_r/value': -1.0,
'/jointset/knee_r/knee_extension_r/value': -1.0,
'/jointset/ankle_r/ankle_plantarflexion_r/value': -1.0,
}
# TODO: Should be 3 muscles, all extensors.
muscles = [
((0, 2), 'glut_max2', 'gluteus maximus'),
((1, 2), 'psoas', 'iliopsoas'),
((2, 2), 'semimem', 'hamstrings'),
((3, 2), 'vas_int', 'vasti'),
]
grid = gridspec.GridSpec(4, 3)
ax = fig.add_subplot(grid[0:4, 0:2])
import cv2
# Convert BGR color ordering to RGB.
image = cv2.imread(os.path.join(root_dir, 'squat_to_stand_visualization/'
'squat_to_stand_visualization.png'))[...,::-1]
ax.imshow(image)
plt.axis('off')
muscle_axes = []
for im, muscle in enumerate(muscles):
ax = fig.add_subplot(grid[muscle[0][0], muscle[0][1]])
# ax.set_title('%s activation' % muscle[1])
title = f' {muscle[2]}'
plt.text(0.5, 0.9, title,
horizontalalignment='center',
transform=ax.transAxes
)
ax.set_yticks([0, 1])
ax.set_ylim([0, 1])
if muscle[0][0] == 3:
ax.set_xlabel('time (s)')
else:
ax.set_xticklabels([])
if muscle[0][1] == 2:
ax.set_ylabel('activation')
utilities.publication_spines(ax)
muscle_axes += [ax]
def plot_solution(sol, label, linestyle='-', color=None):
time = sol.getTimeMat()
for im, muscle in enumerate(muscles):
ax = muscle_axes[im]
ax.plot(time,
sol.getStateMat(
'/forceset/%s_r/activation' % muscle[1]),
linestyle=linestyle, color=color, clip_on=False,
label=label)
ax.autoscale(enable=True, axis='x', tight=True)
predict_solution = osim.MocoTrajectory(self.predict_solution_file % root_dir)
predict_assisted_solution = osim.MocoTrajectory(
self.predict_assisted_solution_file % root_dir)
stiffness = predict_assisted_solution.getParameter('stiffness')
model = self.muscle_driven_model(root_dir)
assisted_model = self.assisted_model(root_dir)
assisted_model.updComponent('forceset/spring').setStiffness(stiffness)
print(f'Stiffness: {stiffness}')
with open(os.path.join(root_dir, 'results/squat_to_stand_stiffness.txt'), 'w') as f:
f.write(f'{stiffness:.0f}')
plot_solution(predict_solution, 'unassisted', color='gray')
plot_solution(predict_assisted_solution, 'assisted')
kinematics_rms = predict_solution.compareContinuousVariablesRMSPattern(
predict_assisted_solution, 'states', '/jointset.*value$')
kinematics_rms_deg = np.rad2deg(kinematics_rms)
print('RMS difference in joint angles between conditions (degrees): '
f'{kinematics_rms_deg}')
with open(os.path.join(root_dir, 'results/squat_to_stand_kinematics_rms.txt'), 'w') as f:
f.write(f'{kinematics_rms_deg:.1f}')
fig.tight_layout() # w_pad=0.2)
muscle_axes[0].legend(frameon=False, handlelength=1,
# bbox_to_anchor=(-1.0, 0.5),
# loc='center',
)
fig.savefig(os.path.join(root_dir, 'figures/squat_to_stand.png'), dpi=600)
coords = ['/jointset/hip_r/hip_extension_r',
'/jointset/knee_r/knee_extension_r',
'/jointset/ankle_r/ankle_plantarflexion_r']
fig = self.plot_joint_moment_breakdown(model,
predict_solution, coords)
fig.savefig(os.path.join(root_dir,
'figures/squat_to_stand_joint_moment_breakdown.png'),
dpi=600)
fig = self.plot_joint_moment_breakdown(assisted_model,
predict_assisted_solution,
coords,
knee_stiffness=stiffness
)
fig.savefig(os.path.join(root_dir, 'figures/squat_to_stand_assisted_'
'joint_moment_breakdown.png'),
dpi=600)
sol_predict_table = osim.TimeSeriesTable(self.predict_solution_file % root_dir)
sol_predict_duration = sol_predict_table.getTableMetaDataString('solver_duration')
sol_predict_duration = float(sol_predict_duration) / 60.0
print('prediction duration ', sol_predict_duration)
with open(os.path.join(root_dir, 'results/'
'squat_to_stand_predict_duration.txt'), 'w') as f:
f.write(f'{sol_predict_duration:.1f}')
sol_predict_assisted_table = osim.TimeSeriesTable(
self.predict_assisted_solution_file % root_dir)
sol_predict_assisted_duration = sol_predict_assisted_table.getTableMetaDataString(
'solver_duration')
sol_predict_assisted_duration = float(
sol_predict_assisted_duration) / 60.0
print('prediction assisted duration ', sol_predict_assisted_duration)
with open(os.path.join(root_dir, 'results/'
'squat_to_stand_predict_assisted_duration.txt'), 'w') as f:
f.write(f'{sol_predict_assisted_duration:.1f}')
# table = osim.analyze(model,
# osim.MocoTrajectory(self.predict_solution_file),
# ['.*normalized_fiber_length'])
# osim.STOFileAdapter.write(table,
# 'squat_to_stand_norm_fiber_length.sto')
report = osim.report.Report(assisted_model,
self.predict_assisted_solution_file % root_dir,
ref_files=[self.predict_solution_file % root_dir])
report.generate()
self.create_ground_reaction_file(model, predict_solution,
os.path.join(root_dir,
'results/squat_to_stand_ground_reaction.mot'))
self.create_ground_reaction_file(assisted_model,
predict_assisted_solution,
os.path.join(root_dir,
'results/squat_to_stand_assisted_ground_reaction.mot'))
def test_C3DFileAdapter(self):
try:
adapter = osim.C3DFileAdapter()
except AttributeError:
# C3D support not available. OpenSim was not compiled with ezc3d.
return
tables = adapter.read(os.path.join(test_dir, 'walking2.c3d'))
forces = adapter.getForcesTable(tables)
markers = adapter.getMarkersTable(tables)
assert markers.getNumRows() == 1249
assert markers.getNumColumns() == 44
assert forces.getNumRows() == 9992
assert forces.getNumColumns() == 6
adapter.setLocationForForceExpression(1)
tables2 = adapter.read(os.path.join(test_dir, 'walking5.c3d'))
# Marker data read from C3D.
markers = adapter.getMarkersTable(tables2)
assert markers.getNumRows() == 1103
assert markers.getNumColumns() == 40
assert markers.getTableMetaDataString('DataRate') == '250.000000'
assert markers.getTableMetaDataString('Units') == 'mm'
# Flatten marker data.
markersFlat = markers.flatten()
assert markersFlat.getNumRows() == 1103
assert markersFlat.getNumColumns() == 40 * 3
# Make sure flattenned marker data is writable/readable to/from file.
markersFilename = 'markers.sto'
stoAdapter = osim.STOFileAdapter()
stoAdapter.write(markersFlat, markersFilename)
markersDouble = osim.TimeSeriesTable(markersFilename)
assert markersDouble.getNumRows() == 1103
assert markersDouble.getNumColumns() == 40 * 3
# Forces data read from C3d.
forces = adapter.getForcesTable(tables2)
assert forces.getNumRows() == 8824
assert forces.getNumColumns() == 6
assert forces.getTableMetaDataString('DataRate') == '2000.000000'
assert forces.getTableMetaDataVectorUnsigned('Types') == (2, 2)
fpCalMats = forces.getTableMetaDataVectorMatrix("CalibrationMatrices")
assert len(fpCalMats) == 2
assert fpCalMats[0].nrow() == 6
assert fpCalMats[0].ncol() == 6
assert fpCalMats[1].nrow() == 6
assert fpCalMats[1].ncol() == 6
fpCorners = forces.getTableMetaDataVectorMatrix("Corners")
assert len(fpCorners) == 2
assert fpCorners[0].nrow() == 3
assert fpCorners[0].ncol() == 4
assert fpCorners[1].nrow() == 3
assert fpCorners[1].ncol() == 4
fpOrigins = forces.getTableMetaDataVectorMatrix("Origins")
assert len(fpOrigins) == 2
assert fpOrigins[0].nrow() == 3
assert fpOrigins[0].ncol() == 1
assert fpOrigins[1].nrow() == 3
assert fpOrigins[1].ncol() == 1
assert forces.getDependentsMetaDataString('units') == ('N', 'mm',
'Nmm', 'N',
'mm', 'Nmm')
# Flatten forces data.
forcesFlat = forces.flatten()
assert forcesFlat.getNumRows() == 8824
assert forcesFlat.getNumColumns() == 6 * 3
# Make sure flattenned forces data is writable/readable to/from file.
forcesFilename = 'forces.sto'
stoAdapter.write(forcesFlat, forcesFilename)
forcesDouble = osim.TimeSeriesTable(forcesFilename)
assert forcesDouble.getNumRows() == 8824
assert forcesDouble.getNumColumns() == 6 * 3
# Clean up.
os.remove(markersFilename)
os.remove(forcesFilename)
def run_tracking_problem(self, root_dir, config):
modelProcessor = self.create_model_processor(root_dir,
for_inverse=False,
config=config)
model = modelProcessor.process()
model.initSystem()
# Count the number of Force objects in the model. We'll use this to
# normalize the control effort cost.
numForces = 0
for actu in model.getComponentsList():
if (actu.getConcreteClassName().endswith('Muscle') or
actu.getConcreteClassName().endswith('Actuator')):
numForces += 1
# Construct the base tracking problem
# -----------------------------------
track = osim.MocoTrack()
track.setName('tracking_walking')
track.setModel(modelProcessor)
if self.coordinate_tracking:
tableProcessor = osim.TableProcessor(os.path.join(root_dir,
'resources/Rajagopal2016/coordinates.mot'))
tableProcessor.append(osim.TabOpLowPassFilter(6))
tableProcessor.append(osim.TabOpUseAbsoluteStateNames())
track.setStatesReference(tableProcessor)
track.set_states_global_tracking_weight(
config.tracking_weight / (2 * model.getNumCoordinates()))
# Don't track some pelvis coordinates to avoid poor walking motion
# solutions.
stateWeights = osim.MocoWeightSet()
weightList = list()
weightList.append(('/jointset/ground_pelvis/pelvis_ty/value', 0))
weightList.append(('/jointset/ground_pelvis/pelvis_tz/value', 0))
weightList.append(('/jointset/ground_pelvis/pelvis_list/value', 0))
weightList.append(('/jointset/ground_pelvis/pelvis_tilt/value', 0))
weightList.append(('/jointset/ground_pelvis/pelvis_rotation/value', 0))
weightList.append(('/jointset/hip_r/hip_rotation_r/value', 0))
# weightList.append(('/jointset/hip_r/hip_adduction_r/value', 0))
weightList.append(('/jointset/hip_l/hip_rotation_l/value', 0))
# weightList.append(('/jointset/hip_l/hip_adduction_l/value', 0))
# weightList.append(('/jointset/ankle_r/ankle_angle_r/value', 10))
# weightList.append(('/jointset/ankle_l/ankle_angle_l/value', 10))
for weight in weightList:
stateWeights.cloneAndAppend(osim.MocoWeight(weight[0], weight[1]))
track.set_states_weight_set(stateWeights)
track.set_apply_tracked_states_to_guess(True)
# track.set_scale_state_weights_with_range(True);
else:
track.setMarkersReferenceFromTRC(os.path.join(root_dir,
'resources/Rajagopal2016/markers.trc'))
track.set_markers_global_tracking_weight(
config.tracking_weight / (2 * model.getNumMarkers()))
iktool = osim.InverseKinematicsTool(os.path.join(root_dir,
'resources/Rajagopal2016/ik_setup_walk.xml'))
iktasks = iktool.getIKTaskSet()
markerWeights = osim.MocoWeightSet()
for marker in model.getComponentsList():
if not type(marker) is osim.Marker: continue
for i in np.arange(iktasks.getSize()):
iktask = iktasks.get(int(i))
if iktask.getName() == marker.getName():
weight = osim.MocoWeight(iktask.getName(),
iktask.getWeight())
markerWeights.cloneAndAppend(weight)
track.set_markers_weight_set(markerWeights)
track.set_allow_unused_references(True)
track.set_track_reference_position_derivatives(True)
track.set_control_effort_weight(config.effort_weight / numForces)
track.set_initial_time(self.initial_time)
track.set_final_time(self.half_time)
track.set_mesh_interval(self.mesh_interval)
# Customize the base tracking problem
# -----------------------------------
study = track.initialize()
problem = study.updProblem()
problem.setTimeBounds(self.initial_time,
[self.half_time-0.2, self.half_time+0.2])
# Set the initial values for the lumbar and pelvis coordinates that
# produce "normal" walking motions.
problem.setStateInfo('/jointset/back/lumbar_extension/value', [], -0.12)
problem.setStateInfo('/jointset/back/lumbar_bending/value', [], 0)
problem.setStateInfo('/jointset/back/lumbar_rotation/value', [], 0.04)
problem.setStateInfo('/jointset/ground_pelvis/pelvis_tx/value', [], 0.446)
problem.setStateInfo('/jointset/ground_pelvis/pelvis_tilt/value', [], 0)
problem.setStateInfo('/jointset/ground_pelvis/pelvis_list/value', [], 0)
problem.setStateInfo('/jointset/ground_pelvis/pelvis_rotation/value', [], 0)
# Update the control effort goal to a cost of transport type cost.
effort = osim.MocoControlGoal().safeDownCast(
problem.updGoal('control_effort'))
effort.setDivideByDisplacement(True)
# Weight residual and reserve actuators low in the effort cost since
# they are already weak.
if config.effort_weight:
for actu in model.getComponentsList():
actuName = actu.getName()
if actu.getConcreteClassName().endswith('Actuator'):
effort.setWeightForControl(actu.getAbsolutePathString(),
0.001)
for muscle in ['psoas', 'iliacus']:
for side in ['l', 'r']:
effort.setWeightForControl(
'/forceset/%s_%s' % (muscle, side), 0.25)
speedGoal = osim.MocoAverageSpeedGoal('speed')
speedGoal.set_desired_average_speed(1.235)
problem.addGoal(speedGoal)
# MocoFrameDistanceConstraint
# ---------------------------
if self.coordinate_tracking:
distanceConstraint = osim.MocoFrameDistanceConstraint()
distanceConstraint.setName('distance_constraint')
# Step width is 0.13 * leg_length
# distance = 0.10 # TODO Should be closer to 0.11.
# Donelan JM, Kram R, Kuo AD. Mechanical and metabolic determinants
# of the preferred step width in human walking.
# Proc Biol Sci. 2001;268(1480):1985–1992.
# doi:10.1098/rspb.2001.1761
# https://www.ncbi.nlm.nih.gov/pmc/articles/PMC1088839/
distanceConstraint.addFramePair(
osim.MocoFrameDistanceConstraintPair(
'/bodyset/calcn_l', '/bodyset/calcn_r', 0.09, np.inf))
distanceConstraint.addFramePair(
osim.MocoFrameDistanceConstraintPair(
'/bodyset/toes_l', '/bodyset/toes_r', 0.06, np.inf))
# distanceConstraint.addFramePair(
# osim.MocoFrameDistanceConstraintPair(
# '/bodyset/calcn_l', '/bodyset/toes_r', distance, np.inf))
# distanceConstraint.addFramePair(
# osim.MocoFrameDistanceConstraintPair(
# '/bodyset/toes_l', '/bodyset/calcn_r', distance, np.inf))
distanceConstraint.setProjection("vector")
distanceConstraint.setProjectionVector(osim.Vec3(0, 0, 1))
problem.addPathConstraint(distanceConstraint)
# Symmetry constraints
# --------------------
statesRef = osim.TimeSeriesTable('tracking_walking_tracked_states.sto')
initIndex = statesRef.getNearestRowIndexForTime(self.initial_time)
symmetry = osim.MocoPeriodicityGoal('symmetry')
# Symmetric coordinate values (except for pelvis_tx) and speeds.
for coord in model.getComponentsList():
if not type(coord) is osim.Coordinate: continue
coordName = coord.getName()
coordValue = coord.getStateVariableNames().get(0)
coordSpeed = coord.getStateVariableNames().get(1)
if coordName.endswith('_r'):
symmetry.addStatePair(osim.MocoPeriodicityGoalPair(
coordValue, coordValue.replace('_r/', '_l/')))
symmetry.addStatePair(osim.MocoPeriodicityGoalPair(
coordSpeed, coordSpeed.replace('_r/', '_l/')))
elif coordName.endswith('_l'):
symmetry.addStatePair(osim.MocoPeriodicityGoalPair(
coordValue, coordValue.replace('_l/', '_r/')))
symmetry.addStatePair(osim.MocoPeriodicityGoalPair(
coordSpeed, coordSpeed.replace('_l/', '_r/')))
elif (coordName.endswith('_bending') or
coordName.endswith('_rotation') or
coordName.endswith('_tz') or
coordName.endswith('_list')):
# This does not include hip rotation,
# because that ends with _l or _r.
symmetry.addStatePair(osim.MocoPeriodicityGoalPair(
coordValue))
symmetry.addNegatedStatePair(osim.MocoPeriodicityGoalPair(
coordSpeed))
elif not coordName.endswith('_tx'):
symmetry.addStatePair(osim.MocoPeriodicityGoalPair(
coordValue))
symmetry.addStatePair(osim.MocoPeriodicityGoalPair(
coordSpeed))
symmetry.addStatePair(osim.MocoPeriodicityGoalPair(
'/jointset/ground_pelvis/pelvis_tx/speed'))
# Symmetric activations.
for actu in model.getComponentsList():
if (not actu.getConcreteClassName().endswith('Muscle') and
not actu.getConcreteClassName().endswith('Actuator')): continue
if actu.getName().endswith('_r'):
symmetry.addStatePair(osim.MocoPeriodicityGoalPair(
actu.getStateVariableNames().get(0),
actu.getStateVariableNames().get(0).replace('_r/', '_l/')))
elif actu.getName().endswith('_l'):
symmetry.addStatePair(osim.MocoPeriodicityGoalPair(
actu.getStateVariableNames().get(0),
actu.getStateVariableNames().get(0).replace('_l/', '_r/')))
elif (actu.getName().endswith('_bending') or
actu.getName().endswith('_rotation') or
actu.getName().endswith('_tz') or
actu.getName().endswith('_list')):
symmetry.addNegatedStatePair(osim.MocoPeriodicityGoalPair(
actu.getStateVariableNames().get(0),
actu.getStateVariableNames().get(0)))
else:
symmetry.addStatePair(osim.MocoPeriodicityGoalPair(
actu.getStateVariableNames().get(0),
actu.getStateVariableNames().get(0)))
problem.addGoal(symmetry)
# Contact tracking
# ----------------
forceNamesRightFoot = ['forceset/contactSphereHeel_r',
'forceset/contactLateralRearfoot_r',
'forceset/contactLateralMidfoot_r',
'forceset/contactLateralToe_r',
'forceset/contactMedialToe_r',
'forceset/contactMedialMidfoot_r']
forceNamesLeftFoot = ['forceset/contactSphereHeel_l',
'forceset/contactLateralRearfoot_l',
'forceset/contactLateralMidfoot_l',
'forceset/contactLateralToe_l',
'forceset/contactMedialToe_l',
'forceset/contactMedialMidfoot_l']
if self.contact_tracking:
contactTracking = osim.MocoContactTrackingGoal('contact', 0.001)
contactTracking.setExternalLoadsFile(
'resources/Rajagopal2016/grf_walk.xml')
contactTracking.addContactGroup(forceNamesRightFoot, 'Right_GRF')
contactTracking.addContactGroup(forceNamesLeftFoot, 'Left_GRF')
contactTracking.setProjection("plane")
contactTracking.setProjectionVector(osim.Vec3(0, 0, 1))
problem.addGoal(contactTracking)
# Configure the solver
# --------------------
solver = osim.MocoCasADiSolver.safeDownCast(study.updSolver())
solver.resetProblem(problem)
solver.set_optim_constraint_tolerance(1e-3)
solver.set_optim_convergence_tolerance(1e-3)
solver.set_multibody_dynamics_mode('implicit')
solver.set_minimize_implicit_multibody_accelerations(True)
solver.set_implicit_multibody_accelerations_weight(
1e-6 / model.getNumCoordinates())
solver.set_implicit_multibody_acceleration_bounds(
osim.MocoBounds(-200, 200))
# Set the guess
# -------------
# Create a guess compatible with this problem.
guess = solver.createGuess()
# Load the inverse problem solution and set its states and controls
# trajectories to the guess.
inverseSolution = osim.MocoTrajectory(
os.path.join(root_dir, self.inverse_solution_relpath))
inverseStatesTable = inverseSolution.exportToStatesTable()
guess.insertStatesTrajectory(inverseStatesTable, True)
# Changing this initial guess has a large negative effect on the
# solution! Lots of knee flexion.
# guess.setState('/jointset/ground_pelvis/pelvis_ty/value',
# osim.Vector(guess.getNumTimes(), 1.01))
inverseControlsTable = inverseSolution.exportToControlsTable()
guess.insertControlsTrajectory(inverseControlsTable, True)
solver.setGuess(guess)
# Solve and print solution.
# -------------------------
solution = study.solve()
solution.write(self.get_solution_path(root_dir, config))
# Create a full gait cycle trajectory from the periodic solution.
fullTraj = osim.createPeriodicTrajectory(solution)
fullTraj.write(self.get_solution_path_fullcycle(root_dir, config))
# Compute ground reaction forces generated by contact sphere from the
# full gait cycle trajectory.
externalLoads = osim.createExternalLoadsTableForGait(
model, fullTraj, forceNamesRightFoot, forceNamesLeftFoot)
osim.writeTableToFile(externalLoads,
self.get_solution_path_grfs(root_dir, config))
def report_results(self, root_dir, args):
self.parse_args(args)
if self.inverse:
sol_inverse_table = osim.TimeSeriesTable(self.mocoinverse_solution_file % root_dir)
inverse_duration = sol_inverse_table.getTableMetaDataString('solver_duration')
inverse_duration = float(inverse_duration) / 60.0
print('inverse duration ', inverse_duration)
with open(os.path.join(root_dir, 'results/'
'motion_prescribed_walking_inverse_duration.txt'), 'w') as f:
f.write(f'{inverse_duration:.1f}')
if self.knee:
sol_inverse_jr_table = osim.TimeSeriesTable(self.mocoinverse_jointreaction_solution_file % root_dir)
inverse_jr_duration = sol_inverse_jr_table.getTableMetaDataString('solver_duration')
inverse_jr_duration = float(inverse_jr_duration) / 60.0
print('inverse joint reaction duration ', inverse_jr_duration)
with open(os.path.join(root_dir, 'results/'
'motion_prescribed_walking_inverse_jr_duration.txt'), 'w') as f:
f.write(f'{inverse_jr_duration:.1f}')
emg = self.load_electromyography(root_dir)
emgPerry = self.load_electromyography_PerryBurnfield(root_dir)
modelProcessor = self.create_model_processor(root_dir)
model = modelProcessor.process()
model.initSystem()
numConstraints = 2
numDOFs = model.getCoordinateSet().getSize() - numConstraints
print(f'Degrees of freedom: {numDOFs}')
if self.inverse:
sol_inverse = osim.MocoTrajectory(
self.mocoinverse_solution_file % root_dir)
time_inverse = sol_inverse.getTimeMat()
most_neg = self.calc_negative_muscle_forces_base(model, sol_inverse)
if most_neg < -0.005:
raise Exception("Muscle forces are too negative! sol_inverse")
if self.knee and self.inverse:
sol_inverse_jointreaction = \
osim.MocoTrajectory(self.mocoinverse_jointreaction_solution_file % root_dir)
sol_inverse_jointreaction.insertStatesTrajectory(
sol_inverse.exportToStatesTable(), False)
most_neg = self.calc_negative_muscle_forces_base(
model, sol_inverse_jointreaction)
if most_neg < -0.005:
raise Exception(
"Muscle forces are too negative! sol_inverse_jointreaction")
mocoinverse_jr_solution_file = \
self.mocoinverse_jointreaction_solution_file.replace('.sto',
'_with_q_u.sto')
sol_inverse_jointreaction.write(mocoinverse_jr_solution_file % root_dir)
time_inverse_jointreaction = sol_inverse_jointreaction.getTimeMat()
plot_breakdown = False
coords = ['/jointset/hip_l/hip_flexion_l',
'/jointset/hip_l/hip_adduction_l',
'/jointset/hip_l/hip_rotation_l',
'/jointset/walker_knee_l/knee_angle_l',
'/jointset/ankle_l/ankle_angle_l']
if plot_breakdown and self.inverse:
fig = utilities.plot_joint_moment_breakdown(model,
sol_inverse,
coords
)
fig.savefig(os.path.join(root_dir, 'results/motion_prescribed_walking_inverse_'
'joint_moment_breakdown.png'),
dpi=600)
# report = osim.report.Report(model, mocoinverse_jr_solution_file % root_dir)
# report.generate()
if plot_breakdown and self.knee:
fig = utilities.plot_joint_moment_breakdown(model,
sol_inverse_jointreaction,
coords
)
fig.savefig(os.path.join(root_dir,
'results/motion_prescribed_walking_inverse_'
'jointreaction_joint_moment_breakdown.png'),
dpi=600)
coords = [
(f'/jointset/ground_pelvis/pelvis_tx', 'pelvis tx', 1.0),
(f'/jointset/ground_pelvis/pelvis_ty', 'pelvis ty', 1.0),
(f'/jointset/ground_pelvis/pelvis_tz', 'pelvis tz', 1.0),
(f'/jointset/ground_pelvis/pelvis_rotation', 'pelvis rotation', 1.0),
(f'/jointset/ground_pelvis/pelvis_list', 'pelvis list', 1.0),
(f'/jointset/ground_pelvis/pelvis_tilt', 'pelvis tilt', 1.0),
(f'/jointset/back/lumbar_rotation', 'lumbar rotation', 1.0),
(f'/jointset/back/lumbar_extension', 'lumbar extension', 1.0),
(f'/jointset/back/lumbar_bending', 'lumbar bending', 1.0),
(f'/jointset/hip_{self.side}/hip_adduction_{self.side}',
'hip adduction', 1.0),
(f'/jointset/hip_{self.side}/hip_rotation_{self.side}',
'hip rotation', 1.0),
(
f'/jointset/hip_{self.side}/hip_flexion_{self.side}', 'hip flexion',
1.0),
(f'/jointset/walker_knee_{self.side}/knee_angle_{self.side}',
'knee flexion', 1.0),
(f'/jointset/ankle_{self.side}/ankle_angle_{self.side}',
'ankle plantarflexion', 1.0),
]
from utilities import toarray
fig = plt.figure(figsize=(4, 8))
for ic, coord in enumerate(coords):
ax = plt.subplot(7, 2, ic + 1)
if self.inverse:
y_inverse = coord[2] * np.rad2deg(
sol_inverse.getStateMat(f'{coord[0]}/value'))
self.plot(ax, time_inverse, y_inverse, label='MocoInverse',
linewidth=2)
ax.plot([0], [0], label='MocoInverse-knee', linewidth=2)
ax.set_xlim(0, 100)
if ic == 1:
ax.legend(frameon=False, handlelength=1.9)
if ic < len(coords) - 1:
ax.set_xticklabels([])
else:
ax.set_xlabel('time (% gait cycle)')
ax.get_xaxis().set_label_coords(0.5, 0)
# The '0' would intersect with the y-axis, so remove it.
ax.set_xticks([0, 50, 100])
ax.set_xticklabels(['', '50', '100'])
ax.set_ylabel(f'{coord[1]} (degrees)')
ax.get_yaxis().set_label_coords(-0.15, 0.5)
ax.spines['bottom'].set_position('zero')
utilities.publication_spines(ax)
fig.savefig(os.path.join(root_dir,
'figures/motion_prescribed_walking_kinematics.png'),
dpi=600)
# Fig7
# ----
fig = plt.figure(figsize=(7.5, 3.3))
gs = gridspec.GridSpec(3, 4) # , width_ratios=[0.8, 1, 1, 1])
ax = fig.add_subplot(gs[0:2, 0])
import cv2
# Convert BGR color ordering to RGB.
image = cv2.imread(
os.path.join(
root_dir,
'figures/motion_prescribed_walking_inverse_model.png'))[
..., ::-1]
ax.imshow(image)
plt.axis('off')
muscles = [
((0, 0), 'glmax2', 'gluteus maximus', 'Perry', 'GluteusMaximusUpper'),
((0, 1), 'iliacus', 'iliacus', 'Perry', 'Iliacus'),
((0, 2), 'recfem', 'rectus femoris', '', 'RF'),
((1, 0), 'semiten', 'semitendinosus', '', 'MH'),
((1, 1), 'bfsh', 'biceps femoris short head', '', 'BF'),
((1, 2), 'vaslat', 'vastus lateralis', '', 'VL'),
((2, 0), 'gasmed', 'medial gastrocnemius', '', 'GAS'),
((2, 1), 'soleus', 'soleus', '', 'SOL'),
((2, 2), 'tibant', 'tibialis anterior', '', 'TA'),
]
legend_handles_and_labels = []
for im, muscle in enumerate(muscles):
ax = plt.subplot(gs[muscle[0][0], muscle[0][1] + 1])
# excitation_path = f'/forceset/{muscle[1]}_{self.side}'
activation_path = f'/forceset/{muscle[1]}_{self.side}/activation'
legend_musc = []
if self.inverse:
inverse_activ = sol_inverse.getStateMat(activation_path)
# inverse_excit = sol_inverse.getControlMat(excitation_path)
handle, = self.plot(ax, time_inverse, inverse_activ,
label='MocoInverse',
color='black',
linewidth=2, zorder=2)
legend_musc.append((handle, 'MocoInverse'))
# inverse_fine_activ = sol_inverse_fine.getStateMat(activation_path)
# handle, = self.plot(ax, time_inverse_fine, inverse_fine_activ,
# label='MocoInverse, fine',
# color='gray',
# linestyle=':',
# linewidth=2, zorder=2)
# legend_musc.append((handle, 'MocoInverse, fine'))
if self.knee and self.inverse:
handle, = self.plot(ax, time_inverse_jointreaction,
# sol_inverse_jointreaction.getControlMat(excitation_path),
sol_inverse_jointreaction.getStateMat(activation_path),
label='MocoInverse, knee',
linewidth=2, zorder=3)
legend_musc.append((handle, 'MocoInverse, knee'))
if self.inverse:
if muscle[3] == 'Perry':
handle = ax.fill_between(emgPerry['percent_gait_cycle'],
emgPerry[muscle[4]] / 100.0,
np.zeros_like(emgPerry[muscle[4]]),
clip_on=False,
color='lightgray',
label='electromyography')
else:
handle = self.plot(ax, emg['time'],
emg[muscle[4]] * np.max(inverse_activ),
shift=False, fill=True, color='lightgray',
label='experiment')
legend_musc.append((handle, 'electromyography'))
if im == 0:
legend_handles_and_labels = legend_musc
ax.set_ylim(0, 1)
ax.set_xlim(0, 100)
ax.set_xticks([0, 50, 100])
if muscle[0][0] < 2:
ax.set_xticklabels([])
else:
ax.set_xlabel('time (% gait cycle)')
if muscle[0][1] == 0:
ax.set_ylabel('activation')
title = f' {muscle[2]}'
plt.text(0.5, 1.20, title,
horizontalalignment='center',
verticalalignment='top',
transform=ax.transAxes)
ax.set_yticks([0, 1])
utilities.publication_spines(ax)
# if self.inverse and self.knee:
legend_handles, legend_labels = zip(*legend_handles_and_labels)
plt.figlegend(legend_handles, legend_labels,
frameon=False,
loc=(0.025, 0.15),
)
fig.tight_layout(h_pad=1, pad=0.4)
self.savefig(fig, os.path.join(root_dir, 'figures/Fig7'))
# Supplementary CMC comparison.
# -----------------------------
if self.cmc:
sol_cmc = osim.TimeSeriesTable(
os.path.join(root_dir, 'results',
'motion_prescribed_walking_cmc_results',
'motion_prescribed_walking_cmc_states.sto'))
time_cmc = np.array(sol_cmc.getIndependentColumn())
sol_so = osim.TimeSeriesTable(
os.path.join(root_dir, 'results',
'motion_prescribed_walking_analyze_results',
'motion_prescribed_walking_analyze_'
'StaticOptimization_activation.sto'))
time_so = np.array(sol_so.getIndependentColumn())
# exc_cmc = osim.TimeSeriesTable(
# os.path.join(root_dir, 'results',
# 'motion_prescribed_walking_cmc_results',
# 'motion_prescribed_walking_cmc_controls.sto'))
# time_exc_cmc = np.array(exc_cmc.getIndependentColumn())
fig = plt.figure(figsize=(5.2, 3.3))
gs = gridspec.GridSpec(3, 3)
legend_handles_and_labels = []
for im, muscle in enumerate(muscles):
ax = plt.subplot(gs[muscle[0][0], muscle[0][1]])
activation_path = f'/forceset/{muscle[1]}_{self.side}/activation'
legend_musc = []
if self.inverse:
inverse_activ = sol_inverse.getStateMat(activation_path)
handle, = self.plot(ax, time_inverse, inverse_activ,
label='MocoInverse',
color='black',
linewidth=2.5, zorder=2)
legend_musc.append((handle, 'MocoInverse'))
if self.cmc:
so_activ = toarray(
sol_so.getDependentColumn(f'{muscle[1]}_{self.side}'))
handle, = self.plot(ax, time_so, so_activ,
label='Static Optimization',
linewidth=1.5,
zorder=2)
legend_musc.append((handle, 'Static Optimization'))
# cmc_excit = toarray(
# exc_cmc.getDependentColumn(f'{muscle[1]}_{self.side}'))
# handle, = self.plot(ax, time_exc_cmc, cmc_excit,
# label='CMC excitation',
# linewidth=2,
# zorder=2)
# legend_musc.append((handle, 'CMC excitation'))
cmc_activ = toarray(sol_cmc.getDependentColumn(activation_path))
handle, = self.plot(ax, time_cmc, cmc_activ,
label='CMC',
linewidth=1.5,
zorder=2)
legend_musc.append((handle, 'CMC'))
# if self.inverse:
# if muscle[3] == 'Perry':
# handle = ax.fill_between(emgPerry['percent_gait_cycle'],
# emgPerry[muscle[4]] / 100.0,
# np.zeros_like(emgPerry[muscle[4]]),
# clip_on=False,
# color='lightgray',
# label='electromyography')
# else:
# handle = self.plot(ax, emg['time'],
# emg[muscle[4]] * np.max(inverse_activ),
# shift=False, fill=True, color='lightgray',
# label='experiment')
# legend_musc.append((handle, 'electromyography'))
if im == 0:
legend_handles_and_labels = legend_musc
ax.set_ylim(0, 1)
ax.set_xlim(0, 100)
ax.set_xticks([0, 50, 100])
if muscle[0][0] < 2:
ax.set_xticklabels([])
else:
ax.set_xlabel('time (% gait cycle)')
if muscle[0][1] == 0:
ax.set_ylabel('activation')
title = f' {muscle[2]}'
plt.text(0.5, 1.20, title,
horizontalalignment='center',
verticalalignment='top',
transform=ax.transAxes)
ax.set_yticks([0, 1])
utilities.publication_spines(ax)
# if self.inverse and self.knee:
legend_handles, legend_labels = zip(*legend_handles_and_labels)
plt.figlegend(legend_handles, legend_labels,
frameon=False,
loc=(0.09, 0.47),
handlelength=1,
fontsize=8,
)
fig.tight_layout(h_pad=1, pad=0.4)
self.savefig(fig, os.path.join(root_dir, 'figures/S2_Fig'))
res_to_genforce_labels = dict()
if self.inverse:
genforces = utilities.calc_net_generalized_forces(
model, sol_inverse)
genforce_labels = genforces.getColumnLabels()
res_inverse = self.calc_reserves(root_dir, sol_inverse)
column_labels = res_inverse.getColumnLabels()
for orig_gen_label in genforce_labels:
gen_label = orig_gen_label.replace('_moment', '')
gen_label = gen_label.replace('_force', '')
for res_label in column_labels:
if gen_label in res_label:
res_to_genforce_labels[res_label] = orig_gen_label
max_res_inverse = -np.inf
max_res_inverse_percent_genforce = -np.inf
max_label = ''
for icol in range(res_inverse.getNumColumns()):
label = column_labels[icol]
if (('arm' in label) or
('elbow' in label) or
('pro_sup' in label)):
continue
column = utilities.toarray(
res_inverse.getDependentColumnAtIndex(icol))
max = np.max(np.abs(column))
genforce_label = res_to_genforce_labels[label]
genforce = utilities.toarray(
genforces.getDependentColumn(genforce_label))
max_genforce = np.max(np.abs(genforce))
max_percent_genforce = 100.0 * (max / max_genforce)
if max_percent_genforce > max_res_inverse_percent_genforce:
max_res_inverse = max
max_label = label
max_res_inverse_percent_genforce = max_percent_genforce
print(f'inverse max abs {label}: {max:.2f} '
f'({max_percent_genforce:.2f}% peak generalized force)')
with open(os.path.join(root_dir, 'results/motion_prescribed_walking_'
'inverse_max_reserve.txt'), 'w') as f:
f.write(f'{max_res_inverse:.2f} N-m in reserve {max_label}\n')
f.write(f'{max_res_inverse_percent_genforce:.2f}% of peak '
f'generalized force')
if self.knee and self.inverse:
genforces = utilities.calc_net_generalized_forces(
model, sol_inverse_jointreaction)
genforce_labels = genforces.getColumnLabels()
res_inverse_jr = self.calc_reserves(root_dir,
sol_inverse_jointreaction)
column_labels = res_inverse_jr.getColumnLabels()
max_res_inverse_jr = -np.inf
max_res_inverse_jr_percent_genforce = -np.inf
max_label = ''
for icol in range(res_inverse.getNumColumns()):
label = column_labels[icol]
if (('arm' in label) or
('elbow' in label) or
('pro_sup' in label)):
continue
column = utilities.toarray(
res_inverse_jr.getDependentColumnAtIndex(icol))
max = np.max(np.abs(column))
genforce_label = res_to_genforce_labels[label]
genforce = utilities.toarray(
genforces.getDependentColumn(genforce_label))
max_genforce = np.max(np.abs(genforce))
max_percent_genforce = 100.0 * (max / max_genforce)
if max_percent_genforce > max_res_inverse_jr_percent_genforce:
max_res_inverse_jr = max
max_label = label
max_res_inverse_jr_percent_genforce = max_percent_genforce
print(f'inverse_jr max abs {label}: {max:.2f} '
f'({max_percent_genforce:.2f}% peak generalized force)')
with open(os.path.join(root_dir, 'results/motion_prescribed_walking_'
'inverse_jr_max_reserve.txt'), 'w') as f:
f.write(f'{max_res_inverse_jr:.2f} N-m in reserve {max_label}\n')
f.write(f'{max_res_inverse_jr_percent_genforce:.2f}% of peak '
f'generalized force')
maxjr_inverse, avgjr_inverse = \
self.calc_knee_reaction_force(root_dir, sol_inverse)
maxjr_inverse_jr, avgjr_inverse_jr = \
self.calc_knee_reaction_force(root_dir, sol_inverse_jointreaction)
print(f'Max joint reaction {maxjr_inverse} -> {maxjr_inverse_jr}')
with open(os.path.join(root_dir, 'results/motion_prescribed_walking_'
'inverse_maxjr.txt'), 'w') as f:
f.write(f'{maxjr_inverse:.1f}')
with open(os.path.join(root_dir, 'results/motion_prescribed_walking_'
'inverse_jr_maxjr.txt'), 'w') as f:
f.write(f'{maxjr_inverse_jr:.1f}')
print(f'Average joint reaction {avgjr_inverse} -> {avgjr_inverse_jr}')
with open(os.path.join(root_dir, 'results/motion_prescribed_walking_'
'inverse_avgjr.txt'), 'w') as f:
f.write(f'{avgjr_inverse:.1f}')
with open(os.path.join(root_dir, 'results/motion_prescribed_walking_'
'inverse_jr_avgjr.txt'), 'w') as f:
f.write(f'{avgjr_inverse_jr:.1f}')
if self.inverse:
states = sol_inverse.exportToStatesTrajectory(model)
duration = sol_inverse.getFinalTime() - sol_inverse.getInitialTime()
avg_speed = (model.calcMassCenterPosition(states[states.getSize() - 1])[0] -
model.calcMassCenterPosition(states[0])[0]) / duration
print(f'Average speed: {avg_speed}')
if self.inverse:
output_fpath = os.path.join(
root_dir, 'results',
'motion_prescribed_walking_inverse_solution_report.pdf')
report = osim.report.Report(
model,
self.mocoinverse_solution_file % root_dir,
output=output_fpath)
report.generate()
if self.knee and self.inverse:
output_fpath = os.path.join(
root_dir, 'results',
'motion_prescribed_walking_inverse_jointreaction_solution_'
'report.pdf')
report = osim.report.Report(
model,
mocoinverse_jr_solution_file % root_dir,
output=output_fpath)
report.generate()
def report_results(self, root_dir, args):
self.parse_args(args)
modelProcessor = self.create_model_processor(root_dir)
model = modelProcessor.process()
state = model.initSystem()
mass = model.getTotalMass(state)
gravity = model.getGravity()
BW = mass*abs(gravity[1])
if self.visualize:
for config in self.configs:
solution = osim.MocoTrajectory(
self.get_solution_path_fullcycle(root_dir, config))
osim.visualize(model, solution.exportToStatesTable())
emg = self.load_electromyography(root_dir)
fig = plt.figure(figsize=(7.5, 7))
gs = gridspec.GridSpec(36, 3)
ax_time = fig.add_subplot(gs[0:12, 0])
ax_grf_x = fig.add_subplot(gs[12:20, 0])
ax_grf_y = fig.add_subplot(gs[20:28, 0])
ax_grf_z = fig.add_subplot(gs[28:36, 0])
ax_add = fig.add_subplot(gs[0:9, 1])
ax_hip = fig.add_subplot(gs[9:18, 1])
ax_knee = fig.add_subplot(gs[18:27, 1])
ax_ankle = fig.add_subplot(gs[27:36, 1])
ax_list = list()
ax_list.append(ax_grf_x)
ax_list.append(ax_grf_y)
ax_list.append(ax_grf_z)
ax_list.append(ax_add)
ax_list.append(ax_hip)
ax_list.append(ax_knee)
ax_list.append(ax_ankle)
muscles = [
(fig.add_subplot(gs[0:4, 2]), 'glmax2', 'gluteus maximus', 'GMAX'),
(fig.add_subplot(gs[4:8, 2]), 'psoas', 'psoas', 'PSOAS'),
(fig.add_subplot(gs[8:12, 2]), 'semiten', 'semitendinosus', 'MH'),
(fig.add_subplot(gs[12:16, 2]), 'recfem', 'rectus femoris', 'RF'),
(fig.add_subplot(gs[16:20, 2]), 'bfsh', 'biceps femoris short head', 'BF'),
(fig.add_subplot(gs[20:24, 2]), 'vaslat', 'vastus lateralis', 'VL'),
(fig.add_subplot(gs[24:28, 2]), 'gasmed', 'medial gastrocnemius', 'GAS'),
(fig.add_subplot(gs[28:32, 2]), 'soleus', 'soleus', 'SOL'),
(fig.add_subplot(gs[32:36, 2]), 'tibant', 'tibialis anterior', 'TA'),
]
cmap = cm.get_cmap(self.cmap)
title_fs = 10
lw = 2.5
# experimental stride time
# ax_time.bar(0, self.final_time - self.initial_time, color='black')
# experimental ground reactions
grf_table = self.load_table(os.path.join(root_dir,
'resources', 'Rajagopal2016', 'grf_walk.mot'))
grf_start = np.argmin(abs(grf_table['time']-self.initial_time))
grf_end = np.argmin(abs(grf_table['time']-self.final_time))
time_grfs = grf_table['time'][grf_start:grf_end]
pgc_grfs = np.linspace(0, 100, len(time_grfs))
ax_grf_x.plot(pgc_grfs,
grf_table['ground_force_l_vx'][grf_start:grf_end]/BW,
color='black', lw=lw+1.0)
ax_grf_y.plot(pgc_grfs,
grf_table['ground_force_l_vy'][grf_start:grf_end]/BW,
color='black', lw=lw+1.0)
ax_grf_z.plot(pgc_grfs,
grf_table['ground_force_l_vz'][grf_start:grf_end]/BW,
color='black', lw=lw+1.0)
# experimental coordinates
coordinates = self.load_table(os.path.join(root_dir, 'resources',
'Rajagopal2016', 'coordinates.mot'))
coords_start = np.argmin(abs(coordinates['time']-self.initial_time))
coords_end = np.argmin(abs(coordinates['time']-self.final_time))
time_coords = coordinates['time'][coords_start:coords_end]
pgc_coords = np.linspace(0, 100, len(time_coords))
ax_add.plot(pgc_coords,
coordinates['hip_adduction_l'][coords_start:coords_end],
color='black', lw=lw + 1.0)
ax_hip.plot(pgc_coords,
coordinates['hip_flexion_l'][coords_start:coords_end],
color='black', lw=lw + 1.0)
ax_knee.plot(pgc_coords,
coordinates['knee_angle_l'][coords_start:coords_end],
color='black', lw=lw + 1.0)
ax_ankle.plot(pgc_coords,
coordinates['ankle_angle_l'][coords_start:coords_end],
color='black', lw=lw + 1.0)
# simulation results
for i, config in enumerate(self.configs):
color = cmap(config.cmap_index)
full_path = self.get_solution_path_fullcycle(root_dir, config)
full_traj = osim.MocoTrajectory(full_path)
sol_path = self.get_solution_path(root_dir, config)
sol_table = osim.TimeSeriesTable(sol_path)
if self.coordinate_tracking:
trackingCostStr = \
sol_table.getTableMetaDataString('objective_state_tracking')
else:
trackingCostStr = \
sol_table.getTableMetaDataString(
'objective_marker_tracking')
trackingCost = float(trackingCostStr) / config.tracking_weight
effortCost = 0
if config.effort_weight:
effortCostStr = \
sol_table.getTableMetaDataString('objective_control_effort')
effortCost = float(effortCostStr) / config.effort_weight
print(f'effort and tracking costs (config: {config.name}): ',
effortCost, trackingCost)
grf_path = self.get_solution_path_grfs(root_dir, config)
grf_table = self.load_table(grf_path)
time = full_traj.getTimeMat()
pgc = np.linspace(0, 100, len(time))
# pareto front
ax_time.plot(trackingCost, effortCost, color=color,
marker='o')
ax_time.text(trackingCost, effortCost, config.legend_entry)
# ax_time.bar(i+1, time[-1]-time[0], color=color)
# ax_time.set_ylim(0, 1.2)
# ax_time.set_yticks([0, 0.2, 0.4, 0.6, 0.8, 1.0, 1.2])
ax_time.set_xlabel('tracking cost')
ax_time.set_ylabel('effort cost')
# ax_time.set_xticks([0, 1, 2, 3])
# ax_time.set_xticklabels(
# ['data', 'track', 'track\n + \neffort', 'effort'])
ax_time.set_title('COST TRADE-OFF\n', weight='bold', size=title_fs)
ax_time.set_aspect(1.0/ax_time.get_data_ratio()*0.8, anchor='N')
utilities.publication_spines(ax_time)
# ground reaction forces
ax_grf_x.plot(pgc, grf_table['ground_force_l_vx']/BW, color=color,
lw=lw)
ax_grf_x.set_ylabel('horizontal force (BW)')
ax_grf_x.set_ylim(-0.35, 0.35)
ax_grf_x.set_yticks([-0.2, 0, 0.2])
ax_grf_x.set_title('GROUND REACTIONS', weight='bold',
size=title_fs)
ax_grf_x.set_xticklabels([])
ax_grf_y.plot(pgc, grf_table['ground_force_l_vy']/BW, color=color,
lw=lw)
ax_grf_y.set_ylabel('vertical force (BW)')
ax_grf_y.set_ylim(0, 1.5)
ax_grf_y.set_yticks([0, 0.5, 1, 1.5])
ax_grf_y.set_xticklabels([])
ax_grf_z.plot(pgc, grf_table['ground_force_l_vz']/BW, color=color,
lw=lw)
ax_grf_z.set_ylabel('transverse force (BW)')
ax_grf_z.set_xlabel('time (% gait cycle)')
ax_grf_z.set_ylim(-0.35, 0.35)
ax_grf_z.set_yticks([-0.2, 0, 0.2])
# kinematics
rad2deg = 180 / np.pi
ax_add.plot(pgc, rad2deg*full_traj.getStateMat(
'/jointset/hip_l/hip_adduction_l/value'), color=color, lw=lw)
ax_add.set_ylabel('hip adduction (degrees)')
ax_add.set_xticklabels([])
ax_add.set_title('KINEMATICS\n', weight='bold', size=title_fs)
ax_hip.plot(pgc, rad2deg*full_traj.getStateMat(
'/jointset/hip_l/hip_flexion_l/value'), color=color, lw=lw)
ax_hip.set_ylabel('hip flexion (degrees)')
ax_hip.set_xticklabels([])
ax_knee.plot(pgc, rad2deg*full_traj.getStateMat(
'/jointset/walker_knee_l/knee_angle_l/value'), color=color,
lw=lw)
ax_knee.set_ylabel('knee flexion (degrees)')
ax_knee.set_ylim(0, 90)
ax_knee.set_xticklabels([])
ax_ankle.plot(pgc, rad2deg*full_traj.getStateMat(
'/jointset/ankle_l/ankle_angle_l/value'), color=color,
lw=lw)
ax_ankle.set_ylabel('ankle dorsiflexion (degrees)')
ax_ankle.set_xlabel('time (% gait cycle)')
for ax in ax_list:
utilities.publication_spines(ax)
ax.set_xlim(0, 100)
ax.set_xticks([0, 50, 100])
# muscle activations
for im, muscle in enumerate(muscles):
activation_path = f'/forceset/{muscle[1]}_l/activation'
ax = muscle[0]
activation = full_traj.getStateMat(activation_path)
ax.plot(pgc, activation, color=color, lw=lw)
# electromyography data
# TODO: do not assume we want to normalize EMG via simulation 0.
if i == 0 and 'PSOAS' not in muscle:
self.plot(ax, emg['time'],
emg[muscle[3]] * np.max(activation), shift=False,
fill=True, color='lightgray',
label='electromyography')
ax.set_ylim(0, 1)
ax.set_yticks([0, 1])
ax.set_xlim(0, 100)
ax.set_xticks([0, 50, 100])
if im < 8:
ax.set_xticklabels([])
ax.text(0.5, 1.2, muscle[2],
horizontalalignment='center',
verticalalignment='top',
transform=ax.transAxes)
utilities.publication_spines(ax)
if im == 0:
ax.set_title('ACTIVATIONS\n', weight='bold', size=title_fs)
if im == 8: ax.set_xlabel('time (% gait cycle)')
fig.align_ylabels([ax_grf_x, ax_grf_y])
fig.align_ylabels([ax_time, ax_add, ax_hip, ax_knee, ax_ankle])
# fig.tight_layout()
fig.subplots_adjust(left=0.07, right=0.97, top=0.94, bottom=0.065,
hspace=200,
wspace=0.5)
fig.savefig(os.path.join(root_dir,
'figures/motion_tracking_walking.png'), dpi=600)
with open(os.path.join(root_dir, 'results/'
'motion_tracking_walking_durations.txt'), 'w') as f:
for config in self.configs:
sol_path = self.get_solution_path(root_dir, config)
sol_table = osim.TimeSeriesTable(sol_path)
duration = sol_table.getTableMetaDataString('solver_duration')
# Convert duration from seconds to hours.
duration = float(duration) / 60.0 / 60.0
print(f'duration (config {config.name}): ', duration)
f.write(f'(config {config.name}): {duration:.2f}\n')
for config in self.configs:
print(f'reserves for config {config.name}:')
sol_path = self.get_solution_path_fullcycle(root_dir, config)
solution = osim.MocoTrajectory(sol_path)
reserves = self.calc_reserves(root_dir, config, solution)
column_labels = reserves.getColumnLabels()
max_res = -np.inf
for icol in range(reserves.getNumColumns()):
column = utilities.toarray(
reserves.getDependentColumnAtIndex(icol))
max = np.max(np.abs(column))
max_res = np.max([max_res, max])
print(f' max abs {column_labels[icol]}: {max}')
muscle_mechanics = self.calc_muscle_mechanics(root_dir, config,
solution)
osim.STOFileAdapter.write(muscle_mechanics,
'results/tracking_walking_muscle_mechanics'
f'{config.name}.sto')
# Generate joint moment breakdown.
modelProcessor = self.create_model_processor(root_dir,
config=config)
model = modelProcessor.process()
print(f'Generating joint moment breakdown for {config.name}.')
coords = [
'/jointset/hip_l/hip_flexion_l',
'/jointset/walker_knee_l/knee_angle_l',
'/jointset/ankle_l/ankle_angle_l'
]
fig = plot_joint_moment_breakdown(model, solution,
coords)
fpath = os.path.join(root_dir,
'results/motion_tracking_walking_' +
f'breakdown_{config.name}.png')
fig.savefig(fpath, dpi=600)
# Generate PDF report.
# --------------------
trajectory_filepath = self.get_solution_path_fullcycle(root_dir,
self.configs[-1]
)
ref_files = list()
ref_files.append('tracking_walking_tracked_states.sto')
report_suffix = ''
for config in self.configs[:-1]:
ref_files.append(self.get_solution_path_fullcycle(root_dir, config))
report_suffix += '_' + config.name
report_suffix += '_' + self.configs[-1].name
report_output = f'motion_tracking_walking{report_suffix}_report.pdf'
report = osim.report.Report(model=model,
trajectory_filepath=trajectory_filepath,
ref_files=ref_files, bilateral=False,
output=report_output)
report.generate()
# %%
import os
import opensim as osim
from utils import read_from_storage
#%%
subject_dir = os.path.abspath('../')
output_dir = os.path.join(subject_dir, 'experimental_data')
trc_file = os.path.join(subject_dir, 'experimental_data/task.trc')
#%%
# load and resample markers at 0.01
df = read_from_storage(trc_file)
# create timeseries table
table = osim.TimeSeriesTable()
table.setColumnLabels(df.columns[1:])
table.addTableMetaDataString('DataRate', '100')
table.addTableMetaDataString('Units', 'mm')
# append data to table
for i in range(df.shape[0]):
table.appendRow(df.time.values[i], osim.RowVector(df.iloc[i, 1:].values))
# write table to file
osim.TRCFileAdapter.write(table.packVec3(),
os.path.join(output_dir, 'task_resampled.trc'))
#%%
def getHalfGaitCycle(grfFile = None):
# Convenience function for getting the middle half gait cycle based on GRF
# data. Note that this function is only applicable to the way the current
# data is structured, but could easily be edited (e.g. for getting a full
# gait cycle, or left to right foot strike etc.)
#
# Input: grfFile - .mot file containing GRF time history
#Check for input
if grfFile is None:
raise ValueError('A GRF file in .mot format is required')
#Load the GRF data
grfTable = osim.TimeSeriesTable(grfFile)
#Convert to more easily readable object for easier manipulation
#Get number of column labels and data rows
nLabels = grfTable.getNumColumns()
nRows = grfTable.getNumRows()
#Pre-allocate numpy array based on data size
grfArray = np.zeros((nRows,nLabels))
#Loop through labels and rows and get data
for iLabel in range(0,nLabels):
#Get current column label
currLabel = grfTable.getColumnLabel(iLabel)
#Store column index value if one of the vertical GRF data
if currLabel == 'calcn_r_ground_force_vy':
rightGrfCol = int(iLabel)
elif currLabel == 'calcn_l_ground_force_vy':
leftGrfCol = int(iLabel)
for iRow in range(0,nRows):
grfArray[iRow,iLabel] = grfTable.getDependentColumn(currLabel).getElt(0,iRow)
#Create a logical for where the right & left foot is in contact with the ground
#based on 50N threshold (which is what was originally used)
#Identify columns for right and left vertical GRF
rightGrfOn = grfArray[:,rightGrfCol] > 50
leftGrfOn = grfArray[:,leftGrfCol] > 50
#Identify the index where right and left GRF starts
#Identify where change in boolean value is present
rightGrfDiff = np.where(rightGrfOn[:-1] != rightGrfOn[1:])[0]
leftGrfDiff = np.where(leftGrfOn[:-1] != leftGrfOn[1:])[0]
#Identify where the index one above the change is true for foot strikes
rightGrfOnInd = list()
for gg in range(0,len(rightGrfDiff)):
if rightGrfOn[rightGrfDiff[gg]+1]:
rightGrfOnInd.append(rightGrfDiff[gg]+1)
leftGrfOnInd = list()
for gg in range(0,len(leftGrfDiff)):
if leftGrfOn[leftGrfDiff[gg]+1]:
leftGrfOnInd.append(leftGrfDiff[gg]+1)
#Find the middle right foot strike
rightStartInd = rightGrfOnInd[int(len(rightGrfOnInd)/2)]
#Find the next left foot strike after the right foot strike
leftStartInd = leftGrfOnInd[np.where(leftGrfOnInd > rightStartInd)[0][0]]
#Identify the times corresponding to these foot strikes
startTime = list(grfTable.getIndependentColumn())[rightStartInd]
endTime = list(grfTable.getIndependentColumn())[leftStartInd]
#Print outputs
print('Start Time: '+str(startTime))
print('End Time: '+str(endTime))
return startTime,endTime
inverse.set_mesh_interval(0.04)
inverse.set_constraint_tolerance(1e-3)
inverse.set_convergence_tolerance(1e-3)
if not os.path.isfile('effortSolution.sto'):
# Part 1f: Solve the problem!
inverseSolution = inverse.solve()
solution = inverseSolution.getMocoSolution()
solution.write('effortSolution.sto')
## Part 2: Plot the muscle redundancy problem solution.
# Load the experimental electromyography data and compare
# the effort minimization solution against this data. We will also use
# it later for the EMG-tracking problem. Each column in emg.sto is
# normalized so the maximum value for each signal is 1.0.
emgReference = osim.TimeSeriesTable('emg.sto')
helpers.compareSolutionToEMG(emgReference, 'effortSolution.sto')
## Part 3: Muscle redundancy problem: EMG-tracking.
# Modify the existing problem we created with the MocoInverse tool to solve
# a new problem where we will track electromyography (EMG) data.
# Part 3a: Call initialize() to get access to the MocoStudy contained within
# the MocoInverse instance. This will allow us to make additional
# modifications to the problem not provided by MocoInverse.
study = inverse.initialize()
problem = study.updProblem()
# Part 3b: Create a MocoControlTrackingGoal, set its weight, and provide
# the EMG data as the tracking reference. We also need to specify the
# reference labels for the four muscles whose EMG we will track.
cubicF = interp1d(xi, yi, kind = 'cubic', fill_value = 'extrapolate')
dVals = cubicF(np.linspace(t[0], t[-1], len(t)))
else:
#Set data values to the original array
dVals = datArr
#Filter data
dFilt = filtfilt(b, a, dVals)
#Set filtered data back in data array
filtData[:,cc] = dFilt
#Create blank time series table to fill
filtMarkers = osim.TimeSeriesTable()
#Fill row vector with data and append to the timeseries table
#Loop through rows
for rr in range(len(t)):
#Create row vector from current row of array
row = osim.RowVector.createFromMat(filtData[rr,:])
#Append row to table
filtMarkers.appendRow(rr,row)
#Set time value
filtMarkers.setIndependentValueAtIndex(rr,t[rr])
#Set column labels
filtMarkers.setColumnLabels(markersKeptList)
#Pack table back to Vec3
