Matlab version of example2DWalking

[Brian Umberger]

First, I would like to thank Chris Dembia, Nick Bianco, and the rest of the team for all of the work they have put into Moco to bring optimal control techniques to OpenSim.

When I started digging into Moco I noticed one of the examples I was most interested in, prediction of human walking by Antoine Falisse, was only available in C++ (see: Resources\Code\CPP\example2DWalking\example2DWalking.cpp). With a little help from Chris I have converted that example to Matlab and am sharing it here. This Matlab script works with the model file (2D_gait.osim) and reference data (referenceCoordinates.sto) in the C++ example2DWalking directory.

I hope people find this useful, and hopefully it encourages others to share examples and tips on the forum.

Best regards,

Brian

Code: Select all

% -------------------------------------------------------------------------- *
% OpenSim Moco: example2DWalking.m                                           %
% -------------------------------------------------------------------------- %
% Copyright (c) 2017-19 Stanford University and the Authors                  %
%                                                                            %
% Author(s): Brian Umberger                                                  %
%                                                                            %
% Licensed under the Apache License, Version 2.0 (the "License"); you may    %
% not use this file except in compliance with the License. You may obtain a  %
% copy of the License at http:%www.apache.org/licenses/LICENSE-2.0   m       %
%                                                                            %
% Unless required by applicable law or agreed to in writing, software        %
% distributed under the License is distributed on an "AS IS" BASIS,          %
% WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   %
% See the License for the specific language governing permissions and        %
% limitations under the License.                                             %
% -------------------------------------------------------------------------- %/

% This is a Matlab implementation of an example optimal control
% problem (2-D walking) orginally created in C++ by Antoine Falisse
% (see: example2DWalking.cpp).
%
% This example features two different optimal control problems:
%  - The first problem is a tracking simulation of walking.
%  - The second problem is a predictive simulation of walking.
%
% The code is inspired from Falisse A, Serrancoli G, Dembia C, Gillis J,
% De Groote F: Algorithmic differentiation improves the computational
% efficiency of OpenSim-based trajectory optimization of human movement.
% PLOS One, 2019.
%
% Model
% -----
% The model described in the file '2D_gait.osim' is a modified version of
% the 'gait10dof18musc.osim' available within OpenSim. We replaced the
% moving knee flexion axis by a fixed flexion axis, replaced the
% Millard2012EquilibriumMuscles by DeGrooteFregly2016Muscles, and added
% SmoothSphereHalfSpaceForces (two contact spheres per foot) to model the
% contact interactions between the feet and the ground.
%
% Data
% ----
% The coordinate data included in the 'referenceCoordinates.sto' comes from
% predictive simulations generated in Falisse et al. 2019.

clear

% Load the Moco libraries
 import org.opensim.modeling.*;

% ---------------------------------------------------------------------------
% Set-up a coordinate tracking problem where the goal is to minimize the
% difference between provided and simulated coordinate values and speeds,
% as well as to minimize an effort cost (squared controls). The provided data
% represents half a gait cycle. Endpoint constraints enforce periodicity of
% the coordinate values (except for pelvis tx) and speeds, coordinate
% actuator controls, and muscle activations.


% Define the optimal control problem
% ==================================
track = MocoTrack();
track.setName("gaitTracking");

% Reference data for tracking problem
tableProcessor = TableProcessor("referenceCoordinates.sto");
tableProcessor.append(TabOpLowPassFilter(6));

modelProcessor = ModelProcessor("2D_gait.osim"); 
track.setModel(modelProcessor);
track.setStatesReference(tableProcessor);
track.set_states_global_tracking_weight(10.0);
track.set_allow_unused_references(true);
track.set_track_reference_position_derivatives(true);
track.set_apply_tracked_states_to_guess(true);
track.set_initial_time(0.0);
track.set_final_time(0.47008941);
study = track.initialize();
problem = study.updProblem();


% Goals
% =====

% Symmetry (to permit simulating only one step)
symmetryGoal = MocoPeriodicityGoal('symmetryGoal');
problem.addGoal(symmetryGoal);
model = modelProcessor.process();
model.initSystem();

