Skip to content

Commit 3aaae00

Browse files
nickbianconickbianco
authored
Merge pull request #4008 from opensim-org/example_3d_walking
Add Moco 3D tracking example with foot-ground contact
2 parents 54a0430 + 7a824ab commit 3aaae00

24 files changed

+2250
-283
lines changed

‎Bindings/Java/Matlab/examples/Moco/example3DWalking/example3DWalking.m‎

Lines changed: 330 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,330 @@
1+
% -------------------------------------------------------------------------- %
2+
% OpenSim Moco: example3DWalking.m %
3+
% -------------------------------------------------------------------------- %
4+
% Copyright (c) 2025 Stanford University and the Authors %
5+
% %
6+
% Author(s): Nicholas Bianco %
7+
% %
8+
% Licensed under the Apache License, Version 2.0 (the "License"); you may %
9+
% not use this file except in compliance with the License. You may obtain a %
10+
% copy of the License at http://www.apache.org/licenses/LICENSE-2.0 %
11+
% %
12+
% Unless required by applicable law or agreed to in writing, software %
13+
% distributed under the License is distributed on an "AS IS" BASIS, %
14+
% WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. %
15+
% See the License for the specific language governing permissions and %
16+
% limitations under the License. %
17+
% -------------------------------------------------------------------------- %
18+
19+
% This example demonstrates how to solve 3D walking optimization problems
20+
% using a foot-ground contact model. Polynomial functions are used to
21+
% represent muscle geometry via the FunctionBasedPath class which
22+
% significantly improves convergence time.
23+
%
24+
% See the README.txt next to this file for more information about the
25+
% reference data used in this example.
26+
27+
function example3DWalking()
28+
29+
% Import the OpenSim libraries.
30+
import org.opensim.modeling.*;
31+
32+
% Model preparation.
33+
% -----------------
34+
% Update the model to prepare it for tracking optimization. The default
35+
% minimimum muscle excitations and activations are set to 0; stiffness,
36+
% damping, and light torque actuation are added to the toes; and contact
37+
% geometry is added to the foot bodies in the model. The height of the
38+
% contact geometry elements are adjusted to better align with the ground.
39+
model = Model('subject_walk_scaled.osim');
40+
model.initSystem();
41+
42+
% Set minimum muscle controls and activations to 0 (default is 0.01).
43+
muscles = model.updMuscles();
44+
for imuscle = 1:muscles.getSize()
45+
muscle = Millard2012EquilibriumMuscle.safeDownCast(...
46+
muscles.get(imuscle-1));
47+
muscle.setMinimumActivation(0.0);
48+
muscle.setMinControl(0.0);
49+
end
50+
51+
% Add stiffness and damping to the joints. Toe stiffness and damping values
52+
% are based on Falisse et al. (2022), "Modeling toes contributes to
53+
% realistic stance knee mechanics in three-dimensional predictive
54+
% simulations of walking."
55+
expressionBasedForceSet = ForceSet(...
56+
'subject_walk_scaled_ExpressionBasedCoordinateForceSet.xml');
57+
for i = 0:expressionBasedForceSet.getSize()-1
58+
model.addComponent(expressionBasedForceSet.get(i).clone());
59+
end
60+
61+
% Add the contact geometry to the model.
62+
contactGeometrySet = ContactGeometrySet(...
63+
'subject_walk_scaled_ContactGeometrySet.xml');
64+
for i = 0:contactGeometrySet.getSize()-1
65+
contactGeometry = contactGeometrySet.get(i).clone();
66+
% Raise the ContactSpheres by 2 cm so that bottom of the spheres
67+
% are better aligned with the ground.
68+
if ~strcmp(contactGeometry.getName(), 'floor')
69+
location = contactGeometry.upd_location();
70+
location.set(1, location.get(1) + 0.02);
71+
end
72+
model.addContactGeometry(contactGeometry);
73+
end
74+
75+
% Add the contact forces to the model.
76+
contactForceSet = ForceSet('subject_walk_scaled_ContactForceSet.xml');
77+
for i = 0:contactForceSet.getSize()-1
78+
model.addComponent(contactForceSet.get(i).clone());
79+
end
80+
model.finalizeConnections();
81+
82+
% Tracking optimization.
83+
% ---------------------
84+
% Solve tracking optimization problems using the modified model. The
85+
% convergence times below were estimated on a machine using a processor
86+
% with 4.7 GHz base clock speed and 12 CPU cores (12 threads).
87+
88+
% Solve a torque-driven tracking problem to create a kinematic
89+
% trajectory that is consistent with the ground reaction forces.
90+
% This problem takes ~10 minutes to solve.
91+
runTrackingStudy(model, false);
92+
93+
% Solve a muscle-driven tracking problem using the kinematic trajectory
94+
% from the torque-driven problem as the initial guess.
95+
% This problem takes ~35 minutes to solve.
96+
runTrackingStudy(model, true);
97+
end
98+
99+
% Construct a MocoStudy to track joint kinematics and ground reaction forces
100+
% using a torque-driven or muscle-driven model with foot-ground contact
101+
% elements. The objective function weights were chosen such that the optimized
102+
% objective value falls roughly in the range [0.1, 10], which generally
103+
% improves convergence.
104+
function runTrackingStudy(model, muscleDriven)
105+
106+
% Import the OpenSim libraries.
107+
import org.opensim.modeling.*;
108+
109+
% Paths to the contact forces in the model.
110+
contactForcesRight = StdVectorString();
111+
contactForcesRight.add('/contactHeel_r');
112+
contactForcesRight.add('/contactLateralRearfoot_r');
113+
contactForcesRight.add('/contactLateralMidfoot_r');
114+
contactForcesRight.add('/contactMedialMidfoot_r');
115+
contactForcesRight.add('/contactLateralToe_r');
116+
contactForcesRight.add('/contactMedialToe_r');
117+
118+
contactForcesLeft = StdVectorString();
119+
contactForcesLeft.add('/contactHeel_l');
120+
contactForcesLeft.add('/contactLateralRearfoot_l');
121+
contactForcesLeft.add('/contactLateralMidfoot_l');
122+
contactForcesLeft.add('/contactMedialMidfoot_l');
123+
contactForcesLeft.add('/contactLateralToe_l');
124+
contactForcesLeft.add('/contactMedialToe_l');
125+
126+
% Configure study-specific settings.
127+
if (muscleDriven)
128+
studyName = 'muscle_driven_tracking';
129+
else
130+
studyName = 'torque_driven_tracking';
131+
132+
% Add weak CoordinateActuators to the toes. For the torque-driven
133+
% simulation, we do not want ModOpAddReserves() to add strong
134+
% actuators to the toes, since the toes will have no active actuation
135+
% in the muscle-driven problem.
136+
ca_toes_l = CoordinateActuator('mtp_angle_l');
137+
ca_toes_l.setName('mtp_angle_l_actuator');
138+
ca_toes_l.setOptimalForce(10);
139+
ca_toes_l.setMinControl(-1.0);
140+
ca_toes_l.setMaxControl(1.0);
141+
model.addForce(ca_toes_l);
142+
143+
ca_toes_r = CoordinateActuator('mtp_angle_r');
144+
ca_toes_r.setName('mtp_angle_r_actuator');
145+
ca_toes_r.setOptimalForce(10);
146+
ca_toes_r.setMinControl(-1.0);
147+
ca_toes_r.setMaxControl(1.0);
148+
model.addForce(ca_toes_r);
149+
end
150+
151+
% Modify the model to prepare it for tracking optimization
152+
model.initSystem();
153+
modelProcessor = ModelProcessor(model);
154+
if (muscleDriven)
155+
modelProcessor.append(ModOpIgnoreTendonCompliance());
156+
modelProcessor.append(ModOpReplaceMusclesWithDeGrooteFregly2016());
157+
modelProcessor.append(ModOpIgnorePassiveFiberForcesDGF());
158+
modelProcessor.append(ModOpScaleActiveFiberForceCurveWidthDGF(1.5));
159+
modelProcessor.append(ModOpReplacePathsWithFunctionBasedPaths( ...
160+
'subject_walk_scaled_FunctionBasedPathSet.xml'));
161+
else
162+
modelProcessor.append(ModOpRemoveMuscles());
163+
modelProcessor.append(ModOpAddReserves(500, 1.0, true, true));
164+
end
165+
166+
% Construct the reference kinematics TableProcessor.
167+
tableProcessor = TableProcessor('coordinates.sto');
168+
tableProcessor.append(TabOpUseAbsoluteStateNames());
169+
tableProcessor.append(TabOpAppendCoupledCoordinateValues());
170+
tableProcessor.append(TabOpAppendCoordinateValueDerivativesAsSpeeds());
171+
172+
% Construct the MocoTrack problem.
173+
track = MocoTrack();
174+
track.setName(studyName);
175+
track.setModel(modelProcessor);
176+
track.setStatesReference(tableProcessor);
177+
track.set_states_global_tracking_weight(0.05);
178+
track.set_control_effort_weight(0.1);
179+
track.set_allow_unused_references(true);
180+
track.set_track_reference_position_derivatives(true);
181+
track.set_initial_time(0.48);
182+
track.set_final_time(1.61);
183+
track.set_mesh_interval(0.02);
184+
185+
% Don't track the veritcal position of the pelvis and only lightly track
186+
% the speed. Let the optimization determine the vertical position of the
187+
% model, which will make it easier to find the position of the feet that
188+
% leads to the best tracking of the kinematics and ground reaction forces.
189+
statesWeightSet = MocoWeightSet();
190+
statesWeightSet.cloneAndAppend(...
191+
MocoWeight('/jointset/ground_pelvis/pelvis_ty/value', 0.0));
192+
statesWeightSet.cloneAndAppend(...
193+
MocoWeight('/jointset/ground_pelvis/pelvis_ty/speed', 0.1));
194+
track.set_states_weight_set(statesWeightSet);
195+
196+
% Get the underlying MocoStudy.
197+
study = track.initialize();
198+
problem = study.updProblem();
199+
200+
% Set bounds on the toe coordinate states.
201+
problem.setStateInfoPattern('/jointset/mtp_.*/value', [-1.0, 1.0]);
202+
problem.setStateInfoPattern('/jointset/mtp_.*/speed', [-20.0, 20.0]);
203+
204+
% Add a MocoContactTrackingGoal to the problem to track the ground
205+
% reaction forces.
206+
contactTracking = MocoContactTrackingGoal(...
207+
'grf_tracking', 5e-3);
208+
contactTracking.setExternalLoadsFile('grf_walk.xml');
209+
toeBodyRight = StdVectorString();
210+
toeBodyRight.add('/bodyset/toes_r');
211+
contactTracking.addContactGroup(MocoContactTrackingGoalGroup(...
212+
contactForcesRight, 'Right_GRF', toeBodyRight));
213+
toeBodyLeft = StdVectorString();
214+
toeBodyLeft.add('/bodyset/toes_l');
215+
contactTracking.addContactGroup(MocoContactTrackingGoalGroup(...
216+
contactForcesLeft, 'Left_GRF', toeBodyLeft));
217+
problem.addGoal(contactTracking);
218+
219+
% Constrain the initial states to be close to the reference.
220+
coordinatesUpdated = tableProcessor.process(model);
221+
labels = coordinatesUpdated.getColumnLabels();
222+
index = coordinatesUpdated.getNearestRowIndexForTime(0.48);
223+
for i = 1:labels.size()
224+
label = string(labels.get(i-1));
225+
value = coordinatesUpdated.getDependentColumn(label);
226+
if (contains(label, '/speed'))
227+
lower = value.get(index) - 0.1;
228+
upper = value.get(index) + 0.1;
229+
else
230+
lower = value.get(index) - 0.05;
231+
upper = value.get(index) + 0.05;
232+
end
233+
problem.setStateInfo(label, [], [lower, upper]);
234+
end
235+
236+
% Constrain the states and controls to be periodic.
237+
if (muscleDriven)
238+
periodicityGoal = MocoPeriodicityGoal('periodicity');
239+
coordinates = model.getCoordinateSet();
240+
for icoord = 1:coordinates.getSize()
241+
coordinate = coordinates.get(icoord-1);
242+
coordName = string(coordinate.getName());
243+
% Exclude the knee_angle_l/r_beta coordinates from the periodicity
244+
% constraint because they are coupled to the knee_angle_l/r
245+
% coordinates.
246+
if (contains(coordName, 'beta'))
247+
continue;
248+
end
249+
if (~contains(coordName, '_tx'))
250+
valueName = coordinate.getStateVariableNames().get(0);
251+
periodicityGoal.addStatePair(...
252+
MocoPeriodicityGoalPair(valueName));
253+
end
254+
speedName = coordinate.getStateVariableNames().get(1);
255+
periodicityGoal.addStatePair(MocoPeriodicityGoalPair(speedName));
256+
end
257+
258+
muscles = model.getMuscles();
259+
for imusc = 1:muscles.getSize()
260+
muscle = muscles.get(imusc-1);
261+
stateName = muscle.getStateVariableNames().get(0);
262+
periodicityGoal.addStatePair(MocoPeriodicityGoalPair(stateName));
263+
controlName = muscle.getAbsolutePathString();
264+
periodicityGoal.addControlPair(...
265+
MocoPeriodicityGoalPair(controlName));
266+
end
267+
268+
actuators = model.getActuators();
269+
for iactu = 1:actuators.getSize()
270+
actu = CoordinateActuator.safeDownCast(actuators.get(iactu-1));
271+
if ~isempty(actu)
272+
controlName = actu.getAbsolutePathString();
273+
periodicityGoal.addControlPair(...
274+
MocoPeriodicityGoalPair(controlName));
275+
end
276+
end
277+
278+
problem.addGoal(periodicityGoal);
279+
end
280+
281+
% Customize the solver settings.
282+
% ------------------------------
283+
solver = MocoCasADiSolver.safeDownCast(study.updSolver());
284+
% Use the Legendre-Gauss-Radau transcription scheme, a psuedospectral
285+
% scheme with high integration accuracy.
286+
solver.set_transcription_scheme('legendre-gauss-radau-3');
287+
% Use the Bordalba et al. (2023) kinematic constraint method.
288+
solver.set_kinematic_constraint_method('Bordalba2023');
289+
% Set the solver's convergence and constraint tolerances.
290+
solver.set_optim_convergence_tolerance(1e-2);
291+
solver.set_optim_constraint_tolerance(1e-4);
292+
% We've updated the MocoProblem, so call resetProblem() to pass the updated
293+
% problem to the solver.
294+
solver.resetProblem(problem);
295+
% When MocoTrack::initialize() is called, the solver is created with a
296+
% default guess. Since we've updated the problem and changed the
297+
% transcription scheme, it is a good idea to generate a new guess. If
298+
% solving a muscle-driven problem, use the solution from the
299+
% torque-driven problem as the initial guess.
300+
guess = solver.createGuess();
301+
torqueDrivenSolutionFile = ...
302+
'example3DWalking_torque_driven_tracking_solution.sto';
303+
if (muscleDriven && exist(torqueDrivenSolutionFile, 'file'))
304+
initialGuess = MocoTrajectory(torqueDrivenSolutionFile);
305+
guess.insertStatesTrajectory(initialGuess.exportToStatesTable(), true);
306+
controls = guess.exportToControlsTable();
307+
controls.updMatrix().setToZero();
308+
guess.insertControlsTrajectory(controls, true);
309+
end
310+
solver.setGuess(guess);
311+
312+
% Solve!
313+
% ------
314+
solution = study.solve();
315+
solution.write(sprintf('example3DWalking_%s_solution.sto', studyName));
316+
317+
% Print the model.
318+
modelSolution = modelProcessor.process();
319+
modelSolution.initSystem();
320+
modelSolution.print(sprintf('example3DWalking_%s_model.osim', studyName));
321+
322+
% Extract the ground reaction forces.
323+
externalForcesTableFlat = opensimMoco.createExternalLoadsTableForGait(...
324+
modelSolution, solution, contactForcesRight, contactForcesLeft);
325+
STOFileAdapter.write(externalForcesTableFlat, ...
326+
sprintf('example3DWalking_%s_ground_reactions.sto', studyName));
327+
328+
% Visualize the solution.
329+
study.visualize(solution);
330+
end

