API  4.5
For C++ developers
exampleIMUTracking_answers.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.

1 
2 import opensim as osim
3 import exampleIMUTracking_helpers as helpers
4 # Add the directory above to access mocoPlotTrajectory.py
5 import sys
6 sys.path.insert(1, '../')
7 import mocoPlotTrajectory as plot
8 import numpy as np
9 import os
10 
11 
15 model = helpers.getTorqueDrivenSquatToStandModel()
16 
17 # Part 1b: Add frames to the model that will represent our IMU locations.
18 # The function addIMUFrame() adds a PhysicalOffsetFrame to a body at a
19 # specified location and orientation. Each frame is added at the path:
20 #
21 # /bodyset/<body_name>/<body_name>_imu_offset
22 #
23 helpers.addIMUFrame(model, 'torso',
24  osim.Vec3(0.08, 0.3, 0), osim.Vec3(0, 0.5*np.pi, 0.5*np.pi))
25 helpers.addIMUFrame(model, 'femur_r',
26  osim.Vec3(0, -0.2, 0.05), osim.Vec3(0, 0, 0.5*np.pi))
27 helpers.addIMUFrame(model, 'tibia_r', osim.Vec3(0, -0.2, 0.05),
28  osim.Vec3(0, 0, 0.5*np.pi))
29 
30 # Part 1c: Add IMU components to the model using the PhysicalOffsetFrames
31 # we just added to the model. We'll use the helper function addModelIMUs()
32 # included with OpenSenseUtilities.
33 imuFramePaths = osim.StdVectorString()
34 imuFramePaths.append('/bodyset/torso/torso_imu_offset')
35 imuFramePaths.append('/bodyset/femur_r/femur_r_imu_offset')
36 imuFramePaths.append('/bodyset/tibia_r/tibia_r_imu_offset')
37 osim.OpenSenseUtilities().addModelIMUs(model, imuFramePaths)
38 model.initSystem()
39 
40 
44 
45 # Part 2a: Create a new MocoStudy.
46 study = osim.MocoStudy()
47 
48 # Part 2b: Initialize the problem and set the model.
49 problem = study.updProblem()
50 problem.setModel(model)
51 
52 # Part 2c: Set bounds on the problem.
53 #
54 # problem.setTimeBounds(initial_bounds, final_bounds)
55 # problem.setStateInfo(path, trajectory_bounds, inital_bounds, final_bounds)
56 #
57 # All *_bounds arguments can be set to a range, [lower upper], or to a
58 # single value (equal lower and upper bounds). Empty brackets, [], indicate
59 # using default bounds (if they exist). You may set multiple state infos at
60 # once using setStateInfoPattern():
61 #
62 # problem.setStateInfoPattern(pattern, trajectory_bounds, inital_bounds, ...
63 # final_bounds)
64 #
65 # This function supports regular expressions in the 'pattern' argument;
66 # use '.*' to match any substring of the state/control path
67 # For example, the following will set all coordinate value state infos:
68 #
69 # problem.setStateInfoPattern('/path/to/states/.*/value', ...)
70 
71 # Time bounds
72 problem.setTimeBounds(0, 1)
73 
74 # Position bounds: the model should start in a squat and finish
75 # standing up.
76 problem.setStateInfo('/jointset/hip_r/hip_flexion_r/value',
77  [-2, 0.5], -2, 0)
78 problem.setStateInfo('/jointset/knee_r/knee_angle_r/value',
79  [-2, 0], -2, 0)
80 problem.setStateInfo('/jointset/ankle_r/ankle_angle_r/value',
81  [-0.5, 0.7], -0.5, 0)
82 
83 # Velocity bounds: all model coordinates should start and end at rest.
84 problem.setStateInfoPattern('/jointset/.*/speed', [], 0, 0)
85 
86 # Part 2d: Add a MocoControlGoal to the problem.
87 problem.addGoal(osim.MocoControlGoal('myeffort'))
88 
89 # Part 2e: Configure the solver.
90 solver = study.initCasADiSolver()
91 solver.set_num_mesh_intervals(25)
92 solver.set_optim_constraint_tolerance(1e-4)
93 solver.set_optim_convergence_tolerance(1e-4)
94 
95 if not os.path.isfile('predictSolution.sto'):
96  # Part 2f: Solve! Write the solution to file, and visualize.
97  predictSolution = study.solve()
98  predictSolution.write('predictSolution.sto')
99  study.visualize(predictSolution)
100 
101 
102 
107 
108 # Part 3a: Load the prediction solution.
109 predictSolution = osim.MocoTrajectory('predictSolution.sto')
110 
111 # Part 3b: Compute the accelerometer signals using the analyzeVec3() free
112 # function included with SimulationUtilities. These free functions can be
113 # accessed in scripting by using the 'opensimSimulation' prefix.
114 outputPaths = osim.StdVectorString()
115 outputPaths.append('.*accelerometer_signal')
116 accelerometerSignals = osim.analyzeVec3(model,
117  predictSolution.exportToStatesTable(),
118  predictSolution.exportToControlsTable(),
119  outputPaths)
120 
121 # Part 3c: Update the column labels of the accelerometer signals to match
122 # the offset frame paths. This is necessary for the tracking goal we'll add
123 # to the problem in Part 4.
124 accelerometerSignals.setColumnLabels(imuFramePaths)
125 
126 # Part 3d: Plot the synthetic acceleration signals.
127 helpers.plotAccelerationSignals(accelerometerSignals)
128 
129 
136 tracking = osim.MocoAccelerationTrackingGoal('acceleration_tracking')
137 tracking.setFramePaths(imuFramePaths)
138 tracking.setAccelerationReference(accelerometerSignals)
139 tracking.setGravityOffset(True)
140 tracking.setExpressAccelerationsInTrackingFrames(True)
141 problem.addGoal(tracking)
142 
143 # Part 4b: Reduce the control cost weight so that the tracking term will
144 # dominate.
145 problem.updGoal('myeffort').setWeight(0.001)
146 
147 if not os.path.isfile('trackingSolution.sto'):
148  # Part 4c: Solve! Write the solution to file, and visualize.
149  trackingSolution = study.solve()
150  trackingSolution.write('trackingSolution.sto')
151  study.visualize(trackingSolution)
152 
153 
154 
157 plot.mocoPlotTrajectory('predictSolution.sto', 'trackingSolution.sto',
158  'predict', 'track')
159 
160 # Part 5b: Compare accelerations from tracking solution to the reference
161 # accelerations.
162 trackingSolution = osim.MocoTrajectory('trackingSolution.sto')
163 accelerometerSignalsTracking = osim.analyzeVec3(model,
164  trackingSolution.exportToStatesTable(),
165  trackingSolution.exportToControlsTable(),
166  outputPaths)
167 accelerometerSignalsTracking.setColumnLabels(imuFramePaths)
168 helpers.plotAccelerationSignals(accelerometerSignals,
169  accelerometerSignalsTracking)