% Symmetric coordinate values (except for pelvis_tx) and speeds
for i = 1:model.getNumStateVariables()
   currentStateName = string(model.getStateVariableNames().getitem(i-1));
   if startsWith(currentStateName , '/jointset')
      if contains(currentStateName,'_r')
          symmetryGoal.addStatePair(MocoPeriodicityGoalPair(currentStateName, ...
                         regexprep(currentStateName,'_r','_l') ) );
      end
      if contains(currentStateName,'_l')
          symmetryGoal.addStatePair(MocoPeriodicityGoalPair(currentStateName, ...
                         regexprep(currentStateName,'_l','_r') ) );
      end
      if (~contains(currentStateName,'_r') && ~contains(currentStateName,'_l') && ...
          ~contains(currentStateName,'pelvis_tx/value')  && ...
          ~contains(currentStateName,'/activation'))
         symmetryGoal.addStatePair(MocoPeriodicityGoalPair(currentStateName));
      end
   end
end

% Symmetric muscle activations
for i = 1:model.getNumStateVariables()
   currentStateName = string(model.getStateVariableNames().getitem(i-1));
   if endsWith(currentStateName,'/activation')
      if contains(currentStateName,'_r')
          symmetryGoal.addStatePair(MocoPeriodicityGoalPair(currentStateName, ...
                         regexprep(currentStateName,'_r','_l')));
      end
      if contains(currentStateName,'_l')
          symmetryGoal.addStatePair(MocoPeriodicityGoalPair(currentStateName, ...
                         regexprep(currentStateName,'_l','_r')));
      end

   end
end

% Example of symmetric coordinate actuator controls
symmetryGoal.addControlPair(MocoPeriodicityGoalPair('/lumbarAct'));

% Get a reference to the MocoControlGoal that is added to every MocoTrack
% problem by default and change the weight
effort = MocoControlGoal.safeDownCast(problem.updGoal("control_effort"));
effort.setWeight(10);


% Bounds
% ======
problem.setStateInfo("/jointset/groundPelvis/pelvis_tilt/value", [-20*pi/180, -10*pi/180]);
problem.setStateInfo("/jointset/groundPelvis/pelvis_tx/value", [0, 1]);
problem.setStateInfo("/jointset/groundPelvis/pelvis_ty/value", [0.75, 1.25]);
problem.setStateInfo("/jointset/hip_l/hip_flexion_l/value", [-10*pi/180, 60*pi/180]);
problem.setStateInfo("/jointset/hip_r/hip_flexion_r/value", [-10*pi/180, 60*pi/180]);
problem.setStateInfo("/jointset/knee_l/knee_angle_l/value", [-50*pi/180, 0]);
problem.setStateInfo("/jointset/knee_r/knee_angle_r/value", [-50*pi/180, 0]);
problem.setStateInfo("/jointset/ankle_l/ankle_angle_l/value", [-15*pi/180, 25*pi/180]);
problem.setStateInfo("/jointset/ankle_r/ankle_angle_r/value", [-15*pi/180, 25*pi/180]);
problem.setStateInfo("/jointset/lumbar/lumbar/value", [0, 20*pi/180]);

% Solve the problem
% =================
gaitTrackingSolution = study.solve();

% Create a full stride from the periodic single step solution
fullStride = opensimMoco.createPeriodicTrajectory(gaitTrackingSolution);
fullStride.write('gaitTracking_solution_fullStride.sto');

% Uncomment next line to visualize the result
% study.visualize(fullStride);

% Extract ground reaction forces
% ==============================
contactSpheres_r = StdVectorString();
contactSpheres_l = StdVectorString();
contactSpheres_r.add("contactSphereHeel_r");
contactSpheres_r.add("contactSphereFront_r");
contactSpheres_l.add("contactSphereHeel_l");
contactSpheres_l.add("contactSphereFront_l");

externalForcesTableFlat = opensimMoco.createExternalLoadsTableForGait(model, ...
                                 fullStride,contactSpheres_r,contactSpheres_l);
opensimMoco.writeTableToFile(externalForcesTableFlat, ...
                             'gaitTracking_solution_fullStride_GRF.sto');



%------------------------------------------------------------------------
% Set-up a gait prediction problem where the goal is to minimize effort
% (squared controls) over distance traveled while enforcing symmetry of 
% the walking cycle and a prescribed average gait speed through endpoint
% constraints. The solution of the coordinate tracking problem is passed
% as an input argument and used as an initial guess for the prediction.


