exampleIMUTracking.py

This is an example that uses a squat-to-stand motion to create synthetic accelerometer data, and then creates a tracking simulation to track the synthetic accelerometer signals. This example is used in hands-on workshops, and accordingly has blanks that users must fill in. See exampleSquatToStand_answers.m for a completed version.

 
import opensim as osim
import exampleIMUTracking_helpers as helpers
# Add the directory above to access mocoPlotTrajectory.py
import sys
sys.path.insert(1, '../')
import mocoPlotTrajectory as plot
import numpy as np
import os
 
 
model = helpers.getTorqueDrivenSquatToStandModel()
 
# Part 1b: Add frames to the model that will represent our IMU locations. 
# The function addIMUFrame() adds a PhysicalOffsetFrame to a body at a 
# specified location and orientation. Each frame is added at the path:
#
# /bodyset/<body_name>/<body_name>_imu_offset
#
helpers.addIMUFrame(model, 'torso', 
    osim.Vec3(0.08, 0.3, 0), osim.Vec3(0, 0.5*np.pi, 0.5*np.pi))
helpers.addIMUFrame(model, 'femur_r', 
    osim.Vec3(0, -0.2, 0.05), osim.Vec3(0, 0, 0.5*np.pi))
helpers.addIMUFrame(model, 'tibia_r', osim.Vec3(0, -0.2, 0.05), 
    osim.Vec3(0, 0, 0.5*np.pi))
 
# Part 1c: Add IMU components to the model using the PhysicalOffsetFrames
# we just added to the model. We'll use the helper function addModelIMUs()
# included with OpenSenseUtilities.
imuFramePaths = osim.StdVectorString()
imuFramePaths.append( )
imuFramePaths.append( )
imuFramePaths.append( )
osim.OpenSenseUtilities().addModelIMUs(model, imuFramePaths)
model.initSystem()
 
 
 
# Part 2a: Create a new MocoStudy.
 
 
# Part 2b: Initialize the problem and set the model.
 
 
# Part 2c: Set bounds on the problem.
#
# problem.setTimeBounds(initial_bounds, final_bounds)
# problem.setStateInfo(path, trajectory_bounds, inital_bounds, final_bounds)
#
# All *_bounds arguments can be set to a range, [lower upper], or to a
# single value (equal lower and upper bounds). Empty brackets, [], indicate
# using default bounds (if they exist). You may set multiple state infos at
# once using setStateInfoPattern():
#
# problem.setStateInfoPattern(pattern, trajectory_bounds, inital_bounds, ...
#       final_bounds)
#
# This function supports regular expressions in the 'pattern' argument;
# use '.*' to match any substring of the state/control path
# For example, the following will set all coordinate value state infos:
#
# problem.setStateInfoPattern('/path/to/states/.*/value', ...)
 
# Time bounds
problem.setTimeBounds( )
 
# Position bounds: the model should start in a squat and finish 
# standing up.
problem.setStateInfo('/jointset/hip_r/hip_flexion_r/value', )
problem.setStateInfo('/jointset/knee_r/knee_angle_r/value', )
problem.setStateInfo('/jointset/ankle_r/ankle_angle_r/value', )
 
# Velocity bounds: all model coordinates should start and end at rest.
problem.setStateInfoPattern('/jointset/.*/speed', )
 
# Part 2d: Add a MocoControlGoal to the problem.
 
 
# Part 2e: Configure the solver.
solver = study.initCasADiSolver()
solver.set_num_mesh_intervals( )
solver.set_optim_constraint_tolerance( )
solver.set_optim_convergence_tolerance( )
 
if not os.path.isfile('predictSolution.sto'):
    # Part 2f: Solve! Write the solution to file, and visualize.
 
 
 
 
 
# Part 3a: Load the prediction solution.
 
 
# Part 3b: Compute the accelerometer signals using the analyzeVec3() free
# function included with SimulationUtilities. These free functions can be
# accessed in scripting by using the 'opensimSimulation' prefix. 
outputPaths = osim.StdVectorString()
outputPaths.append( )
accelerometerSignals = osim.analyzeVec3(model,
    predictSolution.exportToStatesTable(),
    predictSolution.exportToControlsTable(),
    outputPaths)
 
# Part 3c: Update the column labels of the accelerometer signals to match
# the offset frame paths. This is necessary for the tracking goal we'll add
# to the problem in Part 4. 
accelerometerSignals.setColumnLabels(imuFramePaths)
    
# Part 3d: Plot the synthetic acceleration signals.
helpers.plotAccelerationSignals(accelerometerSignals)
 
 
tracking = 
tracking.setFramePaths( )
tracking.setAccelerationReference( )
tracking.setGravityOffset( )
tracking.setExpressAccelerationsInTrackingFrames( )
problem.addGoal( )
 
# Part 4b: Reduce the control cost weight so that the tracking term will
# dominate.
 
 
if not os.path.isfile('trackingSolution.sto'):
    # Part 4c: Solve! Write the solution to file, and visualize.
 
 
 
 
plot.mocoPlotTrajectory('predictSolution.sto', 'trackingSolution.sto',
        'predict', 'track')
 
# Part 5b: Compare accelerations from tracking solution to the reference
# accelerations.
trackingSolution = osim.MocoTrajectory('trackingSolution.sto')
accelerometerSignalsTracking = osim.analyzeVec3(model, 
    trackingSolution.exportToStatesTable(), 
    trackingSolution.exportToControlsTable(), 
    outputPaths)
accelerometerSignalsTracking.setColumnLabels(imuFramePaths)
helpers.plotAccelerationSignals(accelerometerSignals, 
                                accelerometerSignalsTracking)