‎Bindings/Java/Matlab/examples/Moco/example3DWalking/exampleMocoInverse.m‎

Lines changed: 21 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -55,6 +55,13 @@ function solveMocoInverse()
5555
% parameters.
5656
modelProcessor = ModelProcessor('subject_walk_scaled.osim');
5757
modelProcessor.append(ModOpAddExternalLoads('grf_walk.xml'));
58+
% Replace the PinJoints representing the model's toes with WeldJoints.
59+
jointsToWeld = StdVectorString();
60+
jointsToWeld.add("mtp_r");
61+
jointsToWeld.add("mtp_l");
62+
modelProcessor.append(ModOpReplaceJointsWithWelds(jointsToWeld));
63+
% Add CoordinateActuators to the pelvis coordinates.
64+
modelProcessor.append(ModOpAddResiduals(250.0, 50.0, 1.0));
5865
modelProcessor.append(ModOpIgnoreTendonCompliance());
5966
modelProcessor.append(ModOpReplaceMusclesWithDeGrooteFregly2016());
6067
% Only valid for DeGrooteFregly2016Muscles.
@@ -109,6 +116,13 @@ function solveMocoInverseWithEMG()
109116
inverse = MocoInverse();
110117
modelProcessor = ModelProcessor('subject_walk_scaled.osim');
111118
modelProcessor.append(ModOpAddExternalLoads('grf_walk.xml'));
119+
% Replace the PinJoints representing the model's toes with WeldJoints.
120+
jointsToWeld = StdVectorString();
121+
jointsToWeld.add("mtp_r");
122+
jointsToWeld.add("mtp_l");
123+
modelProcessor.append(ModOpReplaceJointsWithWelds(jointsToWeld));
124+
% Add CoordinateActuators to the pelvis coordinates.
125+
modelProcessor.append(ModOpAddResiduals(250.0, 50.0, 1.0));
112126
modelProcessor.append(ModOpIgnoreTendonCompliance());
113127
modelProcessor.append(ModOpReplaceMusclesWithDeGrooteFregly2016());
114128
modelProcessor.append(ModOpIgnorePassiveFiberForcesDGF());
@@ -196,6 +210,13 @@ function solveMocoInverseWithSynergies(numSynergies)
196210
% simplify the problem given the muscle synergy controllers.
197211
modelProcessor = ModelProcessor('subject_walk_scaled.osim');
198212
modelProcessor.append(ModOpAddExternalLoads('grf_walk.xml'));
213+
% Replace the PinJoints representing the model's toes with WeldJoints.
214+
jointsToWeld = StdVectorString();
215+
jointsToWeld.add("mtp_r");
216+
jointsToWeld.add("mtp_l");
217+
modelProcessor.append(ModOpReplaceJointsWithWelds(jointsToWeld));
218+
% Add CoordinateActuators to the pelvis coordinates.
219+
modelProcessor.append(ModOpAddResiduals(250.0, 50.0, 1.0));
199220
modelProcessor.append(ModOpIgnoreTendonCompliance());
200221
modelProcessor.append(ModOpIgnoreActivationDynamics());
201222
modelProcessor.append(ModOpReplaceMusclesWithDeGrooteFregly2016());

0 commit comments

Comments
 (0)