% Define the optimal control problem
% ==================================
study = MocoStudy();
study.setName("gaitPrediction");

problem = study.updProblem();
modelProcessor = ModelProcessor('2D_gait.osim');
problem.setModelProcessor(modelProcessor);


% Goals
% ====

% Symmetry (to permit simulating only one step)
symmetryGoal = MocoPeriodicityGoal('symmetryGoal');
problem.addGoal(symmetryGoal);
model = modelProcessor.process();
model.initSystem();

% Symmetric coordinate values (except for pelvis_tx) and speeds
for i = 1:model.getNumStateVariables()
   currentStateName = string(model.getStateVariableNames().getitem(i-1));
   if startsWith(currentStateName , '/jointset')
      if contains(currentStateName,'_r')
          symmetryGoal.addStatePair(MocoPeriodicityGoalPair(currentStateName, ...
                         regexprep(currentStateName,'_r','_l') ) );
      end
      if contains(currentStateName,'_l')
          symmetryGoal.addStatePair(MocoPeriodicityGoalPair(currentStateName, ...
                         regexprep(currentStateName,'_l','_r') ) );
      end
      if (~contains(currentStateName,'_r') && ~contains(currentStateName,'_l') && ...
          ~contains(currentStateName,'pelvis_tx/value')  && ...
          ~contains(currentStateName,'/activation'))
         symmetryGoal.addStatePair(MocoPeriodicityGoalPair(currentStateName));
      end
   end
end

% Symmetric muscle activations
for i = 1:model.getNumStateVariables()
   currentStateName = string(model.getStateVariableNames().getitem(i-1));
   if endsWith(currentStateName,'/activation')
      if contains(currentStateName,'_r')
          symmetryGoal.addStatePair(MocoPeriodicityGoalPair(currentStateName, ...
                         regexprep(currentStateName,'_r','_l')));
      end
      if contains(currentStateName,'_l')
          symmetryGoal.addStatePair(MocoPeriodicityGoalPair(currentStateName, ...
                         regexprep(currentStateName,'_l','_r')));
      end

   end
end

% Example of symmetric coordinate actuator controls
symmetryGoal.addControlPair(MocoPeriodicityGoalPair('/lumbarAct'));

% Prescribed average gait speed
speedGoal = MocoAverageSpeedGoal('speed');
problem.addGoal(speedGoal);
speedGoal.set_desired_average_speed(1.2);

% Effort over distance
effortGoal = MocoControlGoal('effort', 10);
problem.addGoal(effortGoal);
effortGoal.setExponent(3);
effortGoal.setDivideByDisplacement(true);


% Bounds
% ======
problem.setTimeBounds(0, [0.4, 0.6]);
problem.setStateInfo("/jointset/groundPelvis/pelvis_tilt/value", [-20*pi/180, -10*pi/180]);
problem.setStateInfo("/jointset/groundPelvis/pelvis_tx/value", [0, 1]);
problem.setStateInfo("/jointset/groundPelvis/pelvis_ty/value", [0.75, 1.25]);
problem.setStateInfo("/jointset/hip_l/hip_flexion_l/value", [-10*pi/180, 60*pi/180]);
problem.setStateInfo("/jointset/hip_r/hip_flexion_r/value", [-10*pi/180, 60*pi/180]);
problem.setStateInfo("/jointset/knee_l/knee_angle_l/value", [-50*pi/180, 0]);
problem.setStateInfo("/jointset/knee_r/knee_angle_r/value", [-50*pi/180, 0]);
problem.setStateInfo("/jointset/ankle_l/ankle_angle_l/value", [-15*pi/180, 25*pi/180]);
problem.setStateInfo("/jointset/ankle_r/ankle_angle_r/value", [-15*pi/180, 25*pi/180]);
problem.setStateInfo("/jointset/lumbar/lumbar/value", [0, 20*pi/180]);
    

% Configure the solver
% ====================
solver = study.initCasADiSolver();
solver.set_num_mesh_intervals(50);
solver.set_verbosity(2);
solver.set_optim_solver("ipopt");
solver.set_optim_convergence_tolerance(1e-4);
solver.set_optim_constraint_tolerance(1e-4);
solver.set_optim_max_iterations(1000);
solver.setGuess(gaitTrackingSolution); % Use tracking solution as initial guess


