Matlab version of example2DWalking
Posted: Fri Dec 06, 2019 12:09 pm
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
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');