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.
3 import exampleIMUTracking_helpers
as helpers
6 sys.path.insert(1,
'../')
7 import mocoPlotTrajectory
as plot
15 model = helpers.getTorqueDrivenSquatToStandModel()
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))
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)
46 study = osim.MocoStudy()
49 problem = study.updProblem()
50 problem.setModel(model)
72 problem.setTimeBounds(0, 1)
76 problem.setStateInfo(
'/jointset/hip_r/hip_flexion_r/value',
78 problem.setStateInfo(
'/jointset/knee_r/knee_angle_r/value',
80 problem.setStateInfo(
'/jointset/ankle_r/ankle_angle_r/value',
84 problem.setStateInfoPattern(
'/jointset/.*/speed', [], 0, 0)
87 problem.addGoal(osim.MocoControlGoal(
'myeffort'))
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)
95 if not os.path.isfile(
'predictSolution.sto'):
97 predictSolution = study.solve()
98 predictSolution.write(
'predictSolution.sto')
99 study.visualize(predictSolution)
109 predictSolution = osim.MocoTrajectory(
'predictSolution.sto')
114 outputPaths = osim.StdVectorString()
115 outputPaths.append(
'.*accelerometer_signal')
116 accelerometerSignals = osim.analyzeVec3(model,
117 predictSolution.exportToStatesTable(),
118 predictSolution.exportToControlsTable(),
124 accelerometerSignals.setColumnLabels(imuFramePaths)
127 helpers.plotAccelerationSignals(accelerometerSignals)
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)
145 problem.updGoal(
'myeffort').setWeight(0.001)
147 if not os.path.isfile(
'trackingSolution.sto'):
149 trackingSolution = study.solve()
150 trackingSolution.write(
'trackingSolution.sto')
151 study.visualize(trackingSolution)
157 plot.mocoPlotTrajectory(
'predictSolution.sto',
'trackingSolution.sto',
162 trackingSolution = osim.MocoTrajectory(
'trackingSolution.sto')
163 accelerometerSignalsTracking = osim.analyzeVec3(model,
164 trackingSolution.exportToStatesTable(),
165 trackingSolution.exportToControlsTable(),
167 accelerometerSignalsTracking.setColumnLabels(imuFramePaths)
168 helpers.plotAccelerationSignals(accelerometerSignals,
169 accelerometerSignalsTracking)