% Solve problem
% =============
gaitPredictionSolution = study.solve();

% Create a full stride from the periodic single step solution
fullStride = opensimMoco.createPeriodicTrajectory(gaitPredictionSolution);
fullStride.write('gaitPrediction_solution_fullStride.sto');

% Uncomment next line to visualize the result
% study.visualize(gaitTrackingSolution);


% Extract ground reaction forces
% ==============================
contactSpheres_r = StdVectorString();
contactSpheres_l = StdVectorString();
contactSpheres_r.add("contactSphereHeel_r");
contactSpheres_r.add("contactSphereFront_r");
contactSpheres_l.add("contactSphereHeel_l");
contactSpheres_l.add("contactSphereFront_l");

externalForcesTableFlat = opensimMoco.createExternalLoadsTableForGait(model, ...
                                 fullStride,contactSpheres_r,contactSpheres_l);
opensimMoco.writeTableToFile(externalForcesTableFlat, ...
                             'gaitPrediction_solution_fullStride_GRF.sto');

[Christopher Dembia]

Thank you for sharing this code, Brian

[Karthick Ganesan]

Dear Chris Dembia,
I thank Prof. Umberger for sharing the code. I managed to convert the C++ code to matlab mostly. But this is best written and of great help.
I have few queries.
1. If tendon compliance is included in the model how to modify this code to handle that? I tried but optimizer failed.
2. To use path length approximation for muscle paths, is it sufficient that this property is set as true in the model xml file or should we use the below command as in C++ example code?
modelProcessor.append(ModOpSetPathLengthApproximation(true))
3. Does this code solve the optimization problem with implicit or explicit dynamics?
I will highly appreciate if you could please clarify these doubts.

Thanks,
Karthick.

[Christopher Dembia]

Hi Karthick. Thank you for trying Moco.

1. It is worthwhile consider whether your research question requires tendon compliance. If so, consider enabling tendon compliance for only muscles with long tendons, such as the plantarflexors. Lastly, you likely need to alter your initial guess so that the tendon force is not too large. See `solver.createGuess()` and `solver.setGuess()`. We hope that future versions of Moco will provide an option to set the initial guess for fiber length according to a fiber equilibrum calculation.

2. We have removed the "path length approximation" feature in version 0.2.0 and we do not have a plan for reintroducing the feature. While it does speed up the optimization process, the existing path calculations seem to work well enough.

