OpenSim Moco  0.2.0
example2DWalking.cpp

This example conducts a 2-D prediction of walking, employing a MocoPerodicityGoal.

/* -------------------------------------------------------------------------- *
* OpenSim Moco: example2DWalking.cpp *
* -------------------------------------------------------------------------- *
* Copyright (c) 2017-19 Stanford University and the Authors *
* *
* Author(s): Antoine Falisse *
* *
* 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 *
* *
* 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. *
* -------------------------------------------------------------------------- */
#include <Moco/MocoGoal/MocoOutputGoal.h>
#include <Moco/osimMoco.h>
using namespace OpenSim;
// Set 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.
MocoSolution gaitTracking() {
using SimTK::Pi;
MocoTrack track;
track.setName("gaitTracking");
// Define the optimal control problem.
// ===================================
ModelProcessor modelprocessor = ModelProcessor("2D_gait.osim");
track.setModel(modelprocessor);
track.setStatesReference(
TableProcessor("referenceCoordinates.sto") | TabOpLowPassFilter(6));
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);
MocoStudy study = track.initialize();
MocoProblem& problem = study.updProblem();
// Goals.
// =====
// Symmetry.
auto* symmetryGoal = problem.addGoal<MocoPeriodicityGoal>("symmetryGoal");
Model model = modelprocessor.process();
model.initSystem();
// Symmetric coordinate values (except for pelvis_tx) and speeds.
for (const auto& coord : model.getComponentList<Coordinate>()) {
if (endsWith(coord.getName(), "_r")) {
symmetryGoal->addStatePair({coord.getStateVariableNames()[0],
std::regex_replace(coord.getStateVariableNames()[0],
std::regex("_r"), "_l")});
symmetryGoal->addStatePair({coord.getStateVariableNames()[1],
std::regex_replace(coord.getStateVariableNames()[1],
std::regex("_r"), "_l")});
}
if (endsWith(coord.getName(), "_l")) {
symmetryGoal->addStatePair({coord.getStateVariableNames()[0],
std::regex_replace(coord.getStateVariableNames()[0],
std::regex("_l"), "_r")});
symmetryGoal->addStatePair({coord.getStateVariableNames()[1],
std::regex_replace(coord.getStateVariableNames()[1],
std::regex("_l"), "_r")});
}
if (!endsWith(coord.getName(), "_l") &&
!endsWith(coord.getName(), "_r") &&
!endsWith(coord.getName(), "_tx")) {
symmetryGoal->addStatePair({coord.getStateVariableNames()[0],
coord.getStateVariableNames()[0]});
symmetryGoal->addStatePair({coord.getStateVariableNames()[1],
coord.getStateVariableNames()[1]});
}
}
symmetryGoal->addStatePair({"/jointset/groundPelvis/pelvis_tx/speed"});
// Symmetric coordinate actuator controls.
symmetryGoal->addControlPair({"/lumbarAct"});
// Symmetric muscle activations.
for (const auto& muscle : model.getComponentList<Muscle>()) {
if (endsWith(muscle.getName(), "_r")) {
symmetryGoal->addStatePair({muscle.getStateVariableNames()[0],
std::regex_replace(muscle.getStateVariableNames()[0],
std::regex("_r"), "_l")});
}
if (endsWith(muscle.getName(), "_l")) {
symmetryGoal->addStatePair({muscle.getStateVariableNames()[0],
std::regex_replace(muscle.getStateVariableNames()[0],
std::regex("_l"), "_r")});
}
}
// Effort. Get a reference to the MocoControlGoal that is added to every
// MocoTrack problem by default.
MocoControlGoal& effort =
dynamic_cast<MocoControlGoal&>(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});
// Configure the solver.
// =====================
MocoCasADiSolver& solver = study.updSolver<MocoCasADiSolver>();
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);
// Solve problem.
// ==============
MocoSolution solution = study.solve();
auto full = createPeriodicTrajectory(solution);
full.write("gaitTracking_solution_fullcycle.sto");
// moco.visualize(solution);
return solution;
}
// Set 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.
void gaitPrediction(const MocoSolution& gaitTrackingSolution) {
using SimTK::Pi;
MocoStudy study;
study.setName("gaitPrediction");
// Define the optimal control problem.
// ===================================
MocoProblem& problem = study.updProblem();
ModelProcessor modelprocessor =
ModelProcessor("2D_gait.osim");
problem.setModelProcessor(modelprocessor);
// Goals.
// =====
// Symmetry.
auto* symmetryGoal = problem.addGoal<MocoPeriodicityGoal>("symmetryGoal");
Model model = modelprocessor.process();
model.initSystem();
// Symmetric coordinate values (except for pelvis_tx) and speeds.
for (const auto& coord : model.getComponentList<Coordinate>()) {
if (endsWith(coord.getName(), "_r")) {
symmetryGoal->addStatePair({coord.getStateVariableNames()[0],
std::regex_replace(coord.getStateVariableNames()[0],
std::regex("_r"), "_l")});
symmetryGoal->addStatePair({coord.getStateVariableNames()[1],
std::regex_replace(coord.getStateVariableNames()[1],
std::regex("_r"), "_l")});
}
if (endsWith(coord.getName(), "_l")) {
symmetryGoal->addStatePair({coord.getStateVariableNames()[0],
std::regex_replace(coord.getStateVariableNames()[0],
std::regex("_l"), "_r")});
symmetryGoal->addStatePair({coord.getStateVariableNames()[1],
std::regex_replace(coord.getStateVariableNames()[1],
std::regex("_l"), "_r")});
}
if (!endsWith(coord.getName(), "_l") &&
!endsWith(coord.getName(), "_r") &&
!endsWith(coord.getName(), "_tx")) {
symmetryGoal->addStatePair({coord.getStateVariableNames()[0],
coord.getStateVariableNames()[0]});
symmetryGoal->addStatePair({coord.getStateVariableNames()[1],
coord.getStateVariableNames()[1]});
}
}
symmetryGoal->addStatePair({"/jointset/groundPelvis/pelvis_tx/speed"});
// Symmetric coordinate actuator controls.
symmetryGoal->addControlPair({"/lumbarAct"});
// Symmetric muscle activations.
for (const auto& muscle : model.getComponentList<Muscle>()) {
if (endsWith(muscle.getName(), "_r")) {
symmetryGoal->addStatePair({muscle.getStateVariableNames()[0],
std::regex_replace(muscle.getStateVariableNames()[0],
std::regex("_r"), "_l")});
}
if (endsWith(muscle.getName(), "_l")) {
symmetryGoal->addStatePair({muscle.getStateVariableNames()[0],
std::regex_replace(muscle.getStateVariableNames()[0],
std::regex("_l"), "_r")});
}
}
// Prescribed average gait speed.
auto* speedGoal = problem.addGoal<MocoAverageSpeedGoal>("speed");
speedGoal->set_desired_average_speed(1.2);
// Effort over distance.
auto* effortGoal = problem.addGoal<MocoControlGoal>("effort", 10);
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.
// =====================
auto& 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);
// Use the solution from the tracking simulation as initial guess.
solver.setGuess(gaitTrackingSolution);
// Solve problem.
// ==============
MocoSolution solution = study.solve();
auto full = createPeriodicTrajectory(solution);
full.write("gaitPrediction_solution_fullcycle.sto");
// Extract ground reaction forces.
// ===============================
std::vector<std::string> contactSpheres_r;
std::vector<std::string> contactSpheres_l;
contactSpheres_r.push_back("contactSphereHeel_r");
contactSpheres_r.push_back("contactSphereFront_r");
contactSpheres_l.push_back("contactSphereHeel_l");
contactSpheres_l.push_back("contactSphereFront_l");
TimeSeriesTable externalForcesTableFlat = createExternalLoadsTableForGait(
model, full, contactSpheres_r, contactSpheres_l);
writeTableToFile(externalForcesTableFlat,
"gaitPrediction_solutionGRF_fullcycle.sto");
study.visualize(full);
}
int main() {
try {
const MocoSolution gaitTrackingSolution = gaitTracking();
gaitPrediction(gaitTrackingSolution);
} catch (const std::exception& e) { std::cout << e.what() << std::endl; }
return EXIT_SUCCESS;
}
OpenSim::MocoGoal::setWeight
void setWeight(double weight)
In cost mode, the goal is multiplied by this weight.
Definition: MocoGoal.h:70
OpenSim::MocoStudy::solve
MocoSolution solve() const
Solve the provided MocoProblem using the provided MocoSolver, and obtain the solution to the problem.
OpenSim::MocoStudy::updSolver
MocoSolver & updSolver()
Access the solver.
OpenSim::MocoProblem::setStateInfo
void setStateInfo(const std::string &name, const MocoBounds &, const MocoInitialBounds &={}, const MocoFinalBounds &={})
Set bounds for a state variable for phase 0.
OpenSim::ModelProcessor::process
Model process(const std::string &relativeToDirectory={}) const
Process and obtain the model.
Definition: ModelProcessor.h:109
OpenSim::writeTableToFile
OSIMMOCO_API void writeTableToFile(const TimeSeriesTable &, const std::string &)
Write a single TimeSeriesTable to a file, using the FileAdapter associated with the provided file ext...
OpenSim::MocoCasADiSolver
This solver uses the CasADi library (https://casadi.org) to convert the MocoProblem into a generic no...
Definition: MocoCasADiSolver.h:110
OpenSim::MocoTrack
Definition: MocoTrack.h:157
OpenSim::ModelProcessor
This class describes a workflow for processing a Model using ModelOperators.
Definition: ModelProcessor.h:47
OpenSim::createExternalLoadsTableForGait
OSIMMOCO_API TimeSeriesTable createExternalLoadsTableForGait(Model model, const MocoTrajectory &trajectory, const std::vector< std::string > &forceNamesRightFoot, const std::vector< std::string > &forceNamesLeftFoot)
Obtain the ground reaction forces, centers of pressure, and torques resulting from Force elements (e....
OpenSim::MocoStudy::updProblem
MocoProblem & updProblem()
If using this method in C++, make sure to include the "&" in the return type; otherwise,...
OpenSim::MocoProblem::setTimeBounds
void setTimeBounds(const MocoInitialBounds &, const MocoFinalBounds &)
Set time bounds for phase 0.
OpenSim::endsWith
bool endsWith(const std::string &string, const std::string &ending)
Determine if string ends with the substring ending.
Definition: MocoUtilities.h:83
OpenSim::MocoProblem::addGoal
MocoGoalType * addGoal(Args &&... args)
Add a goal for phase 0.
Definition: MocoProblem.h:487
OpenSim::MocoStudy::visualize
void visualize(const MocoTrajectory &it) const
Interactively visualize a trajectory using the simbody-visualizer.
OpenSim::MocoStudy
The top-level class for solving a custom optimal control problem.
Definition: MocoStudy.h:71
OpenSim
The utilities in this file are categorized as follows:
Definition: About.h:24
OpenSim::MocoTrack::setStatesReference
void setStatesReference(TableProcessor states)
Set the states reference TableProcessor.
Definition: MocoTrack.h:242
OpenSim::MocoSolution
Return type for MocoStudy::solve().
Definition: MocoTrajectory.h:740
OpenSim::MocoProblem::setModelProcessor
void setModelProcessor(ModelProcessor model)
Set a model processor for phase 0.
OpenSim::createPeriodicTrajectory
OSIMMOCO_API MocoTrajectory createPeriodicTrajectory(const MocoTrajectory &halfPeriodTrajectory, std::vector< std::string > addPatterns={".*pelvis_tx/value"}, std::vector< std::string > negatePatterns={ ".*pelvis_list(?!/value).*", ".*pelvis_rotation(?!/value).*", ".*pelvis_tz(?!/value).*", ".*lumbar_bending(?!/value).*", ".*lumbar_rotation(?!/value).*"}, std::vector< std::string > negateAndShiftPatterns={ ".*pelvis_list/value", ".*pelvis_rotation/value", ".*pelvis_tz/value", ".*lumbar_bending/value", ".*lumbar_rotation/value"}, std::vector< std::pair< std::string, std::string >> symmetryPatterns={{R"(_r(\/|_|$))", "_l$1"}, {R"(_l(\/|_|$))", "_r$1"}})
Convert a trajectory covering half the period of a symmetric motion into a trajectory over the full p...
OpenSim::MocoProblem
A description of an optimal control problem, backed by OpenSim Models.
Definition: MocoProblem.h:424
OpenSim::MocoPeriodicityGoal
This goal enforces equality between initial and final variable values in the optimal control problem.
Definition: MocoPeriodicityGoal.h:100
OpenSim::TabOpLowPassFilter
Apply a low-pass filter to the trajectory.
Definition: TableProcessor.h:151
OpenSim::MocoControlGoal
Minimize the sum of the absolute value of the controls raised to a given exponent,...
Definition: MocoControlGoal.h:53
OpenSim::MocoProblem::updGoal
MocoGoal & updGoal(const std::string &name)
Returns a reference to the goal with name "name" in phase 0.
OpenSim::MocoCasADiSolver::setGuess
void setGuess(MocoTrajectory guess)
The number of time points in the trajectory does not need to match num_mesh_intervals; the trajectory...
OpenSim::MocoStudy::initCasADiSolver
MocoCasADiSolver & initCasADiSolver()
Call this method once you have finished setting up your MocoProblem.
OpenSim::MocoAverageSpeedGoal
This goal requires the average speed of the system to match a desired average speed.
Definition: MocoGoal.h:295
OpenSim::TableProcessor
This class describes a workflow for processing a table using TableOperators.
Definition: TableProcessor.h:47
OpenSim::MocoControlGoal::setExponent
void setExponent(int exponent)
Set the exponent on the control signals.
Definition: MocoControlGoal.h:86