From a453ef766f9ac92ffcc739465a5dcb5b80df006f Mon Sep 17 00:00:00 2001 From: Christopher Dembia Date: Thu, 23 Jan 2020 18:02:54 -0800 Subject: [PATCH 1/6] Add doxygen group modeloperator. --- Moco/Moco/ModelOperators.h | 16 +++++++++++++++- Moco/doc/Moco.dox | 4 ++++ Moco/doc/MocoUserGuide.dox | 3 ++- 3 files changed, 21 insertions(+), 2 deletions(-) diff --git a/Moco/Moco/ModelOperators.h b/Moco/Moco/ModelOperators.h index a5f2f997c..c96ba59b9 100644 --- a/Moco/Moco/ModelOperators.h +++ b/Moco/Moco/ModelOperators.h @@ -19,13 +19,16 @@ * -------------------------------------------------------------------------- */ #include "Components/DeGrooteFregly2016Muscle.h" + #include "ModelProcessor.h" #include + namespace OpenSim { /// Invoke DeGrooteFregly2016Muscle::replaceMuscles() on the model. +/// @ingroup modeloperator class OSIMMOCO_API ModOpReplaceMusclesWithDeGrooteFregly2016 : public ModelOperator { OpenSim_DECLARE_CONCRETE_OBJECT( @@ -39,6 +42,7 @@ class OSIMMOCO_API ModOpReplaceMusclesWithDeGrooteFregly2016 }; /// Turn off activation dynamics for all muscles in the model. +/// @ingroup modeloperator class OSIMMOCO_API ModOpIgnoreActivationDynamics : public ModelOperator { OpenSim_DECLARE_CONCRETE_OBJECT( ModOpIgnoreActivationDynamics, ModelOperator); @@ -53,6 +57,7 @@ class OSIMMOCO_API ModOpIgnoreActivationDynamics : public ModelOperator { }; /// Turn off tendon compliance for all muscles in the model. +/// @ingroup modeloperator class OSIMMOCO_API ModOpIgnoreTendonCompliance : public ModelOperator { OpenSim_DECLARE_CONCRETE_OBJECT(ModOpIgnoreTendonCompliance, ModelOperator); @@ -68,7 +73,8 @@ class OSIMMOCO_API ModOpIgnoreTendonCompliance : public ModelOperator { /// For DeGrooteFregly2016Muscle muscles whose 'ignore_tendon_compliance' /// property is false, set the tendon compliance dynamics mode to either /// 'explicit' or 'implicit'. -class OSIMMOCO_API ModOpTendonComplianceDynamicsModeDGF +/// @ingroup modeloperator +class OSIMMOCO_API ModOpTendonComplianceDynamicsModeDGF : public ModelOperator { OpenSim_DECLARE_CONCRETE_OBJECT( ModOpTendonComplianceDynamicsModeDGF, ModelOperator); @@ -100,6 +106,7 @@ class OSIMMOCO_API ModOpTendonComplianceDynamicsModeDGF /// Set the tendon compliance dynamics mode to "implicit" for all /// DeGrooteFregly2016Muscle%s in the model. +/// @ingroup modeloperator class OSIMMOCO_API ModOpUseImplicitTendonComplianceDynamicsDGF : public ModelOperator { OpenSim_DECLARE_CONCRETE_OBJECT( @@ -119,6 +126,7 @@ class OSIMMOCO_API ModOpUseImplicitTendonComplianceDynamicsDGF /// Turn off passive fiber forces for all DeGrooteFregly2016Muscle%s in the /// model. +/// @ingroup modeloperator class OSIMMOCO_API ModOpIgnorePassiveFiberForcesDGF : public ModelOperator { OpenSim_DECLARE_CONCRETE_OBJECT( ModOpIgnorePassiveFiberForcesDGF, ModelOperator); @@ -135,6 +143,7 @@ class OSIMMOCO_API ModOpIgnorePassiveFiberForcesDGF : public ModelOperator { /// Scale the active fiber force curve width for all DeGrooteFregly2016Muscle%s /// in the model. +/// @ingroup modeloperator class OSIMMOCO_API ModOpScaleActiveFiberForceCurveWidthDGF : public ModelOperator { OpenSim_DECLARE_CONCRETE_OBJECT( @@ -159,6 +168,7 @@ class OSIMMOCO_API ModOpScaleActiveFiberForceCurveWidthDGF : }; /// Set the fiber damping for all DeGrooteFregly2016Muscle%s in the model. +/// @ingroup modeloperator class OSIMMOCO_API ModOpFiberDampingDGF : public ModelOperator { OpenSim_DECLARE_CONCRETE_OBJECT(ModOpFiberDampingDGF, ModelOperator); OpenSim_DECLARE_PROPERTY(fiber_damping, double, @@ -180,6 +190,7 @@ OpenSim_DECLARE_CONCRETE_OBJECT(ModOpFiberDampingDGF, ModelOperator); }; /// Scale the max isometric force for all muscles in the model. +/// @ingroup modeloperator class OSIMMOCO_API ModOpScaleMaxIsometricForce : public ModelOperator { OpenSim_DECLARE_CONCRETE_OBJECT(ModOpScaleMaxIsometricForce, ModelOperator); OpenSim_DECLARE_PROPERTY(scale_factor, double, @@ -215,6 +226,7 @@ class OSIMMOCO_API ModOpRemoveMuscles : public ModelOperator { /// Add reserve actuators to the model using /// ModelFactory::createReserveActuators. +/// @ingroup modeloperator class OSIMMOCO_API ModOpAddReserves : public ModelOperator { OpenSim_DECLARE_CONCRETE_OBJECT(ModOpAddReserves, ModelOperator); OpenSim_DECLARE_PROPERTY(optimal_force, double, @@ -253,6 +265,7 @@ class OSIMMOCO_API ModOpAddReserves : public ModelOperator { /// Add external loads (e.g., ground reaction forces) to the model from a /// XML file. +/// @ingroup modeloperator class OSIMMOCO_API ModOpAddExternalLoads : public ModelOperator { OpenSim_DECLARE_CONCRETE_OBJECT(ModOpAddExternalLoads, ModelOperator); OpenSim_DECLARE_PROPERTY(filepath, std::string, @@ -277,6 +290,7 @@ class OSIMMOCO_API ModOpAddExternalLoads : public ModelOperator { } }; +/// @ingroup modeloperator class OSIMMOCO_API ModOpReplaceJointsWithWelds : public ModelOperator { OpenSim_DECLARE_CONCRETE_OBJECT(ModOpReplaceJointsWithWelds, ModelOperator); OpenSim_DECLARE_LIST_PROPERTY(joint_paths, std::string, diff --git a/Moco/doc/Moco.dox b/Moco/doc/Moco.dox index 15ba5e904..652b1307f 100644 --- a/Moco/doc/Moco.dox +++ b/Moco/doc/Moco.dox @@ -67,6 +67,10 @@ Easily format and log messages; intended for internal use by Moco. Utilities for parallelism, strings, date/time, and error-checking. +@defgroup modeloperator Model operators + +Modify a model using these ModelOperator%s, as part of a ModelProcessor. + */ } // namespace OpenSim diff --git a/Moco/doc/MocoUserGuide.dox b/Moco/doc/MocoUserGuide.dox index 29b9c1bdd..4fc0dab75 100644 --- a/Moco/doc/MocoUserGuide.dox +++ b/Moco/doc/MocoUserGuide.dox @@ -76,7 +76,8 @@ Moco contains utilities for creating models, modifying models, working with data, and postprocessing results. 1. ModelFactory: Create standard models and modify existing models. -2. ModelProcessor: Create a workflow of operations to perform on a model. +2. ModelProcessor and @ref modeloperator : Create a workflow of operations + to perform on a model. 3. TableProcessor: Create a workflow of operations to perform on a table. 4. @ref mocomodelutil, including visualizing and analyzing a MocoTrajectory. 5. @ref moconumutil, including interpolating data and reading/writing tables. From d33a431d648e52c3b7f8579dccb59166f85afbf9 Mon Sep 17 00:00:00 2001 From: Christopher Dembia Date: Thu, 23 Jan 2020 18:04:11 -0800 Subject: [PATCH 2/6] Add ModOpAppliesForce. --- CHANGELOG.md | 3 +++ Moco/Moco/ModelOperators.h | 28 ++++++++++++++++++++++++++++ 2 files changed, 31 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 864ef65f5..afbc2ae4a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,7 +1,10 @@ Change Log ========== + 0.4.0 (in development) ---------------------- +- 2020-01-23: Added ModOpAppliesForce to set the appliesForce property + for multiple Forces. 0.3.0 diff --git a/Moco/Moco/ModelOperators.h b/Moco/Moco/ModelOperators.h index c96ba59b9..67f9db7cf 100644 --- a/Moco/Moco/ModelOperators.h +++ b/Moco/Moco/ModelOperators.h @@ -310,6 +310,34 @@ class OSIMMOCO_API ModOpReplaceJointsWithWelds : public ModelOperator { } }; +/// Set the appliesForce property for the specified Force%s. +/// @ingroup modeloperator +class OSIMMOCO_API ModOpAppliesForce : public ModelOperator { +OpenSim_DECLARE_CONCRETE_OBJECT(ModOpAppliesForce, ModelOperator); + OpenSim_DECLARE_PROPERTY(appliesForce, bool, + "The value to use for appliesForce (default: true)."); + OpenSim_DECLARE_LIST_PROPERTY(force_paths, std::string, + "Paths to Forces whose appliesForce should be affected."); + +public: + ModOpAppliesForce() { + constructProperty_appliesForce(true); + constructProperty_force_paths(); + } + ModOpAppliesForce(bool appliesForce, const std::vector& paths) + : ModOpAppliesForce() { + set_appliesForce(appliesForce); + for (const auto& path : paths) { append_force_paths(path); } + } + void operate(Model& model, const std::string&) const override { + model.initSystem(); + for (int i = 0; i < getProperty_force_paths().size(); ++i) { + auto& force = model.updComponent(get_force_paths(i)); + force.set_appliesForce(get_appliesForce()); + } + } +}; + } // namespace OpenSim #endif // MOCO_MODELOPERATORS_H From 64444cd5509661168bd8ac9bfee14fb666c7acf6 Mon Sep 17 00:00:00 2001 From: Christopher Dembia Date: Thu, 23 Jan 2020 18:57:41 -0800 Subject: [PATCH 3/6] First pass at adding contact tracking to MocoTrack, but exampleMocoTrack doesn't solve well yet. --- CHANGELOG.md | 3 + .../example3DWalking/exampleMocoInverse.py | 14 + .../example3DWalking/exampleMocoInverse.cpp | 16 +- .../C++/example3DWalking/exampleMocoTrack.cpp | 41 ++- .../subject_walk_armless.osim | 312 ++++++++++++++++++ Moco/Moco/MocoTrack.cpp | 23 +- Moco/Moco/MocoTrack.h | 20 ++ Moco/doc/MocoExamples.dox | 2 +- Moco/doc/MocoTrack.dox | 25 +- 9 files changed, 443 insertions(+), 13 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index afbc2ae4a..4ffe15911 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -3,6 +3,9 @@ Change Log 0.4.0 (in development) ---------------------- +- 2020-01-23: Added contact tracking to MocoTrack, and updated exampleMocoTrack + to provide an example of contact tracking. + - 2020-01-23: Added ModOpAppliesForce to set the appliesForce property for multiple Forces. diff --git a/Moco/Bindings/Python/examples/example3DWalking/exampleMocoInverse.py b/Moco/Bindings/Python/examples/example3DWalking/exampleMocoInverse.py index 2ced741f1..df9f062fc 100644 --- a/Moco/Bindings/Python/examples/example3DWalking/exampleMocoInverse.py +++ b/Moco/Bindings/Python/examples/example3DWalking/exampleMocoInverse.py @@ -40,6 +40,20 @@ # Only valid for DeGrooteFregly2016Muscles. modelProcessor.append(osim.ModOpScaleActiveFiberForceCurveWidthDGF(1.5)) modelProcessor.append(osim.ModOpAddReserves(1.0)) +modelProcessor.append(osim.ModOpAppliesForce(False, [ + "/forceset/contactSphereHeel_r", + "/forceset/contactLateralRearfoot_r", + "/forceset/contactLateralMidfoot_r", + "/forceset/contactLateralToe_r", + "/forceset/contactMedialToe_r", + "/forceset/contactMedialMidfoot_r", + "/forceset/contactSphereHeel_l", + "/forceset/contactLateralRearfoot_l", + "/forceset/contactLateralMidfoot_l", + "/forceset/contactLateralToe_l", + "/forceset/contactMedialToe_l", + "/forceset/contactMedialMidfoot_l" +])) inverse.setModel(modelProcessor) # Construct a TableProcessor of the coordinate data and pass it to the diff --git a/Moco/Examples/C++/example3DWalking/exampleMocoInverse.cpp b/Moco/Examples/C++/example3DWalking/exampleMocoInverse.cpp index 542d570ae..83e015df7 100644 --- a/Moco/Examples/C++/example3DWalking/exampleMocoInverse.cpp +++ b/Moco/Examples/C++/example3DWalking/exampleMocoInverse.cpp @@ -45,7 +45,21 @@ int main() { ModOpIgnorePassiveFiberForcesDGF() | // Only valid for DeGrooteFregly2016Muscles. ModOpScaleActiveFiberForceCurveWidthDGF(1.5) | - ModOpAddReserves(1.0); + ModOpAddReserves(1.0) | + ModOpAppliesForce(false, { + "/forceset/contactSphereHeel_r", + "/forceset/contactLateralRearfoot_r", + "/forceset/contactLateralMidfoot_r", + "/forceset/contactLateralToe_r", + "/forceset/contactMedialToe_r", + "/forceset/contactMedialMidfoot_r", + "/forceset/contactSphereHeel_l", + "/forceset/contactLateralRearfoot_l", + "/forceset/contactLateralMidfoot_l", + "/forceset/contactLateralToe_l", + "/forceset/contactMedialToe_l", + "/forceset/contactMedialMidfoot_l", + }); inverse.setModel(modelProcessor); // Construct a TableProcessor of the coordinate data and pass it to the diff --git a/Moco/Examples/C++/example3DWalking/exampleMocoTrack.cpp b/Moco/Examples/C++/example3DWalking/exampleMocoTrack.cpp index 82765653f..50ea15590 100644 --- a/Moco/Examples/C++/example3DWalking/exampleMocoTrack.cpp +++ b/Moco/Examples/C++/example3DWalking/exampleMocoTrack.cpp @@ -19,7 +19,7 @@ /// This example features two different tracking problems solved using the /// MocoTrack tool. /// - The first problem demonstrates the basic usage of the tool interface -/// to solve a torque-driven marker tracking problem. +/// to solve a torque-driven marker and contact tracking problem. /// - The second problem shows how to customize a muscle-driven state tracking /// problem using more advanced features of the tool interface. /// @@ -30,6 +30,23 @@ using namespace OpenSim; +std::vector contacts_r { + "/forceset/contactSphereHeel_r", + "/forceset/contactLateralRearfoot_r", + "/forceset/contactLateralMidfoot_r", + "/forceset/contactLateralToe_r", + "/forceset/contactMedialToe_r", + "/forceset/contactMedialMidfoot_r", +}; +std::vector contacts_l { + "/forceset/contactSphereHeel_l", + "/forceset/contactLateralRearfoot_l", + "/forceset/contactLateralMidfoot_l", + "/forceset/contactLateralToe_l", + "/forceset/contactMedialToe_l", + "/forceset/contactMedialMidfoot_l", +}; + void torqueDrivenMarkerTracking() { // Create and name an instance of the MocoTrack tool. @@ -52,7 +69,10 @@ void torqueDrivenMarkerTracking() { // Add CoordinateActuators to the model degrees-of-freedom. This // ignores the pelvis coordinates which already have residual // CoordinateActuators. - ModOpAddReserves(250)); + ModOpAddReserves(250) | + ModOpAppliesForce(false, contacts_r) | + ModOpAppliesForce(false, contacts_l) + ); // Use this convenience function to set the MocoTrack markers reference // directly from a TRC file. By default, the markers data is filtered at @@ -109,7 +129,6 @@ void muscleDrivenStateTracking() { // parameters. ModelProcessor modelProcessor = ModelProcessor("subject_walk_armless.osim") | - ModOpAddExternalLoads("grf_walk.xml") | ModOpIgnoreTendonCompliance() | ModOpReplaceMusclesWithDeGrooteFregly2016() | // Only valid for DeGrooteFregly2016Muscles. @@ -135,6 +154,13 @@ void muscleDrivenStateTracking() { // the derivative of splined position data. track.set_track_reference_position_derivatives(true); + auto& contactTracking = track.updContactTrackingGoal(); + contactTracking.setEnabled(true); + contactTracking.setWeight(0.00001); + contactTracking.setExternalLoadsFile("grf_walk.xml"); + contactTracking.addContactGroup(contacts_r, "Right_GRF"); + contactTracking.addContactGroup(contacts_l, "Left_GRF"); + // Initial time, final time, and mesh interval. track.set_initial_time(0.81); track.set_final_time(1.65); @@ -162,8 +188,15 @@ void muscleDrivenStateTracking() { } } - // Solve and visualize. + // Solve. MocoSolution solution = study.solve(); + + // Write simulated ground reaction forces to a file. + TimeSeriesTable externalLoads = createExternalLoadsTableForGait( + model, solution, contacts_r, contacts_l); + writeTableToFile(externalLoads, "example3DWalking_MocoTrack_GRF.sto"); + + // Visualize. study.visualize(solution); } diff --git a/Moco/Examples/C++/example3DWalking/subject_walk_armless.osim b/Moco/Examples/C++/example3DWalking/subject_walk_armless.osim index 4985c68c8..d1a217c7e 100644 --- a/Moco/Examples/C++/example3DWalking/subject_walk_armless.osim +++ b/Moco/Examples/C++/example3DWalking/subject_walk_armless.osim @@ -11689,6 +11689,318 @@ 10 + + + /bodyset/calcn_r + + /ground + + 0.0146421 0 -0.0122799 + + 0.035000000000000003 + + 0 0 0 + + 0 0 -1.5707963267948966 + + 400000 + + 2 + + 0.80000000000000004 + + 0.80000000000000004 + + 0.5 + + 0.20000000000000001 + + + + /bodyset/calcn_r + + /ground + + 0.084929099999999993 0 0.033864900000000003 + + 0.035000000000000003 + + 0 0 0 + + 0 0 -1.5707963267948966 + + 400000 + + 2 + + 0.80000000000000004 + + 0.80000000000000004 + + 0.5 + + 0.20000000000000001 + + + + /bodyset/calcn_r + + /ground + + 0.153362 0 0.059482899999999998 + + 0.035000000000000003 + + 0 0 0 + + 0 0 -1.5707963267948966 + + 400000 + + 2 + + 0.80000000000000004 + + 0.80000000000000004 + + 0.5 + + 0.20000000000000001 + + + + /bodyset/calcn_r + + /ground + + 0.22301399999999999 0 0.067046099999999997 + + 0.035000000000000003 + + 0 0 0 + + 0 0 -1.5707963267948966 + + 400000 + + 2 + + 0.80000000000000004 + + 0.80000000000000004 + + 0.5 + + 0.20000000000000001 + + + + /bodyset/calcn_r + + /ground + + 0.27436300000000002 0 -0.026039 + + 0.035000000000000003 + + 0 0 0 + + 0 0 -1.5707963267948966 + + 400000 + + 2 + + 0.80000000000000004 + + 0.80000000000000004 + + 0.5 + + 0.20000000000000001 + + + + /bodyset/calcn_r + + /ground + + 0.20363700000000001 0 -0.039868800000000003 + + 0.035000000000000003 + + 0 0 0 + + 0 0 -1.5707963267948966 + + 400000 + + 2 + + 0.80000000000000004 + + 0.80000000000000004 + + 0.5 + + 0.20000000000000001 + + + + /bodyset/calcn_l + + /ground + + 0.0146421 0 0.0122799 + + 0.035000000000000003 + + 0 0 0 + + 0 0 -1.5707963267948966 + + 400000 + + 2 + + 0.80000000000000004 + + 0.80000000000000004 + + 0.5 + + 0.20000000000000001 + + + + /bodyset/calcn_l + + /ground + + 0.084929099999999993 0 -0.033864900000000003 + + 0.035000000000000003 + + 0 0 0 + + 0 0 -1.5707963267948966 + + 400000 + + 2 + + 0.80000000000000004 + + 0.80000000000000004 + + 0.5 + + 0.20000000000000001 + + + + /bodyset/calcn_l + + /ground + + 0.153362 0 -0.059482899999999998 + + 0.035000000000000003 + + 0 0 0 + + 0 0 -1.5707963267948966 + + 400000 + + 2 + + 0.80000000000000004 + + 0.80000000000000004 + + 0.5 + + 0.20000000000000001 + + + + /bodyset/calcn_l + + /ground + + 0.22301399999999999 0 -0.067046099999999997 + + 0.035000000000000003 + + 0 0 0 + + 0 0 -1.5707963267948966 + + 400000 + + 2 + + 0.80000000000000004 + + 0.80000000000000004 + + 0.5 + + 0.20000000000000001 + + + + /bodyset/calcn_l + + /ground + + 0.27436300000000002 0 0.026039 + + 0.035000000000000003 + + 0 0 0 + + 0 0 -1.5707963267948966 + + 400000 + + 2 + + 0.80000000000000004 + + 0.80000000000000004 + + 0.5 + + 0.20000000000000001 + + + + /bodyset/calcn_l + + /ground + + 0.20363700000000001 0 0.039868800000000003 + + 0.035000000000000003 + + 0 0 0 + + 0 0 -1.5707963267948966 + + 400000 + + 2 + + 0.80000000000000004 + + 0.80000000000000004 + + 0.5 + + 0.20000000000000001 + diff --git a/Moco/Moco/MocoTrack.cpp b/Moco/Moco/MocoTrack.cpp index a73f8ed84..46e675789 100644 --- a/Moco/Moco/MocoTrack.cpp +++ b/Moco/Moco/MocoTrack.cpp @@ -19,6 +19,7 @@ #include "MocoTrack.h" #include "MocoCasADiSolver/MocoCasADiSolver.h" +#include "MocoGoal/MocoContactTrackingGoal.h" #include "MocoGoal/MocoControlGoal.h" #include "MocoGoal/MocoControlTrackingGoal.h" #include "MocoGoal/MocoMarkerTrackingGoal.h" @@ -47,6 +48,13 @@ void MocoTrack::constructProperties() { constructProperty_allow_unused_references(false); constructProperty_guess_file(""); constructProperty_apply_tracked_states_to_guess(false); + { + MocoContactTrackingGoal contactTracking; + contactTracking.setName("contact_tracking"); + contactTracking.setEnabled(false); + contactTracking.setWeight(0.1); + constructProperty_MocoContactTrackingGoal(contactTracking); + } constructProperty_minimize_control_effort(true); constructProperty_control_effort_weight(0.001); } @@ -58,12 +66,12 @@ MocoStudy MocoTrack::initialize() { MocoProblem& problem = study.updProblem(); // Modeling. - // --------- + // ========= Model model = get_model().process(getDocumentDirectory()); model.initSystem(); // Goals. - // ------ + // ====== // State tracking cost. TimeSeriesTable tracked_states; if (!get_states_reference().empty()) { @@ -94,8 +102,15 @@ MocoStudy MocoTrack::initialize() { effort->setWeight(get_control_effort_weight()); } + // Contact tracking. + // ----------------- + if (get_MocoContactTrackingGoal().getEnabled()) { + problem.addGoal(std::unique_ptr( + get_MocoContactTrackingGoal().clone())); + } + // Set the time range. - // ------------------- + // =================== if (get_clip_time_range()) { m_timeInfo.initial += 1e-3; m_timeInfo.final -= 1e-3; @@ -103,7 +118,7 @@ MocoStudy MocoTrack::initialize() { problem.setTimeBounds(m_timeInfo.initial, m_timeInfo.final); // Configure solver. - // ----------------- + // ================= MocoCasADiSolver& solver = study.initCasADiSolver(); solver.set_num_mesh_intervals(m_timeInfo.numMeshIntervals); solver.set_multibody_dynamics_mode("explicit"); diff --git a/Moco/Moco/MocoTrack.h b/Moco/Moco/MocoTrack.h index bd0a492eb..ce900befd 100644 --- a/Moco/Moco/MocoTrack.h +++ b/Moco/Moco/MocoTrack.h @@ -29,6 +29,7 @@ namespace OpenSim { +class MocoContactTrackingGoal; class MocoWeightSet; class MocoProblem; class MocoTrajectory; @@ -71,6 +72,17 @@ class MocoTrajectory; /// properties give you finer control over the tracking costs, letting you set /// weights for individual reference data tracking errors. /// +/// Contact tracking +/// ---------------- +/// If the model contains contact elements and you have experimental contact +/// data that you would like these contact elements to match, you can use the +/// MocoContactTrackingGoal property of MocoTrack. Use updContactTrackingGoal() +/// to access this goal. By default, this goal is disabled; enable it via up +/// updContactTrackingGoal().setEnabled(). +/// +/// Contact is usually modeled with ExternalLoads data or contact elements in +/// the model, but not both. +/// /// Control effort minimization /// --------------------------- /// By default, a MocoControlCost term is added to the underlying problem with @@ -227,6 +239,9 @@ class OSIMMOCO_API MocoTrack : public MocoTool { "This " "will override any guess information provided via `guess_file`."); + OpenSim_DECLARE_UNNAMED_PROPERTY(MocoContactTrackingGoal, + "TODO"); + OpenSim_DECLARE_PROPERTY(minimize_control_effort, bool, "Whether or not to minimize actuator control effort in the problem." "Default: true."); @@ -265,6 +280,11 @@ class OSIMMOCO_API MocoTrack : public MocoTool { TabOpLowPassFilter(lowpassFilterFreq)); } + /// TODO + MocoContactTrackingGoal& updContactTrackingGoal() { + return upd_MocoContactTrackingGoal(); + } + MocoStudy initialize(); MocoSolution solve(bool visualize = false); diff --git a/Moco/doc/MocoExamples.dox b/Moco/doc/MocoExamples.dox index 4334b134e..614c8449c 100644 --- a/Moco/doc/MocoExamples.dox +++ b/Moco/doc/MocoExamples.dox @@ -122,7 +122,7 @@ MinimizeJointReaction | pendulum | MocoJointReactionGoal, MocoControlGoal, presc name | model/motion | description | interfaces ---- | ------------ | ----------- | ---------- -MocoTrack | lower-limb | Using the MocoTrack tool for walking with marker tracking and state tracking; generating a PDF report | [MATLAB](@ref exampleMocoTrack.m), [Python](@ref exampleMocoTrack.py), [C++](@ref exampleMocoTrack.cpp) +MocoTrack | lower-limb | Using the MocoTrack tool for walking with marker tracking, state tracking, and contact tracking (MocoContactTrackingGoal); generating a PDF report | [MATLAB](@ref exampleMocoTrack.m), [Python](@ref exampleMocoTrack.py), [C++](@ref exampleMocoTrack.cpp) MocoInverse | lower-limb | Using the MocoInverse tool for walking; generating a PDF report | [MATLAB](@ref exampleMocoInverse.m), [Python](@ref exampleMocoInverse.py), [C++](@ref exampleMocoInverse.cpp) 2DWalking | walking | MocoTrack, MocoStudy prediction, MocoControlGoal, MocoAverageSpeedGoal, MocoPerodicityGoal, createPeriodicSolution | [C++](@ref example2DWalking.cpp) MarkerTracking10DOF | walking | MocoControlGoal, MocoMarkerTrackingGoal | [MATLAB](@ref exampleMarkerTracking10DOF.m) diff --git a/Moco/doc/MocoTrack.dox b/Moco/doc/MocoTrack.dox index 7d9634ca5..5d20aee0e 100644 --- a/Moco/doc/MocoTrack.dox +++ b/Moco/doc/MocoTrack.dox @@ -2,6 +2,8 @@ @page mocotrack MocoTrack: motion tracking +### Background + For some research applications in musculoskeletal biomechanics, it is not sufficient to perform a simulation where model kinematics are prescribed directly from experimental data. These approaches, called 'inverse' problems, @@ -15,20 +17,37 @@ either difficult to pose or computationally burdensome, recent methods using direct collocation for solving tracking problems in biomechanics have proven this now to be a viable tool [1,2]. +### MocoTrack + In Moco, you can use the MocoTrack tool to solve tracking optimization problems. The MocoTrack tool accepts both marker trajectory and model coordinate kinematic data sets and adds a MocoMarkerTrackingGoal and/or MocoStateTrackingGoal term(s) accordingly. Both types of kinematic data can be tracked simultaneously and -weights for each cost term can be supplied. In addition, weights can be set for -individual components of tracked data sets (e.g., tracking markers on bony +weights for each cost term can be supplied. In addition, weights can be set for +individual components of tracked data sets (e.g., tracking markers on bony landmarks preferentially over markers on soft tissue). +### Contact + +Contact between the model and its environment is usually represented via +measured forces (using ExternalLoads) or via compliant contact elements +in the model (e.g., SmoothSphereHalfSpaceForce). If the simulated motion +deviates from the experimentally observed motion, then measured contact forces +should not be applied to the model---the altered motion would lead to +contact forces that differ from the contact data. Therefore, we recommend using +compliant contact elements when using MocoTrack. You can minimize +the error between the compliant contact elements and measured contact force data +using a MocoContactTrackingGoal, which MocoTrack provides. + +### Additional features + Additional features provided by MocoTrack include: ensuring that the problem time window is consistent with all data sets, filling in missing coordinate speed states to track if only coordinate values are available, and appending tracking data to the problem initial guess. -Examples in the Moco distribution: +### Examples in the Moco distribution + - MATLAB: [Resources/Code/Matlab/exampleMocoTrack/exampleMocoTrack.m](@ref exampleMocoTrack.m) - Python: [Resources/Code/Python/exampleMocoTrack/exampleMocoTrack.py](@ref exampleMocoTrack.py) - C++: [Resources/Code/CPP/exampleMocoTrack/exampleMocoTrack.cpp](@ref exampleMocoTrack.cpp) From e0ae34763aabd88168510c9177194766a1d97746 Mon Sep 17 00:00:00 2001 From: Christopher Dembia Date: Mon, 2 Mar 2020 16:48:35 -0800 Subject: [PATCH 4/6] Update example2DWalking for MocoTrack's contact tracking. --- .../example2DWalking/example2DWalking.m | 42 +-- .../C++/example2DWalking/example2DWalking.cpp | 34 +- .../example3DWalking/exampleMocoInverse.cpp | 16 +- .../C++/example3DWalking/exampleMocoTrack.cpp | 41 +-- .../subject_walk_armless.osim | 312 ------------------ Moco/Moco/MocoTrack.h | 2 +- 6 files changed, 46 insertions(+), 401 deletions(-) diff --git a/Moco/Bindings/Java/Matlab/Examples/example2DWalking/example2DWalking.m b/Moco/Bindings/Java/Matlab/Examples/example2DWalking/example2DWalking.m index 802e02625..070cfd97d 100644 --- a/Moco/Bindings/Java/Matlab/Examples/example2DWalking/example2DWalking.m +++ b/Moco/Bindings/Java/Matlab/Examples/example2DWalking/example2DWalking.m @@ -86,12 +86,32 @@ track.set_apply_tracked_states_to_guess(true); track.set_initial_time(0.0); track.set_final_time(0.47008941); + +% Optionally, add a contact tracking goal. +if GRFTrackingWeight ~= 0 + % Track the right and left vertical and fore-aft ground reaction forces. + contactTracking = track.updContactTrackingGoal(); + contactTracking.setEnabled(true); + contactTracking.setWeight(GRFTrackingWeight); + contactTracking.setExternalLoadsFile('referenceGRF.xml'); + forceNamesRightFoot = StdVectorString(); + forceNamesRightFoot.add('contactSphereHeel_r'); + forceNamesRightFoot.add('contactSphereFront_r'); + contactTracking.addContactGroup(forceNamesRightFoot, 'Right_GRF'); + forceNamesLeftFoot = StdVectorString(); + forceNamesLeftFoot.add('contactSphereHeel_l'); + forceNamesLeftFoot.add('contactSphereFront_l'); + contactTracking.addContactGroup(forceNamesLeftFoot, 'Left_GRF'); + contactTracking.setProjection('plane'); + contactTracking.setProjectionVector(Vec3(0, 0, 1)); +end + study = track.initialize(); problem = study.updProblem(); -% Goals -% ===== +% Additional goals +% ================ % Symmetry (to permit simulating only one step) symmetryGoal = MocoPeriodicityGoal('symmetryGoal'); @@ -147,24 +167,6 @@ effort = MocoControlGoal.safeDownCast(problem.updGoal('control_effort')); effort.setWeight(controlEffortWeight); -% Optionally, add a contact tracking goal. -if GRFTrackingWeight ~= 0 - % Track the right and left vertical and fore-aft ground reaction forces. - contactTracking = MocoContactTrackingGoal('contact', GRFTrackingWeight); - contactTracking.setExternalLoadsFile('referenceGRF.xml'); - forceNamesRightFoot = StdVectorString(); - forceNamesRightFoot.add('contactSphereHeel_r'); - forceNamesRightFoot.add('contactSphereFront_r'); - contactTracking.addContactGroup(forceNamesRightFoot, 'Right_GRF'); - forceNamesLeftFoot = StdVectorString(); - forceNamesLeftFoot.add('contactSphereHeel_l'); - forceNamesLeftFoot.add('contactSphereFront_l'); - contactTracking.addContactGroup(forceNamesLeftFoot, 'Left_GRF'); - contactTracking.setProjection('plane'); - contactTracking.setProjectionVector(Vec3(0, 0, 1)); - problem.addGoal(contactTracking); -end - % Bounds % ====== diff --git a/Moco/Examples/C++/example2DWalking/example2DWalking.cpp b/Moco/Examples/C++/example2DWalking/example2DWalking.cpp index 25e335bf8..0fc18e6b5 100644 --- a/Moco/Examples/C++/example2DWalking/example2DWalking.cpp +++ b/Moco/Examples/C++/example2DWalking/example2DWalking.cpp @@ -76,11 +76,27 @@ MocoSolution gaitTracking(double controlEffortWeight = 10, track.set_apply_tracked_states_to_guess(true); track.set_initial_time(0.0); track.set_final_time(0.47008941); + + // Optionally, add a contact tracking goal. + if (GRFTrackingWeight != 0) { + // Track the right and left vertical and fore-aft ground reaction forces. + auto& contactTracking = track.updContactTrackingGoal(); + contactTracking.setEnabled(true); + contactTracking.setWeight(GRFTrackingWeight); + contactTracking.setExternalLoadsFile("referenceGRF.xml"); + contactTracking.addContactGroup( + {"contactSphereHeel_r", "contactSphereFront_r"},"Right_GRF"); + contactTracking.addContactGroup( + {"contactSphereHeel_l", "contactSphereFront_l"}, "Left_GRF"); + contactTracking.setProjection("plane"); + contactTracking.setProjectionVector(SimTK::Vec3(0, 0, 1)); + } + MocoStudy study = track.initialize(); MocoProblem& problem = study.updProblem(); - // Goals. - // ===== + // Additional goals. + // ================= // Symmetry. auto* symmetryGoal = problem.addGoal("symmetryGoal"); Model model = modelprocessor.process(); @@ -134,20 +150,6 @@ MocoSolution gaitTracking(double controlEffortWeight = 10, dynamic_cast(problem.updGoal("control_effort")); effort.setWeight(controlEffortWeight); - // Optionally, add a contact tracking goal. - if (GRFTrackingWeight != 0) { - // Track the right and left vertical and fore-aft ground reaction forces. - auto* contactTracking = problem.addGoal( - "contact", GRFTrackingWeight); - contactTracking->setExternalLoadsFile("referenceGRF.xml"); - contactTracking->addContactGroup( - {"contactSphereHeel_r", "contactSphereFront_r"},"Right_GRF"); - contactTracking->addContactGroup( - {"contactSphereHeel_l", "contactSphereFront_l"}, "Left_GRF"); - contactTracking->setProjection("plane"); - contactTracking->setProjectionVector(SimTK::Vec3(0, 0, 1)); - } - // Bounds. // ======= problem.setStateInfo("/jointset/groundPelvis/pelvis_tilt/value", diff --git a/Moco/Examples/C++/example3DWalking/exampleMocoInverse.cpp b/Moco/Examples/C++/example3DWalking/exampleMocoInverse.cpp index 83e015df7..542d570ae 100644 --- a/Moco/Examples/C++/example3DWalking/exampleMocoInverse.cpp +++ b/Moco/Examples/C++/example3DWalking/exampleMocoInverse.cpp @@ -45,21 +45,7 @@ int main() { ModOpIgnorePassiveFiberForcesDGF() | // Only valid for DeGrooteFregly2016Muscles. ModOpScaleActiveFiberForceCurveWidthDGF(1.5) | - ModOpAddReserves(1.0) | - ModOpAppliesForce(false, { - "/forceset/contactSphereHeel_r", - "/forceset/contactLateralRearfoot_r", - "/forceset/contactLateralMidfoot_r", - "/forceset/contactLateralToe_r", - "/forceset/contactMedialToe_r", - "/forceset/contactMedialMidfoot_r", - "/forceset/contactSphereHeel_l", - "/forceset/contactLateralRearfoot_l", - "/forceset/contactLateralMidfoot_l", - "/forceset/contactLateralToe_l", - "/forceset/contactMedialToe_l", - "/forceset/contactMedialMidfoot_l", - }); + ModOpAddReserves(1.0); inverse.setModel(modelProcessor); // Construct a TableProcessor of the coordinate data and pass it to the diff --git a/Moco/Examples/C++/example3DWalking/exampleMocoTrack.cpp b/Moco/Examples/C++/example3DWalking/exampleMocoTrack.cpp index 50ea15590..8d6c06c1f 100644 --- a/Moco/Examples/C++/example3DWalking/exampleMocoTrack.cpp +++ b/Moco/Examples/C++/example3DWalking/exampleMocoTrack.cpp @@ -19,7 +19,7 @@ /// This example features two different tracking problems solved using the /// MocoTrack tool. /// - The first problem demonstrates the basic usage of the tool interface -/// to solve a torque-driven marker and contact tracking problem. +/// to solve a torque-driven marker tracking problem. /// - The second problem shows how to customize a muscle-driven state tracking /// problem using more advanced features of the tool interface. /// @@ -30,23 +30,6 @@ using namespace OpenSim; -std::vector contacts_r { - "/forceset/contactSphereHeel_r", - "/forceset/contactLateralRearfoot_r", - "/forceset/contactLateralMidfoot_r", - "/forceset/contactLateralToe_r", - "/forceset/contactMedialToe_r", - "/forceset/contactMedialMidfoot_r", -}; -std::vector contacts_l { - "/forceset/contactSphereHeel_l", - "/forceset/contactLateralRearfoot_l", - "/forceset/contactLateralMidfoot_l", - "/forceset/contactLateralToe_l", - "/forceset/contactMedialToe_l", - "/forceset/contactMedialMidfoot_l", -}; - void torqueDrivenMarkerTracking() { // Create and name an instance of the MocoTrack tool. @@ -69,10 +52,7 @@ void torqueDrivenMarkerTracking() { // Add CoordinateActuators to the model degrees-of-freedom. This // ignores the pelvis coordinates which already have residual // CoordinateActuators. - ModOpAddReserves(250) | - ModOpAppliesForce(false, contacts_r) | - ModOpAppliesForce(false, contacts_l) - ); + ModOpAddReserves(250)); // Use this convenience function to set the MocoTrack markers reference // directly from a TRC file. By default, the markers data is filtered at @@ -129,6 +109,7 @@ void muscleDrivenStateTracking() { // parameters. ModelProcessor modelProcessor = ModelProcessor("subject_walk_armless.osim") | + ModOpAddExternalLoads("grf_walk.xml") | ModOpIgnoreTendonCompliance() | ModOpReplaceMusclesWithDeGrooteFregly2016() | // Only valid for DeGrooteFregly2016Muscles. @@ -154,13 +135,6 @@ void muscleDrivenStateTracking() { // the derivative of splined position data. track.set_track_reference_position_derivatives(true); - auto& contactTracking = track.updContactTrackingGoal(); - contactTracking.setEnabled(true); - contactTracking.setWeight(0.00001); - contactTracking.setExternalLoadsFile("grf_walk.xml"); - contactTracking.addContactGroup(contacts_r, "Right_GRF"); - contactTracking.addContactGroup(contacts_l, "Left_GRF"); - // Initial time, final time, and mesh interval. track.set_initial_time(0.81); track.set_final_time(1.65); @@ -188,15 +162,8 @@ void muscleDrivenStateTracking() { } } - // Solve. + // Solve and visualize. MocoSolution solution = study.solve(); - - // Write simulated ground reaction forces to a file. - TimeSeriesTable externalLoads = createExternalLoadsTableForGait( - model, solution, contacts_r, contacts_l); - writeTableToFile(externalLoads, "example3DWalking_MocoTrack_GRF.sto"); - - // Visualize. study.visualize(solution); } diff --git a/Moco/Examples/C++/example3DWalking/subject_walk_armless.osim b/Moco/Examples/C++/example3DWalking/subject_walk_armless.osim index d1a217c7e..4985c68c8 100644 --- a/Moco/Examples/C++/example3DWalking/subject_walk_armless.osim +++ b/Moco/Examples/C++/example3DWalking/subject_walk_armless.osim @@ -11689,318 +11689,6 @@ 10 - - - /bodyset/calcn_r - - /ground - - 0.0146421 0 -0.0122799 - - 0.035000000000000003 - - 0 0 0 - - 0 0 -1.5707963267948966 - - 400000 - - 2 - - 0.80000000000000004 - - 0.80000000000000004 - - 0.5 - - 0.20000000000000001 - - - - /bodyset/calcn_r - - /ground - - 0.084929099999999993 0 0.033864900000000003 - - 0.035000000000000003 - - 0 0 0 - - 0 0 -1.5707963267948966 - - 400000 - - 2 - - 0.80000000000000004 - - 0.80000000000000004 - - 0.5 - - 0.20000000000000001 - - - - /bodyset/calcn_r - - /ground - - 0.153362 0 0.059482899999999998 - - 0.035000000000000003 - - 0 0 0 - - 0 0 -1.5707963267948966 - - 400000 - - 2 - - 0.80000000000000004 - - 0.80000000000000004 - - 0.5 - - 0.20000000000000001 - - - - /bodyset/calcn_r - - /ground - - 0.22301399999999999 0 0.067046099999999997 - - 0.035000000000000003 - - 0 0 0 - - 0 0 -1.5707963267948966 - - 400000 - - 2 - - 0.80000000000000004 - - 0.80000000000000004 - - 0.5 - - 0.20000000000000001 - - - - /bodyset/calcn_r - - /ground - - 0.27436300000000002 0 -0.026039 - - 0.035000000000000003 - - 0 0 0 - - 0 0 -1.5707963267948966 - - 400000 - - 2 - - 0.80000000000000004 - - 0.80000000000000004 - - 0.5 - - 0.20000000000000001 - - - - /bodyset/calcn_r - - /ground - - 0.20363700000000001 0 -0.039868800000000003 - - 0.035000000000000003 - - 0 0 0 - - 0 0 -1.5707963267948966 - - 400000 - - 2 - - 0.80000000000000004 - - 0.80000000000000004 - - 0.5 - - 0.20000000000000001 - - - - /bodyset/calcn_l - - /ground - - 0.0146421 0 0.0122799 - - 0.035000000000000003 - - 0 0 0 - - 0 0 -1.5707963267948966 - - 400000 - - 2 - - 0.80000000000000004 - - 0.80000000000000004 - - 0.5 - - 0.20000000000000001 - - - - /bodyset/calcn_l - - /ground - - 0.084929099999999993 0 -0.033864900000000003 - - 0.035000000000000003 - - 0 0 0 - - 0 0 -1.5707963267948966 - - 400000 - - 2 - - 0.80000000000000004 - - 0.80000000000000004 - - 0.5 - - 0.20000000000000001 - - - - /bodyset/calcn_l - - /ground - - 0.153362 0 -0.059482899999999998 - - 0.035000000000000003 - - 0 0 0 - - 0 0 -1.5707963267948966 - - 400000 - - 2 - - 0.80000000000000004 - - 0.80000000000000004 - - 0.5 - - 0.20000000000000001 - - - - /bodyset/calcn_l - - /ground - - 0.22301399999999999 0 -0.067046099999999997 - - 0.035000000000000003 - - 0 0 0 - - 0 0 -1.5707963267948966 - - 400000 - - 2 - - 0.80000000000000004 - - 0.80000000000000004 - - 0.5 - - 0.20000000000000001 - - - - /bodyset/calcn_l - - /ground - - 0.27436300000000002 0 0.026039 - - 0.035000000000000003 - - 0 0 0 - - 0 0 -1.5707963267948966 - - 400000 - - 2 - - 0.80000000000000004 - - 0.80000000000000004 - - 0.5 - - 0.20000000000000001 - - - - /bodyset/calcn_l - - /ground - - 0.20363700000000001 0 0.039868800000000003 - - 0.035000000000000003 - - 0 0 0 - - 0 0 -1.5707963267948966 - - 400000 - - 2 - - 0.80000000000000004 - - 0.80000000000000004 - - 0.5 - - 0.20000000000000001 - diff --git a/Moco/Moco/MocoTrack.h b/Moco/Moco/MocoTrack.h index ce900befd..0d333ef97 100644 --- a/Moco/Moco/MocoTrack.h +++ b/Moco/Moco/MocoTrack.h @@ -77,7 +77,7 @@ class MocoTrajectory; /// If the model contains contact elements and you have experimental contact /// data that you would like these contact elements to match, you can use the /// MocoContactTrackingGoal property of MocoTrack. Use updContactTrackingGoal() -/// to access this goal. By default, this goal is disabled; enable it via up +/// to access this goal. By default, this goal is disabled; enable it via /// updContactTrackingGoal().setEnabled(). /// /// Contact is usually modeled with ExternalLoads data or contact elements in From dfcd78528f40323a8c071e5ac705145bd2362fcd Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Thu, 9 Apr 2020 11:00:32 -0700 Subject: [PATCH 5/6] Get torque-driven state tracking to work --- .../example3DWalking/exampleMocoInverse.cpp | 34 +- .../C++/example3DWalking/exampleMocoTrack.cpp | 146 +++++++-- .../subject_walk_armless.osim | 308 +++++++++++++++++- Moco/Moco/MocoTrack.cpp | 10 +- 4 files changed, 465 insertions(+), 33 deletions(-) diff --git a/Moco/Examples/C++/example3DWalking/exampleMocoInverse.cpp b/Moco/Examples/C++/example3DWalking/exampleMocoInverse.cpp index 5f0c43aea..e9ab228a9 100644 --- a/Moco/Examples/C++/example3DWalking/exampleMocoInverse.cpp +++ b/Moco/Examples/C++/example3DWalking/exampleMocoInverse.cpp @@ -49,7 +49,22 @@ void solveMocoInverse() { ModOpIgnorePassiveFiberForcesDGF() | // Only valid for DeGrooteFregly2016Muscles. ModOpScaleActiveFiberForceCurveWidthDGF(1.5) | - ModOpAddReserves(1.0); + ModOpAddReserves(1.0) | + // Disable the contact force elements in the model. + ModOpAppliesForce(false, { + "/forceset/contactHeel_r", + "/forceset/contactLateralRearfoot_r", + "/forceset/contactLateralMidfoot_r", + "/forceset/contactLateralToe_r", + "/forceset/contactMedialToe_r", + "/forceset/contactMedialMidfoot_r", + "/forceset/contactHeel_l", + "/forceset/contactLateralRearfoot_l", + "/forceset/contactLateralMidfoot_l", + "/forceset/contactLateralToe_l", + "/forceset/contactMedialToe_l", + "/forceset/contactMedialMidfoot_l" + }); inverse.setModel(modelProcessor); // Construct a TableProcessor of the coordinate data and pass it to the @@ -91,7 +106,22 @@ void solveMocoInverseWithEMG() { ModOpIgnorePassiveFiberForcesDGF() | // Only valid for DeGrooteFregly2016Muscles. ModOpScaleActiveFiberForceCurveWidthDGF(1.5) | - ModOpAddReserves(1.0); + ModOpAddReserves(1.0) | + // Disable the contact force elements in the model. + ModOpAppliesForce(false, { + "/forceset/contactHeel_r", + "/forceset/contactLateralRearfoot_r", + "/forceset/contactLateralMidfoot_r", + "/forceset/contactLateralToe_r", + "/forceset/contactMedialToe_r", + "/forceset/contactMedialMidfoot_r", + "/forceset/contactHeel_l", + "/forceset/contactLateralRearfoot_l", + "/forceset/contactLateralMidfoot_l", + "/forceset/contactLateralToe_l", + "/forceset/contactMedialToe_l", + "/forceset/contactMedialMidfoot_l" + }); inverse.setModel(modelProcessor); inverse.setKinematics(TableProcessor("coordinates.sto")); inverse.set_initial_time(0.81); diff --git a/Moco/Examples/C++/example3DWalking/exampleMocoTrack.cpp b/Moco/Examples/C++/example3DWalking/exampleMocoTrack.cpp index 8d6c06c1f..55507f5d6 100644 --- a/Moco/Examples/C++/example3DWalking/exampleMocoTrack.cpp +++ b/Moco/Examples/C++/example3DWalking/exampleMocoTrack.cpp @@ -16,6 +16,7 @@ * limitations under the License. * * -------------------------------------------------------------------------- */ +// TODO update comments for contact tracking /// This example features two different tracking problems solved using the /// MocoTrack tool. /// - The first problem demonstrates the basic usage of the tool interface @@ -30,7 +31,8 @@ using namespace OpenSim; -void torqueDrivenMarkerTracking() { +void torqueDrivenMarkerTracking( + bool useFootGroundContact, bool enableContactTracking) { // Create and name an instance of the MocoTrack tool. MocoTrack track; @@ -41,18 +43,56 @@ void torqueDrivenMarkerTracking() { // ModelOperators. Operations are performed in the order that they are // appended to the model. In C++, you may use the pipe operator '|' to // append ModelOperators. - track.setModel( + auto modelProcessor = // Create the base Model by passing in the model file. ModelProcessor("subject_walk_armless.osim") | - // Add ground reaction external loads in lieu of a ground-contact - // model. - ModOpAddExternalLoads("grf_walk.xml") | // Remove all the muscles in the model's ForceSet. ModOpRemoveMuscles() | // Add CoordinateActuators to the model degrees-of-freedom. This // ignores the pelvis coordinates which already have residual // CoordinateActuators. - ModOpAddReserves(250)); + ModOpAddReserves(250); + + std::vector forceNamesRightFoot = { + "forceset/contactHeel_r", + "forceset/contactLateralRearfoot_r", + "forceset/contactLateralMidfoot_r", + "forceset/contactLateralToe_r", + "forceset/contactMedialToe_r", + "forceset/contactMedialMidfoot_r"}; + std::vector forceNamesLeftFoot = { + "forceset/contactHeel_l", + "forceset/contactLateralRearfoot_l", + "forceset/contactLateralMidfoot_l", + "forceset/contactLateralToe_l", + "forceset/contactMedialToe_l", + "forceset/contactMedialMidfoot_l"}; + if (useFootGroundContact) { + if (enableContactTracking) { + // Configure the existing MocoContactTrackingGoal in MocoTrack. + auto& contactTracking = track.updContactTrackingGoal(); + contactTracking.setEnabled(true); + contactTracking.setWeight(1e-4 / 2*forceNamesRightFoot.size()); + contactTracking.setExternalLoadsFile("grf_walk.xml"); + contactTracking.addContactGroup(forceNamesRightFoot, "Right_GRF"); + contactTracking.addContactGroup(forceNamesLeftFoot, "Left_GRF"); + // Track only the sagittal plane reaction forces. + contactTracking.setProjection("plane"); + contactTracking.setProjectionVector(SimTK::Vec3(0, 0, 1)); + } + } else { + // Add ground reaction external loads in lieu of a ground-contact + // model. + modelProcessor.append(ModOpAddExternalLoads("grf_walk.xml")); + // Disable the contact force elements in the model. + auto forceNamesAll = forceNamesRightFoot; + forceNamesAll.insert(forceNamesAll.end(), forceNamesLeftFoot.begin(), + forceNamesLeftFoot.end()); + modelProcessor.append(ModOpAppliesForce(false, forceNamesAll)); + } + + track.setModel(modelProcessor); + modelProcessor.process().print("subject_walk_armless_torque_driven.osim"); // Use this convenience function to set the MocoTrack markers reference // directly from a TRC file. By default, the markers data is filtered at @@ -97,7 +137,8 @@ void torqueDrivenMarkerTracking() { MocoSolution solution = track.solve(true); } -void muscleDrivenStateTracking() { +void muscleDrivenStateTracking( + bool useFootGroundContact, bool enableContactTracking) { // Create and name an instance of the MocoTrack tool. MocoTrack track; @@ -107,15 +148,58 @@ void muscleDrivenStateTracking() { // muscles in the model are replaced with optimization-friendly // DeGrooteFregly2016Muscles, and adjustments are made to the default muscle // parameters. - ModelProcessor modelProcessor = + //ModelProcessor modelProcessor = + // ModelProcessor("subject_walk_armless.osim") | + // ModOpAddExternalLoads("grf_walk.xml") | + // ModOpIgnoreTendonCompliance() | + // ModOpReplaceMusclesWithDeGrooteFregly2016() | + // // Only valid for DeGrooteFregly2016Muscles. + // ModOpIgnorePassiveFiberForcesDGF() | + // // Only valid for DeGrooteFregly2016Muscles. + // ModOpScaleActiveFiberForceCurveWidthDGF(1.5); + + auto modelProcessor = + // Create the base Model by passing in the model file. ModelProcessor("subject_walk_armless.osim") | - ModOpAddExternalLoads("grf_walk.xml") | - ModOpIgnoreTendonCompliance() | - ModOpReplaceMusclesWithDeGrooteFregly2016() | - // Only valid for DeGrooteFregly2016Muscles. - ModOpIgnorePassiveFiberForcesDGF() | - // Only valid for DeGrooteFregly2016Muscles. - ModOpScaleActiveFiberForceCurveWidthDGF(1.5); + // Remove all the muscles in the model's ForceSet. + ModOpRemoveMuscles() | + // Add CoordinateActuators to the model degrees-of-freedom. This + // ignores the pelvis coordinates which already have residual + // CoordinateActuators. + ModOpAddReserves(250); + + std::vector forceNamesRightFoot = {"forceset/contactHeel_r", + "forceset/contactLateralRearfoot_r", + "forceset/contactLateralMidfoot_r", "forceset/contactLateralToe_r", + "forceset/contactMedialToe_r", "forceset/contactMedialMidfoot_r"}; + std::vector forceNamesLeftFoot = {"forceset/contactHeel_l", + "forceset/contactLateralRearfoot_l", + "forceset/contactLateralMidfoot_l", "forceset/contactLateralToe_l", + "forceset/contactMedialToe_l", "forceset/contactMedialMidfoot_l"}; + if (useFootGroundContact) { + if (enableContactTracking) { + // Configure the existing MocoContactTrackingGoal in MocoTrack. + auto& contactTracking = track.updContactTrackingGoal(); + contactTracking.setEnabled(true); + contactTracking.setWeight(1e-4 / 2 * forceNamesRightFoot.size()); + contactTracking.setExternalLoadsFile("grf_walk.xml"); + contactTracking.addContactGroup(forceNamesRightFoot, "Right_GRF"); + contactTracking.addContactGroup(forceNamesLeftFoot, "Left_GRF"); + // Track only the sagittal plane reaction forces. + contactTracking.setProjection("plane"); + contactTracking.setProjectionVector(SimTK::Vec3(0, 0, 1)); + } + } else { + // Add ground reaction external loads in lieu of a ground-contact + // model. + modelProcessor.append(ModOpAddExternalLoads("grf_walk.xml")); + // Disable the contact force elements in the model. + auto forceNamesAll = forceNamesRightFoot; + forceNamesAll.insert(forceNamesAll.end(), forceNamesLeftFoot.begin(), + forceNamesLeftFoot.end()); + modelProcessor.append(ModOpAppliesForce(false, forceNamesAll)); + } + track.setModel(modelProcessor); // Construct a TableProcessor of the coordinate data and pass it to the @@ -126,6 +210,11 @@ void muscleDrivenStateTracking() { track.setStatesReference(TableProcessor("coordinates.sto")); track.set_states_global_tracking_weight(10); + MocoWeightSet stateWeights; + stateWeights.cloneAndAppend({"/jointset/ground_pelvis/pelvis_ty/value", 0}); + stateWeights.cloneAndAppend({"/jointset/ground_pelvis/pelvis_tz/value", 0}); + track.set_states_weight_set(stateWeights); + // This setting allows extra data columns contained in the states // reference that don't correspond to model coordinates. track.set_allow_unused_references(true); @@ -147,20 +236,20 @@ void muscleDrivenStateTracking() { // Get a reference to the MocoControlGoal that is added to every MocoTrack // problem by default. - MocoProblem& problem = study.updProblem(); - MocoControlGoal& effort = - dynamic_cast(problem.updGoal("control_effort")); + //MocoProblem& problem = study.updProblem(); + //MocoControlGoal& effort = + // dynamic_cast(problem.updGoal("control_effort")); // Put a large weight on the pelvis CoordinateActuators, which act as the // residual, or 'hand-of-god', forces which we would like to keep as small // as possible. - Model model = modelProcessor.process(); - for (const auto& coordAct : model.getComponentList()) { - auto coordPath = coordAct.getAbsolutePathString(); - if (coordPath.find("pelvis") != std::string::npos) { - effort.setWeightForControl(coordPath, 10); - } - } + // Model model = modelProcessor.process(); + // for (const auto& coordAct : model.getComponentList()) { + // auto coordPath = coordAct.getAbsolutePathString(); + // if (coordPath.find("pelvis") != std::string::npos) { + // effort.setWeightForControl(coordPath, 10); + // } + //} // Solve and visualize. MocoSolution solution = study.solve(); @@ -169,15 +258,18 @@ void muscleDrivenStateTracking() { int main() { + bool useFootGroundContact = true; + bool enableContactTracking = false; + // Solve the torque-driven marker tracking problem. // This problem takes a few minutes to solve. - torqueDrivenMarkerTracking(); + //torqueDrivenMarkerTracking(useFootGroundContact, enableContactTracking); // Solve the muscle-driven state tracking problem. // This problem could take an hour or more to solve, depending on the // number of processor cores available for parallelization. With 12 cores, // it takes around 25 minutes. - muscleDrivenStateTracking(); + muscleDrivenStateTracking(useFootGroundContact, enableContactTracking); return EXIT_SUCCESS; } diff --git a/Moco/Examples/C++/example3DWalking/subject_walk_armless.osim b/Moco/Examples/C++/example3DWalking/subject_walk_armless.osim index 4985c68c8..c19e9a086 100644 --- a/Moco/Examples/C++/example3DWalking/subject_walk_armless.osim +++ b/Moco/Examples/C++/example3DWalking/subject_walk_armless.osim @@ -11689,6 +11689,247 @@ 10 + + /contactgeometryset/heel_r + + /contactgeometryset/floor + + 400000 + + 2 + + 0.80000000000000004 + + 0.80000000000000004 + + 0.5 + + 0.20000000000000001 + + 300 + + 50 + + + /contactgeometryset/lateralRearfoot_r + /contactgeometryset/floor + + 400000 + + 2 + + 0.80000000000000004 + + 0.80000000000000004 + + 0.5 + + 0.20000000000000001 + + 300 + + 50 + + + /contactgeometryset/lateralMidfoot_r + /contactgeometryset/floor + + 400000 + + 2 + + 0.80000000000000004 + + 0.80000000000000004 + + 0.5 + + 0.20000000000000001 + + 300 + + 50 + + + /contactgeometryset/lateralToe_r + /contactgeometryset/floor + + 400000 + + 2 + + 0.80000000000000004 + + 0.80000000000000004 + + 0.5 + + 0.20000000000000001 + + 300 + + 50 + + + /contactgeometryset/medialToe_r + /contactgeometryset/floor + + 400000 + + 2 + + 0.80000000000000004 + + 0.80000000000000004 + + 0.5 + + 0.20000000000000001 + + 300 + + 50 + + + /contactgeometryset/medialMidfoot_r + /contactgeometryset/floor + + 400000 + + 2 + + 0.80000000000000004 + + 0.80000000000000004 + + 0.5 + + 0.20000000000000001 + + 300 + + 50 + + + /contactgeometryset/heel_l + /contactgeometryset/floor + + 400000 + + 2 + + 0.80000000000000004 + + 0.80000000000000004 + + 0.5 + + 0.20000000000000001 + + 300 + + 50 + + + /contactgeometryset/lateralRearfoot_l + /contactgeometryset/floor + + 400000 + + 2 + + 0.80000000000000004 + + 0.80000000000000004 + + 0.5 + + 0.20000000000000001 + + 300 + + 50 + + + /contactgeometryset/lateralMidfoot_l + /contactgeometryset/floor + + 400000 + + 2 + + 0.80000000000000004 + + 0.80000000000000004 + + 0.5 + + 0.20000000000000001 + + 300 + + 50 + + + /contactgeometryset/lateralToe_l + /contactgeometryset/floor + + 400000 + + 2 + + 0.80000000000000004 + + 0.80000000000000004 + + 0.5 + + 0.20000000000000001 + + 300 + + 50 + + + /contactgeometryset/medialToe_l + /contactgeometryset/floor + + 400000 + + 2 + + 0.80000000000000004 + + 0.80000000000000004 + + 0.5 + + 0.20000000000000001 + + 300 + + 50 + + + /contactgeometryset/medialMidfoot_l + /contactgeometryset/floor + + 400000 + + 2 + + 0.80000000000000004 + + 0.80000000000000004 + + 0.5 + + 0.20000000000000001 + + 300 + + 50 + @@ -11932,7 +12173,72 @@ - + + + /ground + 0 0 -1.5707963267948966 + + + /bodyset/calcn_r + 0.035000000000000003 + 0.0146421 0 -0.0122799 + + + /bodyset/calcn_r + 0.035000000000000003 + 0.084929099999999993 0 0.033864900000000003 + + + /bodyset/calcn_r + 0.035000000000000003 + 0.153362 0 0.059482899999999998 + + + /bodyset/calcn_r + 0.035000000000000003 + 0.22301399999999999 0 0.067046099999999997 + + + /bodyset/calcn_r + 0.035000000000000003 + 0.27436300000000002 0 -0.026039 + + + /bodyset/calcn_r + 0.035000000000000003 + 0.20363700000000001 0 -0.039868800000000003 + + + /bodyset/calcn_l + 0.035000000000000003 + 0.0146421 0 0.0122799 + + + /bodyset/calcn_l + 0.035000000000000003 + 0.084929099999999993 0 -0.033864900000000003 + + + /bodyset/calcn_l + 0.035000000000000003 + 0.153362 0 -0.059482899999999998 + + + /bodyset/calcn_l + 0.035000000000000003 + 0.22301399999999999 0 -0.067046099999999997 + + + /bodyset/calcn_l + 0.035000000000000003 + 0.27436300000000002 0 0.026039 + + + /bodyset/calcn_l + 0.035000000000000003 + 0.20363700000000001 0 0.039868800000000003 + + diff --git a/Moco/Moco/MocoTrack.cpp b/Moco/Moco/MocoTrack.cpp index 46e675789..071c76b72 100644 --- a/Moco/Moco/MocoTrack.cpp +++ b/Moco/Moco/MocoTrack.cpp @@ -121,9 +121,13 @@ MocoStudy MocoTrack::initialize() { // ================= MocoCasADiSolver& solver = study.initCasADiSolver(); solver.set_num_mesh_intervals(m_timeInfo.numMeshIntervals); - solver.set_multibody_dynamics_mode("explicit"); - solver.set_optim_convergence_tolerance(1e-2); - solver.set_optim_constraint_tolerance(1e-2); + solver.set_multibody_dynamics_mode("implicit"); + solver.set_minimize_implicit_multibody_accelerations(true); + solver.set_implicit_multibody_accelerations_weight( + 1e-4 / model.getNumCoordinates()); + solver.set_implicit_multibody_acceleration_bounds({-250, 250}); + solver.set_optim_convergence_tolerance(1e-3); + solver.set_optim_constraint_tolerance(1e-3); solver.set_optim_finite_difference_scheme("forward"); // Set the problem guess. From 3757261b4ca1be39cb4fd991f3d8e8b164f23bb0 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Mon, 13 Apr 2020 10:04:52 -0700 Subject: [PATCH 6/6] Various updates to MocoTrack example --- .../C++/example3DWalking/coordinates.sto | 2 +- .../C++/example3DWalking/exampleMocoTrack.cpp | 37 +++++++++++++------ .../subject_walk_armless.osim | 4 +- Moco/Moco/MocoTrack.cpp | 2 +- 4 files changed, 29 insertions(+), 16 deletions(-) diff --git a/Moco/Examples/C++/example3DWalking/coordinates.sto b/Moco/Examples/C++/example3DWalking/coordinates.sto index 205bb1383..2241d1c81 100644 --- a/Moco/Examples/C++/example3DWalking/coordinates.sto +++ b/Moco/Examples/C++/example3DWalking/coordinates.sto @@ -4,7 +4,7 @@ nRows=1350 nColumns=40 inDegrees=yes endheader -time /jointset/ground_pelvis/pelvis_tilt/value /jointset/ground_pelvis/pelvis_list/value /jointset/ground_pelvis/pelvis_rotation/value /jointset/ground_pelvis/pelvis_tx/value /jointset/ground_pelvis/pelvis_ty/value /jointset/ground_pelvis/pelvis_tz/value /jointset/hip_r/hip_flexion_r/value /jointset/hip_r/hip_adduction_r/value /jointset/hip_r/hip_rotation_r/value /jointset/walker_knee_r/knee_angle_r/value /jointset/patellofemoral_r/knee_angle_r_beta/value /jointset/ankle_r/ankle_angle_r/value subtalar_angle_r mtp_angle_r /jointset/hip_l/hip_flexion_l/value /jointset/hip_l/hip_adduction_l/value /jointset/hip_l/hip_rotation_l/value /jointset/walker_knee_l/knee_angle_l/value /jointset/patellofemoral_l/knee_angle_l_beta/value /jointset/ankle_l/ankle_angle_l/value subtalar_angle_l mtp_angle_l /jointset/back/lumbar_extension/value /jointset/back/lumbar_bending/value /jointset/back/lumbar_rotation/value /jointset/acromial_r/arm_flex_r/value /jointset/acromial_r/arm_add_r/value /jointset/acromial_r/arm_rot_r/value /jointset/elbow_r/elbow_flex_r/value pro_sup_r/value wrist_flex_r/value wrist_dev_r/value /jointset/acromial_l/arm_flex_l/value /jointset/acromial_l/arm_add_l/value /jointset/acromial_l/arm_rot_l/value /jointset/elbow_l/elbow_flex_l/value pro_sup_l/value wrist_flex_l/value wrist_dev_l/value +time /jointset/ground_pelvis/pelvis_tilt/value /jointset/ground_pelvis/pelvis_list/value /jointset/ground_pelvis/pelvis_rotation/value /jointset/ground_pelvis/pelvis_tx/value /jointset/ground_pelvis/pelvis_ty/value /jointset/ground_pelvis/pelvis_tz/value /jointset/hip_r/hip_flexion_r/value /jointset/hip_r/hip_adduction_r/value /jointset/hip_r/hip_rotation_r/value /jointset/walker_knee_r/knee_angle_r/value knee_angle_r_beta/value /jointset/ankle_r/ankle_angle_r/value subtalar_angle_r mtp_angle_r /jointset/hip_l/hip_flexion_l/value /jointset/hip_l/hip_adduction_l/value /jointset/hip_l/hip_rotation_l/value /jointset/walker_knee_l/knee_angle_l/value knee_angle_l_beta/value /jointset/ankle_l/ankle_angle_l/value subtalar_angle_l mtp_angle_l lumbar_extension/value lumbar_bending/value lumbar_rotation/value arm_flex_r/value arm_add_r/value arm_rot_r/value elbow_flex_r/value pro_sup_r/value wrist_flex_r/value wrist_dev_r/value arm_flex_l/value arm_add_l/value arm_rot_l/value elbow_flex_l/value pro_sup_l/value wrist_flex_l/value wrist_dev_l/value 0.45000000 0.09205103 -2.62004342 8.24616452 0.00987536 0.96796985 -0.23591827 22.72236815 2.25027648 -10.98568412 21.66554404 0.37813508 1.11190758 0.00000000 0.00000000 3.13209472 -6.76984074 0.40993409 56.58663166 0.98762304 -15.84704902 0.00000000 0.00000000 -7.09465432 4.50637853 -11.37733924 -1.96965993 -3.22718812 -25.97469731 31.19903290 54.25053307 0.00000000 0.00000000 6.44290483 -8.47862584 -0.13079848 50.71868877 50.88654014 0.00000000 0.00000000 0.45100000 0.12743263 -2.63117095 8.24191750 0.01116159 0.96813874 -0.23579893 22.62090835 2.29021992 -10.97293980 21.70455667 0.37881598 1.23457806 -0.00000000 0.00000000 3.30879214 -6.81368150 0.39172851 56.91455847 0.99334644 -15.97792993 -0.00000000 -0.00000000 -7.12690401 4.51996701 -11.32114438 -1.95457397 -3.23167696 -25.94707837 31.21379253 54.25014688 -0.00000000 -0.00000000 6.43319765 -8.45976796 -0.16705849 50.61743762 50.88424544 -0.00000000 -0.00000000 0.45200000 0.16276613 -2.64184109 8.23732895 0.01244562 0.96830835 -0.23567981 22.51864828 2.32995416 -10.96004098 21.74224426 0.37947375 1.35712308 0.00000000 0.00000000 3.48529886 -6.85667506 0.37350714 57.24015597 0.99902919 -16.10191006 0.00000000 -0.00000000 -7.15928798 4.53300431 -11.26439744 -1.93925727 -3.23628444 -25.91868234 31.22886860 54.24972139 -0.00000000 -0.00000000 6.42352711 -8.44125133 -0.20317978 50.51649445 50.88193916 -0.00000000 0.00000000 diff --git a/Moco/Examples/C++/example3DWalking/exampleMocoTrack.cpp b/Moco/Examples/C++/example3DWalking/exampleMocoTrack.cpp index 55507f5d6..49d161588 100644 --- a/Moco/Examples/C++/example3DWalking/exampleMocoTrack.cpp +++ b/Moco/Examples/C++/example3DWalking/exampleMocoTrack.cpp @@ -162,11 +162,11 @@ void muscleDrivenStateTracking( // Create the base Model by passing in the model file. ModelProcessor("subject_walk_armless.osim") | // Remove all the muscles in the model's ForceSet. - ModOpRemoveMuscles() | + ModOpRemoveMuscles();// | // Add CoordinateActuators to the model degrees-of-freedom. This // ignores the pelvis coordinates which already have residual // CoordinateActuators. - ModOpAddReserves(250); + //ModOpAddReserves(250); std::vector forceNamesRightFoot = {"forceset/contactHeel_r", "forceset/contactLateralRearfoot_r", @@ -201,13 +201,15 @@ void muscleDrivenStateTracking( } track.setModel(modelProcessor); + modelProcessor.process().print("subject_walk_armless_torque_driven.osim"); // Construct a TableProcessor of the coordinate data and pass it to the // tracking tool. TableProcessors can be used in the same way as // ModelProcessors by appending TableOperators to modify the base table. // A TableProcessor with no operators, as we have here, simply returns the // base table. - track.setStatesReference(TableProcessor("coordinates.sto")); + auto tableProcessor = TableProcessor("coordinates.sto"); + track.setStatesReference(tableProcessor); track.set_states_global_tracking_weight(10); MocoWeightSet stateWeights; @@ -227,7 +229,7 @@ void muscleDrivenStateTracking( // Initial time, final time, and mesh interval. track.set_initial_time(0.81); track.set_final_time(1.65); - track.set_mesh_interval(0.08); + track.set_mesh_interval(0.05); // Instead of calling solve(), call initialize() to receive a pre-configured // MocoStudy object based on the settings above. Use this to customize the @@ -236,21 +238,32 @@ void muscleDrivenStateTracking( // Get a reference to the MocoControlGoal that is added to every MocoTrack // problem by default. - //MocoProblem& problem = study.updProblem(); + MocoProblem& problem = study.updProblem(); + + + TimeSeriesTable refTable("muscle_driven_state_tracking_tracked_states.sto"); + auto startIndex = refTable.getNearestRowIndexForTime(0.81); + for (auto refLabel : refTable.getColumnLabels()) { + if (refLabel.find("/jointset/ground_pelvis") != std::string::npos) { + auto refCol = refTable.getDependentColumn(refLabel); + problem.setStateInfo(refLabel, {}, refCol[startIndex]); + } + } + //MocoControlGoal& effort = // dynamic_cast(problem.updGoal("control_effort")); // Put a large weight on the pelvis CoordinateActuators, which act as the // residual, or 'hand-of-god', forces which we would like to keep as small // as possible. - // Model model = modelProcessor.process(); - // for (const auto& coordAct : model.getComponentList()) { - // auto coordPath = coordAct.getAbsolutePathString(); - // if (coordPath.find("pelvis") != std::string::npos) { - // effort.setWeightForControl(coordPath, 10); - // } + //Model model = modelProcessor.process(); + //for (const auto& coordAct : model.getComponentList()) { + // auto coordPath = coordAct.getAbsolutePathString(); + // if (coordPath.find("pelvis") != std::string::npos) { + // effort.setWeightForControl(coordPath, 10); + // } //} - + // // Solve and visualize. MocoSolution solution = study.solve(); study.visualize(solution); diff --git a/Moco/Examples/C++/example3DWalking/subject_walk_armless.osim b/Moco/Examples/C++/example3DWalking/subject_walk_armless.osim index c19e9a086..92e4cea01 100644 --- a/Moco/Examples/C++/example3DWalking/subject_walk_armless.osim +++ b/Moco/Examples/C++/example3DWalking/subject_walk_armless.osim @@ -2334,7 +2334,7 @@ 0 - -99999.899999999994 99999.899999999994 + -5 5 false @@ -2845,7 +2845,7 @@ 0 - -99999.899999999994 99999.899999999994 + -5 5 false diff --git a/Moco/Moco/MocoTrack.cpp b/Moco/Moco/MocoTrack.cpp index 071c76b72..a674df114 100644 --- a/Moco/Moco/MocoTrack.cpp +++ b/Moco/Moco/MocoTrack.cpp @@ -124,7 +124,7 @@ MocoStudy MocoTrack::initialize() { solver.set_multibody_dynamics_mode("implicit"); solver.set_minimize_implicit_multibody_accelerations(true); solver.set_implicit_multibody_accelerations_weight( - 1e-4 / model.getNumCoordinates()); + 1e-3 / model.getNumCoordinates()); solver.set_implicit_multibody_acceleration_bounds({-250, 250}); solver.set_optim_convergence_tolerance(1e-3); solver.set_optim_constraint_tolerance(1e-3);