3. By default, MocoStudy uses explicit multibody dynamics. The documentation (https://opensim-org.github.io/opensim-moco-site/docs/) provides more detail. You can determine this from a MocoSolution: implicit multibody dynamics produces columns for generalized accelerations, while explicit multibody dynamics does not.

[Brian Umberger]

Hello Karthick,

I have solved this example problem using compliant tendons. I do agree with Chris that in some cases using activation dynamics alone is sufficient. For example, De Groote et al. (Ann Biomed Eng, 2016) found that the forces predicted in walking using rigid and compliant tendons were nearly the same. So if you only need the forces the simple approach works well. It may only be worth the additional complexity if you really need to know the internal muscle-tendon dynamics, such as for predicting metabolic cost.

That said, if anyone does want to try the example problem with full muscle-tendon dynamics, first you will need to change ignore_tendon_compliance for the muscles to false. As Chris noted, the initial guess will need to be modified to ensure the tendon forces in the guess are not too large. To set the normalized_tendon_force in the guess for all muscles to 0.2:

Code: Select all

solver = MocoCasADiSolver.safeDownCast(study.updSolver());
solver.resetProblem(problem);
guess = solver.getGuess();
numRows = guess.getNumTimes();

StateNames = model.getStateVariableNames();
for i = 1:model.getNumStateVariables()
   currentStateName = string(StateNames.getitem(i-1));
   if contains(currentStateName,'normalized_tendon_force')
      guess.setState(currentStateName, linspace(0.2,0.2,numRows));
   end
end
solver.setGuess(guess);

You can optionally change the bounds on normalized_tendon_force and enforce tendon force periodicity using the original code as an example to work from. I found that solving the problem with full muscle-tendon dynamics required tighter tolerances and a finer temporal mesh in order to get a good solution. On my laptop these changes substantially increased the run time (~50 min vs. <5 min for the tracking problem).

Best,
Brian

[Karthick Ganesan]

Dear Chris Dembia and Brian Umberger,
Thank you very much for your suggestions and the code. I am interested in the metabolic cost. So I think I will have to consider tendon compliance.
I followed your suggestions and set the normalized_tendon_force initial guess as 0.2. But the tracking problem is not converging. It reached the default maximum iterations of 3000. I tried with only compliant plantarflexor tendons and also with all tendons set as compliant. I tried with and without enforcing tendon force periodicity. I am not sure what is going wrong.


Thanks & Regards,
Karthick.

[Brian Umberger]

Here is code that solves the tracking problem with compliant tendons. This code assumes a model "2D_gait_complianttendon.osim" where ignore_tendon_compliance is set to false for all muscles (attached - could also be done programmatically). This is not thoroughly tested and could no doubt be improved, but it seems to work. It converged for me in ~700 iterations on a fine mesh to a tight tolerance (inf_pr = 5.36e-09) and the result looks reasonable.

Best,
Brian

Code: Select all

% -------------------------------------------------------------------------- *
% OpenSim Moco: example2DWalking_ComplTendon.m                               %
% -------------------------------------------------------------------------- %
 
clear

% Load the Moco libraries
import org.opensim.modeling.*;


% Define the optimal control problem
% ==================================
track = MocoTrack();
track.setName("gaitTracking");


% Reference data for tracking problem
tableProcessor = TableProcessor("referenceCoordinates.sto");
tableProcessor.append(TabOpLowPassFilter(6));

modelProcessor = ModelProcessor("2D_gait_complianttendon.osim");
modelProcessor.append(ModOpIgnorePassiveFiberForcesDGF());
modelProcessor.append(ModOpScaleActiveFiberForceCurveWidthDGF(1.5));

track.setModel(modelProcessor);
track.setStatesReference(tableProcessor);
track.set_states_global_tracking_weight(10.0);
track.set_allow_unused_references(true);
track.set_track_reference_position_derivatives(true);
track.set_apply_tracked_states_to_guess(true);
track.set_initial_time(0.0);
track.set_final_time(0.47008941);
study = track.initialize();

model = modelProcessor.process();
model.initSystem();

problem = study.updProblem();


% Goals
% =====

% Symmetry (to permit simulating only one step)
symmetryGoal = MocoPeriodicityGoal('symmetryGoal');
problem.addGoal(symmetryGoal);
model = modelProcessor.process();
model.initSystem();

% Symmetric coordinate values (except for pelvis_tx) and speeds
for i = 1:model.getNumStateVariables()
   currentStateName = string(model.getStateVariableNames().getitem(i-1));
   if startsWith(currentStateName , '/jointset')
      if contains(currentStateName,'_r')
          symmetryGoal.addStatePair(MocoPeriodicityGoalPair(currentStateName, ...
                         regexprep(currentStateName,'_r','_l') ) );
      end
      if contains(currentStateName,'_l')
          symmetryGoal.addStatePair(MocoPeriodicityGoalPair(currentStateName, ...
                         regexprep(currentStateName,'_l','_r') ) );
      end
      if (~contains(currentStateName,'_r') && ~contains(currentStateName,'_l') && ...
          ~contains(currentStateName,'pelvis_tx/value')  && ...
          ~contains(currentStateName,'/activation'))
         symmetryGoal.addStatePair(MocoPeriodicityGoalPair(currentStateName));
      end
   end
end

% Symmetric muscle activations
for i = 1:model.getNumStateVariables()
   currentStateName = string(model.getStateVariableNames().getitem(i-1));
   if endsWith(currentStateName,'/activation')
      if contains(currentStateName,'_r')
          symmetryGoal.addStatePair(MocoPeriodicityGoalPair(currentStateName, ...
                         regexprep(currentStateName,'_r','_l')));
      end
      if contains(currentStateName,'_l')
          symmetryGoal.addStatePair(MocoPeriodicityGoalPair(currentStateName, ...
                         regexprep(currentStateName,'_l','_r')));
      end

   end
end

% Symmetric coordinate actuator controls
symmetryGoal.addControlPair(MocoPeriodicityGoalPair('/lumbarAct'));

% Get a reference to the MocoControlGoal that is added to every MocoTrack
% problem by default and change the weight
effort = MocoControlGoal.safeDownCast(problem.updGoal("control_effort"));
effort.setWeight(5);


% Bounds
% ======
problem.setStateInfo("/jointset/groundPelvis/pelvis_tilt/value", [-20*pi/180, -10*pi/180]);
problem.setStateInfo("/jointset/groundPelvis/pelvis_tx/value", [0, 1]);
problem.setStateInfo("/jointset/groundPelvis/pelvis_ty/value", [0.75, 1.25]);
problem.setStateInfo("/jointset/hip_l/hip_flexion_l/value", [-10*pi/180, 60*pi/180]);
problem.setStateInfo("/jointset/hip_r/hip_flexion_r/value", [-10*pi/180, 60*pi/180]);
problem.setStateInfo("/jointset/knee_l/knee_angle_l/value", [-50*pi/180, 0]);
problem.setStateInfo("/jointset/knee_r/knee_angle_r/value", [-50*pi/180, 0]);
problem.setStateInfo("/jointset/ankle_l/ankle_angle_l/value", [-15*pi/180, 25*pi/180]);
problem.setStateInfo("/jointset/ankle_r/ankle_angle_r/value", [-15*pi/180, 25*pi/180]);
problem.setStateInfo("/jointset/lumbar/lumbar/value", [0, 20*pi/180]);

problem.setStateInfoPattern('/forceset/.*/normalized_tendon_force', [0, 1.5], [], []);
problem.setStateInfoPattern('/forceset/.*/activation',   [0.01, 1.0], [], []);


% Set the normalized tendon forces in guess
% =========================================
solver = MocoCasADiSolver.safeDownCast(study.updSolver());
solver.resetProblem(problem);
guess = solver.getGuess();
numRows = guess.getNumTimes();

StateNames = model.getStateVariableNames();
for i = 1:model.getNumStateVariables()
   currentStateName = string(StateNames.getitem(i-1));
   if contains(currentStateName,'normalized_tendon_force')
      guess.setState(currentStateName, linspace(0.2,0.2,numRows));
   end
end
solver.setGuess(guess);

% Set some options for the solver and then solve the problem
% ==========================================================
solver.set_optim_max_iterations(1000);
solver.set_num_mesh_intervals(100);
solver.set_optim_constraint_tolerance(1e-5);
solver.set_optim_convergence_tolerance(1e-5);

gaitTrackingSolution = study.solve();


% Create a full stride from the periodic single step solution
fullStride = opensimMoco.createPeriodicTrajectory(gaitTrackingSolution);
fullStride.write('gaitTracking_solution_fullStride.sto');

% Uncomment next line to visualize the result
% study.visualize(fullStride);

[Aaron Fox]

First off, thanks to Brian and Chris for providing a Matlab variant of this example. I was able to work through the example fine and have everything working.

My question potentially relates more to Antoine's original work (I should probably go back and read the paper again) - but does anyone have an idea of what sort of gait speeds this example would hold up to? I currently have an optimisation running in the background with 2.2m/s as the goal speed, so I'll see how this ends up - but are there any obvious foreseeable problems of extracting this example out to jogging and running speeds?

Thanks,

Aaron

[Brian Umberger]

Hi Aaron,

Your post is a few days old so maybe you have already worked through this, but I did try faster speeds for the predictive case when I was first testing this out. I solved a sequence of problems with progressively faster target speeds and easily got the model to run at moderate speeds (I forget exactly, maybe 4 m/s).

The main considerations are: 1) some of the bounds will need to be relaxed to permit greater joint range of motion, and 2) the model has a simplified knee joint that may not give accurate muscle lengths and moment arms at more flexed angles. The latter issue is specific to that model and is not a limitation of the optimal control approach, which seemed to work well for varying speed across a wide range (though I did not try to find the max speed).

Best,
Brian

[Aaron Fox]

HI Brian,

Thanks for the response.

I agree with what you've mentioned - the bounds (and potentially the initial guess as well) remaining unchanged did make the solution different to what you would expect. For those interested, if unchanged (and given a moderate-ish speed) the model will effectively perform the faster task still with a walking like movement.

Aaron