From 8f4ba60109377be56a5a04c72549817348f27368 Mon Sep 17 00:00:00 2001 From: nickbianco Date: Mon, 14 Oct 2019 09:20:12 -0700 Subject: [PATCH 01/11] Edits to preprint branch for inverse problem --- .../Components/DeGrooteFregly2016Muscle.cpp | 26 +++++++++-- .../Components/DeGrooteFregly2016Muscle.h | 14 ++++++ Moco/Moco/MocoInverse.cpp | 8 +++- Moco/Moco/MocoInverse.h | 3 ++ Moco/Moco/MocoStudy.cpp | 8 ++-- Moco/Moco/ModelOperators.h | 46 ++++++++++++++++++- Moco/Tests/testMocoInverse.cpp | 15 ++++++ 7 files changed, 109 insertions(+), 11 deletions(-) diff --git a/Moco/Moco/Components/DeGrooteFregly2016Muscle.cpp b/Moco/Moco/Components/DeGrooteFregly2016Muscle.cpp index 24027e75d..72700856a 100644 --- a/Moco/Moco/Components/DeGrooteFregly2016Muscle.cpp +++ b/Moco/Moco/Components/DeGrooteFregly2016Muscle.cpp @@ -53,7 +53,12 @@ void DeGrooteFregly2016Muscle::constructProperties() { constructProperty_activation_time_constant(0.015); constructProperty_deactivation_time_constant(0.060); constructProperty_default_activation(0.5); + constructProperty_clamp_activation(false); + constructProperty_minimum_activation(0.0); constructProperty_default_normalized_tendon_force(0.5); + constructProperty_clamp_normalized_tendon_length(false); + constructProperty_minimum_normalized_tendon_length(1.0); + constructProperty_maximum_normalized_tendon_length(2.5); constructProperty_active_force_width_scale(1.0); constructProperty_fiber_damping(0.0); constructProperty_tendon_strain_at_one_norm_force(0.049); @@ -182,7 +187,11 @@ void DeGrooteFregly2016Muscle::computeStateVariableDerivatives( // Activation dynamics. // -------------------- if (!get_ignore_activation_dynamics()) { - const auto& activation = getActivation(s); + auto activation = getActivation(s); + if (get_clamp_activation()) { + activation = + SimTK::clamp(get_minimum_activation(), activation, 1.0); + } const auto& excitation = getControl(s); static const double actTimeConst = get_activation_time_constant(); static const double deactTimeConst = get_deactivation_time_constant(); @@ -212,9 +221,15 @@ void DeGrooteFregly2016Muscle::computeStateVariableDerivatives( // normalized tendon length, so using the chain rule, to get // normalized tendon force derivative with respect to time, we // multiply by normalized fiber velocity. + double normTendonLength = mli.normTendonLength; + if (get_clamp_normalized_tendon_length()) { + normTendonLength = SimTK::clamp( + get_minimum_normalized_tendon_length(), normTendonLength, + get_maximum_normalized_tendon_length()); + } normTendonForceDerivative = fvi.normTendonVelocity * - calcTendonForceMultiplierDerivative(mli.normTendonLength); + calcTendonForceMultiplierDerivative(normTendonLength); } else { normTendonForceDerivative = getDiscreteVariableValue( s, DERIVATIVE_NORMALIZED_TENDON_FORCE_NAME); @@ -451,8 +466,9 @@ void DeGrooteFregly2016Muscle::calcMuscleLengthInfo( // TODO the Millard model sets fiber velocity to zero when the // tendon is buckling, but this may create a discontinuity. std::cout << "Warning: DeGrooteFregly2016Muscle '" << getName() - << "' is buckling (length < tendon_slack_length) at time " - << s.getTime() << " s." << std::endl; + << "' is buckling (normFiberLength = " << mli.normFiberLength + << ", normTendonLength = " << mli.normTendonLength << ") " + << "at time " << s.getTime() << " s." << std::endl; } } @@ -551,7 +567,7 @@ void DeGrooteFregly2016Muscle::computeInitialFiberEquilibrium( mdi.tendonForce, mdi.fiberForceAlongTendon); }; - const auto equilNormTendonForce = solveBisection(calcResidual, + const auto equilNormTendonForce = solveBisection(calcResidual, m_minNormTendonForce, m_maxNormTendonForce, 1e-10, 1e-10, 100); setNormalizedTendonForce(s, equilNormTendonForce); diff --git a/Moco/Moco/Components/DeGrooteFregly2016Muscle.h b/Moco/Moco/Components/DeGrooteFregly2016Muscle.h index 81e6dd927..cf6e8d8aa 100644 --- a/Moco/Moco/Components/DeGrooteFregly2016Muscle.h +++ b/Moco/Moco/Components/DeGrooteFregly2016Muscle.h @@ -94,9 +94,23 @@ class OSIMMOCO_API DeGrooteFregly2016Muscle : public Muscle { OpenSim_DECLARE_PROPERTY(default_activation, double, "Value of activation in the default state returned by " "initSystem()."); + OpenSim_DECLARE_PROPERTY(clamp_activation, bool, + "Clamp activations between the value set by the " + "'minimum_activation' property and 1."); + OpenSim_DECLARE_PROPERTY(minimum_activation, double, + "The enforced activation lower bound if 'clamp_activation' is set " + "to true. Default: 0."); OpenSim_DECLARE_PROPERTY(default_normalized_tendon_force, double, "Value of normalized tendon force in the default state returned by " "initSystem()."); + OpenSim_DECLARE_PROPERTY(clamp_normalized_tendon_length, bool, + "Clamp normalized tendon length between the value set by the " + "'minimum_normalized_tendon_length' and " + "'maximum_normalized_tendon_length' properties."); + OpenSim_DECLARE_PROPERTY(minimum_normalized_tendon_length, double, + "Normalized tendon length lower bound. Default: 1."); + OpenSim_DECLARE_PROPERTY(maximum_normalized_tendon_length, double, + "Normalized tendon length upper bound. Default: 2.5."); OpenSim_DECLARE_PROPERTY(active_force_width_scale, double, "Scale factor for the width of the active force-length curve. " "Larger values make the curve wider. " diff --git a/Moco/Moco/MocoInverse.cpp b/Moco/Moco/MocoInverse.cpp index 2ce75c9c6..1a6a09c25 100644 --- a/Moco/Moco/MocoInverse.cpp +++ b/Moco/Moco/MocoInverse.cpp @@ -43,6 +43,8 @@ void MocoInverse::constructProperties() { constructProperty_tolerance(1e-3); constructProperty_output_paths(); constructProperty_reserves_weight(1.0); + constructProperty_auxiliary_derivatives_weight(0.01); + constructProperty_auxiliary_derivatives_bound(10.0); } MocoStudy MocoInverse::initialize() const { return initializeInternal().first; } @@ -110,7 +112,11 @@ std::pair MocoInverse::initializeInternal() const { solver.set_optim_constraint_tolerance(get_tolerance()); solver.set_interpolate_control_midpoints(false); solver.set_minimize_implicit_auxiliary_derivatives(true); - solver.set_implicit_auxiliary_derivatives_weight(0.01); + solver.set_implicit_auxiliary_derivatives_weight( + get_auxiliary_derivatives_weight()); + solver.set_implicit_auxiliary_derivative_bounds( + {-get_auxiliary_derivatives_bound(), + get_auxiliary_derivatives_bound()}); // The sparsity detection works fine with DeGrooteFregly2016Muscle. solver.set_optim_sparsity_detection("random"); // Forward is 3x faster than central. diff --git a/Moco/Moco/MocoInverse.h b/Moco/Moco/MocoInverse.h index 537a17666..c104d70ab 100644 --- a/Moco/Moco/MocoInverse.h +++ b/Moco/Moco/MocoInverse.h @@ -135,6 +135,9 @@ class OSIMMOCO_API MocoInverse : public MocoTool { "the model operator ModOpAddReserves, which names each appended " "actuator in this format. Default weight: 1.") + OpenSim_DECLARE_PROPERTY(auxiliary_derivatives_weight, double, "TODO") + OpenSim_DECLARE_PROPERTY(auxiliary_derivatives_bound, double, "TODO") + MocoInverse() { constructProperties(); } void setKinematics(TableProcessor kinematics) { diff --git a/Moco/Moco/MocoStudy.cpp b/Moco/Moco/MocoStudy.cpp index c05be7aae..634eb607c 100644 --- a/Moco/Moco/MocoStudy.cpp +++ b/Moco/Moco/MocoStudy.cpp @@ -82,16 +82,16 @@ MocoSolution MocoStudy::solve() const { // Temporarily disable printing of negative muscle force warnings so the // output stream isn't flooded while computing finite differences. - bool oldWarningFlag = Muscle::getPrintWarnings(); - Muscle::setPrintWarnings(false); + //bool oldWarningFlag = Muscle::getPrintWarnings(); + //Muscle::setPrintWarnings(false); MocoSolution solution; try { solution = get_solver().solve(); } catch (const Exception&) { - Muscle::setPrintWarnings(oldWarningFlag); + //Muscle::setPrintWarnings(oldWarningFlag); throw; } - Muscle::setPrintWarnings(oldWarningFlag); + //Muscle::setPrintWarnings(oldWarningFlag); bool originallySealed = solution.isSealed(); if (get_write_solution() != "false") { diff --git a/Moco/Moco/ModelOperators.h b/Moco/Moco/ModelOperators.h index a86e8ce59..769a34ded 100644 --- a/Moco/Moco/ModelOperators.h +++ b/Moco/Moco/ModelOperators.h @@ -175,7 +175,51 @@ class OSIMMOCO_API ModOpScaleMaxIsometricForce : public ModelOperator { void operate(Model& model, const std::string&) const override { model.finalizeFromProperties(); for (auto& muscle : model.updComponentList()) { - muscle.set_max_isometric_force(get_scale_factor()); + muscle.set_max_isometric_force( + get_scale_factor() * muscle.get_max_isometric_force()); + } + } +}; + +/// Scale the tendon slack length for all muscles in the model. +class OSIMMOCO_API ModOpScaleTendonSlackLength : public ModelOperator { + OpenSim_DECLARE_CONCRETE_OBJECT(ModOpScaleTendonSlackLength, ModelOperator); + OpenSim_DECLARE_PROPERTY( + scale_factor, double, "The tendon slack length scale factor."); + +public: + ModOpScaleTendonSlackLength() { constructProperty_scale_factor(1); } + ModOpScaleTendonSlackLength(double scaleFactor) + : ModOpScaleTendonSlackLength() { + set_scale_factor(scaleFactor); + } + void operate(Model& model, const std::string&) const override { + model.finalizeFromProperties(); + for (auto& muscle : model.updComponentList()) { + muscle.set_tendon_slack_length( + get_scale_factor() * muscle.get_tendon_slack_length()); + } + } +}; + +/// Scale the optimal fiber length for all muscles in the model. +class OSIMMOCO_API ModOpScaleOptimalFiberLength : public ModelOperator { + OpenSim_DECLARE_CONCRETE_OBJECT( + ModOpScaleOptimalFiberLength, ModelOperator); + OpenSim_DECLARE_PROPERTY( + scale_factor, double, "The optimal fiber length scale factor."); + +public: + ModOpScaleOptimalFiberLength() { constructProperty_scale_factor(1); } + ModOpScaleOptimalFiberLength(double scaleFactor) + : ModOpScaleOptimalFiberLength() { + set_scale_factor(scaleFactor); + } + void operate(Model& model, const std::string&) const override { + model.finalizeFromProperties(); + for (auto& muscle : model.updComponentList()) { + muscle.set_optimal_fiber_length( + get_scale_factor() * muscle.get_optimal_fiber_length()); } } }; diff --git a/Moco/Tests/testMocoInverse.cpp b/Moco/Tests/testMocoInverse.cpp index d86b3d738..aa4867d47 100644 --- a/Moco/Tests/testMocoInverse.cpp +++ b/Moco/Tests/testMocoInverse.cpp @@ -147,6 +147,21 @@ TEST_CASE("MocoInverse Rajagopal2016, 18 muscles") { MocoSolution solution = inverse.solve().getMocoSolution(); solution.write("testMocoInverse_subject_18musc_solution.sto"); + Model model = modelProcessor.process(); + std::vector outputPaths; + + const auto& muscles = model.getMuscles(); + for (int i = 0; i < muscles.getSize(); ++i) { + const auto& muscle = muscles.get(i); + outputPaths.push_back(muscle.getAbsolutePathString() + + "\\|tendon_length"); + outputPaths.push_back( + muscle.getAbsolutePathString() + "\\|normalized_fiber_length"); + } + + TimeSeriesTable outputTable = analyze(model, solution, outputPaths); + writeTableToFile(outputTable, "output_table.sto"); + MocoTrajectory std("std_testMocoInverse_subject_18musc_solution.sto"); const auto expected = std.getControlsTrajectory(); CHECK(std.compareContinuousVariablesRMS(solution, From 4ba8371432ef673db32c5e5709c2d36e22742547 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Mon, 14 Oct 2019 15:18:03 -0700 Subject: [PATCH 02/11] Fix python bindings issue // add /bigobj flag --- Moco/Bindings/Python/CMakeLists.txt | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/Moco/Bindings/Python/CMakeLists.txt b/Moco/Bindings/Python/CMakeLists.txt index 79799cca2..5fb7b6c41 100644 --- a/Moco/Bindings/Python/CMakeLists.txt +++ b/Moco/Bindings/Python/CMakeLists.txt @@ -159,23 +159,23 @@ macro(MocoAddPythonModule) # We purposefully wrap deprecated functions, so no need to see such # warnings. - if(${CMAKE_CXX_COMPILER_ID} MATCHES "GNU" OR - ${CMAKE_CXX_COMPILER_ID} MATCHES "Clang") + if("${CMAKE_CXX_COMPILER_ID}" MATCHES "GNU" OR + "${CMAKE_CXX_COMPILER_ID}" MATCHES "Clang") # Turn off optimization for SWIG wrapper code. Optimization slows down # compiling and also requires lots of memory. Also, there's not much to # gain from an optimized wrapper file. # Note that the last optimization flag is what counts for GCC. So an -O0 # later on the command line overrides a previous -O2. set(_COMPILE_FLAGS "-O0 -Wno-deprecated-declarations") - elseif(${CMAKE_CXX_COMPILER_ID} MATCHES "MSVC") + elseif("${CMAKE_CXX_COMPILER_ID}" MATCHES "MSVC") # TODO disable optimization on Windows. # Don't warn about: # 4996: deprecated functions. # 4114: "const const T" - set(_COMPILE_FLAGS "/wd4996 /wd4114") + set(_COMPILE_FLAGS "/bigobj /wd4996 /wd4114") endif() set_source_files_properties("${_output_cxx_file}" - PROPERTIES COMPILE_FLAGS "${_COMPILE_FLAGS}") + PROPERTIES COMPILE_FLAGS "${_COMPILE_FLAGS}") add_library(${_libname} SHARED ${_output_cxx_file} ${_output_header_file}) From df8f790f34b8dc01d62532bf744e24353144e8fd Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Tue, 15 Oct 2019 16:55:52 -0700 Subject: [PATCH 03/11] Clamp tendon forces --- .../Components/DeGrooteFregly2016Muscle.cpp | 23 ++++++++++++++++--- 1 file changed, 20 insertions(+), 3 deletions(-) diff --git a/Moco/Moco/Components/DeGrooteFregly2016Muscle.cpp b/Moco/Moco/Components/DeGrooteFregly2016Muscle.cpp index 72700856a..0bee1db05 100644 --- a/Moco/Moco/Components/DeGrooteFregly2016Muscle.cpp +++ b/Moco/Moco/Components/DeGrooteFregly2016Muscle.cpp @@ -255,8 +255,14 @@ void DeGrooteFregly2016Muscle::calcMuscleLengthInfoHelper( if (ignoreTendonCompliance) { mli.normTendonLength = 1.0; } else { + double normTendonForceClamped = normTendonForce; + if (get_clamp_normalized_tendon_length()) { + normTendonForceClamped = SimTK::clamp( + getMinNormalizedTendonForce(), + normTendonForceClamped, getMaxNormalizedTendonForce()); + } mli.normTendonLength = - calcTendonForceLengthInverseCurve(normTendonForce); + calcTendonForceLengthInverseCurve(normTendonForceClamped); } mli.tendonStrain = mli.normTendonLength - 1.0; mli.tendonLength = get_tendon_slack_length() * mli.normTendonLength; @@ -291,7 +297,13 @@ void DeGrooteFregly2016Muscle::calcFiberVelocityInfoHelper( const SimTK::Real& normTendonForceDerivative = SimTK::NaN) const { if (isTendonDynamicsExplicit && !ignoreTendonCompliance) { - const auto& normFiberForce = normTendonForce / mli.cosPennationAngle; + double normTendonForceClamped = normTendonForce; + if (get_clamp_normalized_tendon_length()) { + normTendonForceClamped = SimTK::clamp(getMinNormalizedTendonForce(), + normTendonForceClamped, getMaxNormalizedTendonForce()); + } + const auto& normFiberForce = + normTendonForceClamped / mli.cosPennationAngle; fvi.fiberForceVelocityMultiplier = (normFiberForce - mli.fiberPassiveForceLengthMultiplier) / (activation * mli.fiberActiveForceLengthMultiplier); @@ -377,7 +389,12 @@ void DeGrooteFregly2016Muscle::calcMuscleDynamicsInfoHelper( mdi.normTendonForce = mdi.normFiberForce * mli.cosPennationAngle; mdi.tendonForce = mdi.fiberForceAlongTendon; } else { - mdi.normTendonForce = normTendonForce; + double normTendonForceClamped = normTendonForce; + if (get_clamp_normalized_tendon_length()) { + normTendonForceClamped = SimTK::clamp(getMinNormalizedTendonForce(), + normTendonForceClamped, getMaxNormalizedTendonForce()); + } + mdi.normTendonForce = normTendonForceClamped; mdi.tendonForce = maxIsometricForce * mdi.normTendonForce; } From 4e77683b6c8e1dbaa5937bc994722f3de92e3f05 Mon Sep 17 00:00:00 2001 From: Christopher Dembia Date: Wed, 16 Oct 2019 13:05:24 -0700 Subject: [PATCH 04/11] Add more thorough equilibrium test. --- .../Components/DeGrooteFregly2016Muscle.cpp | 9 +- Moco/Tests/testDeGrooteFregly2016Muscle.cpp | 133 ++++++++++++------ 2 files changed, 93 insertions(+), 49 deletions(-) diff --git a/Moco/Moco/Components/DeGrooteFregly2016Muscle.cpp b/Moco/Moco/Components/DeGrooteFregly2016Muscle.cpp index 0bee1db05..cc557a6b7 100644 --- a/Moco/Moco/Components/DeGrooteFregly2016Muscle.cpp +++ b/Moco/Moco/Components/DeGrooteFregly2016Muscle.cpp @@ -663,11 +663,12 @@ SimTK::Real DeGrooteFregly2016Muscle::solveBisection( row[0] = calcResidual(x[i]); table.appendRow(x[i], row); } - writeTableToFile(table, "DEBUG_solveBisection_residual.sto"); + writeTableToFile( + table, "DeGrooteFregly2016Muscle_solveBisection_residual.sto"); + OPENSIM_THROW_FRMOBJ(Exception, + format("Function has same sign at bounds of %f and %f.", left, + right)); } - OPENSIM_THROW_IF_FRMOBJ(sameSign, Exception, - format("Function has same sign at bounds of %f and %f.", left, - right)); SimTK::Real residualMidpoint; SimTK::Real residualLeft = calcResidual(left); diff --git a/Moco/Tests/testDeGrooteFregly2016Muscle.cpp b/Moco/Tests/testDeGrooteFregly2016Muscle.cpp index bedfc12ef..e48f63a92 100644 --- a/Moco/Tests/testDeGrooteFregly2016Muscle.cpp +++ b/Moco/Tests/testDeGrooteFregly2016Muscle.cpp @@ -591,9 +591,9 @@ TEST_CASE("DeGrooteFregly2016Muscle basics") { mutMuscle.set_active_force_width_scale(1.2); muscle.setActivation(state, 1.0); const double normFiberLength = 0.8; - coord.setValue(state, - normFiberLength * muscle.get_optimal_fiber_length() + - muscle.get_tendon_slack_length()); + coord.setValue( + state, normFiberLength * muscle.get_optimal_fiber_length() + + muscle.get_tendon_slack_length()); coord.setSpeedValue(state, 0.0); model.realizePosition(state); @@ -617,7 +617,7 @@ TEST_CASE("DeGrooteFregly2016Muscle basics") { FiberForceFunction fiberForceFunc(muscle, state, false); SimTK::Differentiator diffFiberStiffness(fiberForceFunc); SimTK::Real fiberStiffness = diffFiberStiffness.calcDerivative( - normFiberLength * muscle.get_optimal_fiber_length()); + normFiberLength * muscle.get_optimal_fiber_length()); CHECK(muscle.getFiberStiffness(state) == Approx(fiberStiffness)); FiberForceFunction fiberForceFuncAlongTendon(muscle, state, true); @@ -625,7 +625,8 @@ TEST_CASE("DeGrooteFregly2016Muscle basics") { fiberForceFuncAlongTendon); SimTK::Real fiberStiffnessAlongTendon = diffFiberStiffnessAlongTendon.calcDerivative( - normFiberLength * muscle.get_optimal_fiber_length()); + normFiberLength * + muscle.get_optimal_fiber_length()); CHECK(muscle.getFiberStiffnessAlongTendon(state) == Approx(fiberStiffnessAlongTendon)); @@ -740,38 +741,6 @@ TEST_CASE("DeGrooteFregly2016Muscle basics") { CHECK(muscle.getStress(state) == Approx((fv + fpass) * cosPenn)); } - SECTION("initial equilibrium") { - auto& mutMuscle = - model.updComponent("muscle"); - mutMuscle.set_ignore_tendon_compliance(false); - mutMuscle.set_tendon_compliance_dynamics_mode("explicit"); - - const double pennOpt = 0.12; - double cosPenn = cos(pennOpt); - mutMuscle.set_pennation_angle_at_optimal(pennOpt); - state = model.initSystem(); - muscle.setActivation(state, 1.0); - const double muscleLength = - muscle.get_optimal_fiber_length() * cosPenn + - muscle.get_tendon_slack_length(); - coord.setValue(state, muscleLength); - const double Vmax = muscle.get_optimal_fiber_length() * - muscle.get_max_contraction_velocity(); - const double muscleTendonVelocity = -0.21 * Vmax; - coord.setSpeedValue(state, muscleTendonVelocity); - - model.realizeDynamics(state); - muscle.computeInitialFiberEquilibrium(state); - - model.realizeDynamics(state); - CHECK(muscle.getNormalizedTendonForceDerivative(state) == - Approx(0.0).margin(1e-6)); - - mutMuscle.set_tendon_compliance_dynamics_mode("implicit"); - model.realizeDynamics(state); - CHECK(muscle.getEquilibriumResidual(state) == Approx(0.0)); - } - SECTION("tendon compliance") { auto& mutMuscle = model.updComponent("muscle"); @@ -986,9 +955,8 @@ TEST_CASE("DeGrooteFregly2016Muscle basics") { } } -Model createHangingMuscleModel( - bool ignoreActivationDynamics, bool ignoreTendonCompliance, - bool isTendonDynamicsExplicit) { +Model createHangingMuscleModel(bool ignoreActivationDynamics, + bool ignoreTendonCompliance, bool isTendonDynamicsExplicit) { Model model; model.setName("isometric_muscle"); model.set_gravity(SimTK::Vec3(9.81, 0, 0)); @@ -1042,9 +1010,8 @@ TEMPLATE_TEST_CASE("Hanging muscle minimum time", "", MocoCasADiSolver) { SimTK::Real initHeight = 0.15; SimTK::Real finalHeight = 0.14; - Model model = createHangingMuscleModel( - ignoreActivationDynamics, ignoreTendonCompliance, - isTendonDynamicsExplicit); + Model model = createHangingMuscleModel(ignoreActivationDynamics, + ignoreTendonCompliance, isTendonDynamicsExplicit); SimTK::State state = model.initSystem(); @@ -1070,8 +1037,8 @@ TEMPLATE_TEST_CASE("Hanging muscle minimum time", "", MocoCasADiSolver) { } if (!ignoreTendonCompliance) { if (isTendonDynamicsExplicit) { - auto* initial_equilibrium = - problem.addGoal(); + auto* initial_equilibrium = + problem.addGoal(); initial_equilibrium->setName("initial_force_equilibrium"); // problem.setStateInfo("/forceset/actuator/normalized_tendon_force", // {0, 2}); @@ -1204,3 +1171,79 @@ TEMPLATE_TEST_CASE("Hanging muscle minimum time", "", MocoCasADiSolver) { CHECK(error < 0.015); } } + +TEST_CASE("Fiber equilibrium") { + Model model; + model.setName("muscle"); + auto* body = new Body("body", 0.5, SimTK::Vec3(0), SimTK::Inertia(0)); + model.addComponent(body); + auto* joint = new SliderJoint("joint", model.getGround(), *body); + auto& coord = joint->updCoordinate(SliderJoint::Coord::TranslationX); + coord.setName("x"); + model.addComponent(joint); + auto* musclePtr = new DeGrooteFregly2016Muscle(); + musclePtr->set_ignore_tendon_compliance(true); + musclePtr->set_fiber_damping(0); + musclePtr->setName("muscle"); + musclePtr->addNewPathPoint("origin", model.updGround(), SimTK::Vec3(0)); + musclePtr->addNewPathPoint("insertion", *body, SimTK::Vec3(0)); + model.addComponent(musclePtr); + + auto& muscle = model.getComponent("muscle"); + auto& mutMuscle = model.updComponent("muscle"); + mutMuscle.set_ignore_tendon_compliance(false); + + const double pennOpt = 0.12; + double cosPenn = cos(pennOpt); + mutMuscle.set_pennation_angle_at_optimal(pennOpt); + SimTK::State state = model.initSystem(); + const double Vmax = muscle.get_optimal_fiber_length() * + muscle.get_max_contraction_velocity(); + const double muscleTendonVelocity = -0.21 * Vmax; + coord.setSpeedValue(state, muscleTendonVelocity); + + const double L = muscle.get_optimal_fiber_length() * cosPenn + + muscle.get_tendon_slack_length(); + + const auto activations = createVectorLinspace(4, 0.01, 1.0); + const auto lengths = createVectorLinspace(10, 1.1 * L, 0.6 * L); + // Around normalized length of 0.66857, the residual does not cross zero. + for (int ia = 0; ia < activations.size(); ++ia) { + for (int ilen = 0; ilen < lengths.size(); ++ilen) { + std::cout << "activation: " << activations[ia] + << "; normalized length: " << lengths[ilen] / L + << std::endl; + + mutMuscle.set_tendon_compliance_dynamics_mode("explicit"); + + muscle.setActivation(state, activations[ia]); + coord.setValue(state, lengths[ilen]); + + model.realizeDynamics(state); + muscle.computeEquilibrium(state); + + model.realizeDynamics(state); + std::cout << " fiber length: " << muscle.getFiberLength(state) + << "\n normalized fiber length: " + << muscle.getNormalizedFiberLength(state) + << "\n tendon length: " + << muscle.getTendonLength(state) + << "\n normalized tendon length: " + << muscle.getTendonLength(state) / muscle.get_tendon_slack_length() + << "\n normalized tendon force: " + << muscle.getNormalizedTendonForce(state) + << "\n normalized active fiber force along tendon: " + << muscle.getActiveFiberForceAlongTendon(state) / muscle.get_max_isometric_force() + << "\n normalized passive fiber force along tendon: " + << muscle.getPassiveFiberForceAlongTendon(state) / muscle.get_max_isometric_force() + << std::endl; + CHECK(muscle.getNormalizedTendonForceDerivative(state) == + Approx(0.0).margin(1e-6)); + + mutMuscle.set_tendon_compliance_dynamics_mode("implicit"); + model.realizeDynamics(state); + CHECK(muscle.getEquilibriumResidual(state) == Approx(0.0)); + } + } + +} From da6640d89acad28e10668f07ecdea3117b39054f Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Fri, 12 Jun 2020 14:29:03 -0700 Subject: [PATCH 05/11] Muscle explicit tendon dynamics working (first pass) --- .../Components/DeGrooteFregly2016Muscle.h | 2 +- .../MocoInitialForceEquilibriumGoal.cpp | 2 +- Moco/Moco/MocoUtilities.cpp | 20 +-- .../DeGrooteFregly2016MuscleDerivatives.m | 9 +- Moco/Tests/testMocoActuators.cpp | 164 ++++++++---------- 5 files changed, 94 insertions(+), 103 deletions(-) diff --git a/Moco/Moco/Components/DeGrooteFregly2016Muscle.h b/Moco/Moco/Components/DeGrooteFregly2016Muscle.h index 629cdfbdb..7c3c6898c 100644 --- a/Moco/Moco/Components/DeGrooteFregly2016Muscle.h +++ b/Moco/Moco/Components/DeGrooteFregly2016Muscle.h @@ -592,7 +592,7 @@ class OSIMMOCO_API DeGrooteFregly2016Muscle : public Muscle { // The stiffness of the fiber along the direction of the tendon. For // small changes in length parallel to the fiber, this quantity is // d_fiberForceAlongTendon / d_fiberLength = - // d/d_fiberLength(fiberForce * cosPenneationAngle) + // d/d_fiberLength(fiberForce * cosPennationAngle) return fiberStiffness * cosPennationAngle + fiberForce * partialCosPennationAnglePartialFiberLength; } diff --git a/Moco/Moco/MocoGoal/MocoInitialForceEquilibriumGoal.cpp b/Moco/Moco/MocoGoal/MocoInitialForceEquilibriumGoal.cpp index a4f4633f8..ddff368a9 100644 --- a/Moco/Moco/MocoGoal/MocoInitialForceEquilibriumGoal.cpp +++ b/Moco/Moco/MocoGoal/MocoInitialForceEquilibriumGoal.cpp @@ -44,7 +44,7 @@ void MocoInitialForceEquilibriumGoal::initializeOnModelImpl( void MocoInitialForceEquilibriumGoal::calcGoalImpl( const GoalInput& input, SimTK::Vector& goal) const { const auto& s = input.initial_state; - getModel().realizeVelocity(s); + getModel().realizeDynamics(s); if (getModeIsCost()) { for (int i = 0; i < (int)m_muscleRefs.size(); ++i) { const auto residual = m_muscleRefs[i]->getTendonForce(s) - diff --git a/Moco/Moco/MocoUtilities.cpp b/Moco/Moco/MocoUtilities.cpp index e0e1cdb76..9fa3023a5 100644 --- a/Moco/Moco/MocoUtilities.cpp +++ b/Moco/Moco/MocoUtilities.cpp @@ -869,17 +869,17 @@ SimTK::Real OpenSim::solveBisection( maxIterations)); const bool sameSign = calcResidual(left) * calcResidual(right) >= 0; - if (sameSign) { - const auto x = createVectorLinspace(1000, left, right); - TimeSeriesTable table; - table.setColumnLabels({"residual"}); - SimTK::RowVector row(1); - for (int i = 0; i < x.nrow(); ++i) { - row[0] = calcResidual(x[i]); - table.appendRow(x[i], row); - } - // writeTableToFile(table, "DEBUG_solveBisection_residual.sto"); + //if (sameSign) { + const auto x = createVectorLinspace(1000, left, right); + TimeSeriesTable table; + table.setColumnLabels({"residual"}); + SimTK::RowVector row(1); + for (int i = 0; i < x.nrow(); ++i) { + row[0] = calcResidual(x[i]); + table.appendRow(x[i], row); } + writeTableToFile(table, "DEBUG_solveBisection_residual.sto"); + //} OPENSIM_THROW_IF(sameSign, Exception, format("Function has same sign at bounds of %f and %f.", left, right)); diff --git a/Moco/Sandbox/DeGrooteFregly2016MuscleDerivatives.m b/Moco/Sandbox/DeGrooteFregly2016MuscleDerivatives.m index 02cca02c7..73675bc0a 100644 --- a/Moco/Sandbox/DeGrooteFregly2016MuscleDerivatives.m +++ b/Moco/Sandbox/DeGrooteFregly2016MuscleDerivatives.m @@ -1,4 +1,4 @@ -syms b1 b2 b3 b4 lMtilde min_lMtilde c1 c2 c3 lTtilde kT kPE e0 +syms b1 b2 b3 b4 lMtilde min_lMtilde c1 c2 c3 lTtilde kT kPE e0 vMtilde d1 d2 d3 d4 fprintf('Derivative of tendon force length curve \n') fprintf('======================================= \n') @@ -12,6 +12,13 @@ f_act = b1 * exp((-0.5*(lMtilde-b2)^2) / ((b3 + b4*lMtilde)^2)); simplify(diff(f_act, lMtilde)) +fprintf('Derivative of active fiber force velocity curve \n') +fprintf('=============================================== \n') +tempV = d2 * vMtilde + d3; +tempLogArg = tempV + sqrt(tempV^2 + 1.0); +f_v = d1 * log(tempLogArg) + d4; +simplify(diff(f_v, vMtilde)) + fprintf('Derivative of passive fiber force length curve \n') fprintf('============================================== \n') diff --git a/Moco/Tests/testMocoActuators.cpp b/Moco/Tests/testMocoActuators.cpp index ca96e07ee..e9f038fd9 100644 --- a/Moco/Tests/testMocoActuators.cpp +++ b/Moco/Tests/testMocoActuators.cpp @@ -1017,7 +1017,8 @@ TEST_CASE("DeGrooteFregly2016Muscle basics") { } } -Model createHangingMuscleModel(bool ignoreActivationDynamics, +Model createHangingMuscleModel(double optimalFiberLength, + double tendonSlackLength,bool ignoreActivationDynamics, bool ignoreTendonCompliance, bool isTendonDynamicsExplicit) { Model model; model.setName("isometric_muscle"); @@ -1033,10 +1034,9 @@ Model createHangingMuscleModel(bool ignoreActivationDynamics, auto* actu = new DeGrooteFregly2016Muscle(); actu->setName("actuator"); - actu->set_max_isometric_force(30.0); - actu->set_optimal_fiber_length(0.10); - actu->set_tendon_slack_length(0.05); - actu->set_tendon_strain_at_one_norm_force(0.10); + actu->set_max_isometric_force(100.0); + actu->set_optimal_fiber_length(optimalFiberLength); + actu->set_tendon_slack_length(tendonSlackLength); actu->set_ignore_activation_dynamics(ignoreActivationDynamics); actu->set_ignore_tendon_compliance(ignoreTendonCompliance); actu->set_fiber_damping(0.01); @@ -1044,7 +1044,7 @@ Model createHangingMuscleModel(bool ignoreActivationDynamics, actu->set_tendon_compliance_dynamics_mode("implicit"); } actu->set_max_contraction_velocity(10); - actu->set_pennation_angle_at_optimal(0.10); + actu->set_pennation_angle_at_optimal(0); actu->addNewPathPoint("origin", model.updGround(), SimTK::Vec3(0)); actu->addNewPathPoint("insertion", *body, SimTK::Vec3(0)); model.addForce(actu); @@ -1057,10 +1057,7 @@ Model createHangingMuscleModel(bool ignoreActivationDynamics, TEMPLATE_TEST_CASE("Hanging muscle minimum time", "", MocoCasADiSolver) { auto ignoreActivationDynamics = GENERATE(true, false); auto ignoreTendonCompliance = GENERATE(true, false); - auto isTendonDynamicsExplicit = GENERATE(false); - - // TODO: Some problem has a bad initial guess and the constraint violation - // goes to 1e+14. Maybe the bounds on the coordinate should be tighter. + auto isTendonDynamicsExplicit = GENERATE(true, false); std::cout.rdbuf(LogManager::cout.rdbuf()); std::cout.rdbuf(LogManager::cout.rdbuf()); @@ -1069,13 +1066,18 @@ TEMPLATE_TEST_CASE("Hanging muscle minimum time", "", MocoCasADiSolver) { CAPTURE(ignoreTendonCompliance); CAPTURE(isTendonDynamicsExplicit); - SimTK::Real initHeight = 0.15; - SimTK::Real finalHeight = 0.14; + const double optimalFiberLength = 0.1; + const double tendonSlackLength = 0.05; + SimTK::Real initHeight = 0.151; + SimTK::Real finalHeight = 0.15; + MocoBounds heightBounds(0.145, 0.155); + MocoBounds speedBounds(-10, 10); + MocoBounds actuBounds(0.01, 1); - Model model = createHangingMuscleModel(ignoreActivationDynamics, + Model model = createHangingMuscleModel(optimalFiberLength, + tendonSlackLength, ignoreActivationDynamics, ignoreTendonCompliance, isTendonDynamicsExplicit); - - SimTK::State state = model.initSystem(); + model.initSystem(); // Minimum time trajectory optimization. // ------------------------------------- @@ -1085,48 +1087,40 @@ TEMPLATE_TEST_CASE("Hanging muscle minimum time", "", MocoCasADiSolver) { MocoStudy study; MocoProblem& problem = study.updProblem(); problem.setModelCopy(model); - problem.setTimeBounds(0, {0.05, 1.0}); + problem.setTimeBounds(0, 0.1); problem.setStateInfo( - "/joint/height/value", {0.14, 0.16}, initHeight, finalHeight); - problem.setStateInfo("/joint/height/speed", {-1, 1}, 0, 0); - problem.setControlInfo("/forceset/actuator", {0.01, 1}); - - // Initial state constraints/costs. - if (!ignoreActivationDynamics) { - auto* initial_activation = - problem.addGoal(); - initial_activation->setName("initial_activation"); - } - if (!ignoreTendonCompliance) { - if (isTendonDynamicsExplicit) { - auto* initial_equilibrium = - problem.addGoal(); - initial_equilibrium->setName("initial_force_equilibrium"); - // problem.setStateInfo("/forceset/actuator/normalized_tendon_force", - // {0, 2}); - } else { - // The problem performs better when this is in cost mode. - auto* initial_equilibrium = problem.addGoal< - MocoInitialVelocityEquilibriumDGFGoal>(); - initial_equilibrium->setName("initial_velocity_equilibrium"); - initial_equilibrium->setMode("cost"); - initial_equilibrium->setWeight(0.001); - } - } + "/joint/height/value", heightBounds, initHeight, finalHeight); + problem.setStateInfo( + "/joint/height/speed", speedBounds, 0, 0); + problem.setControlInfo("/forceset/actuator", actuBounds); + + auto* initial_equilibrium = + problem.addGoal(); + initial_equilibrium->setName("initial_equilibrium"); - problem.addGoal(); + problem.addGoal("final_time"); auto& solver = study.initSolver(); - solver.set_num_mesh_intervals(25); - solver.set_multibody_dynamics_mode("implicit"); + solver.set_num_mesh_intervals(30); + solver.set_multibody_dynamics_mode("explicit"); solver.set_optim_convergence_tolerance(1e-4); solver.set_optim_constraint_tolerance(1e-4); + solver.set_minimize_implicit_auxiliary_derivatives(true); solutionTrajOpt = study.solve(); std::string solutionFilename = "testDeGrooteFregly2016Muscle_solution"; if (!ignoreActivationDynamics) solutionFilename += "_actdyn"; if (ignoreTendonCompliance) solutionFilename += "_rigidtendon"; if (isTendonDynamicsExplicit) solutionFilename += "_exptendyn"; + + std::vector outputPaths; + outputPaths.emplace_back(".*tendon_force.*"); + outputPaths.emplace_back(".*fiber_force_along_tendon.*"); + outputPaths.emplace_back(".*length.*"); + outputPaths.emplace_back(".*velocity.*"); + auto table = study.analyze(solutionTrajOpt, outputPaths); + writeTableToFile(table, solutionFilename + "_outputs.sto"); + solutionFilename += ".sto"; solutionTrajOpt.write(solutionFilename); } @@ -1153,7 +1147,7 @@ TEMPLATE_TEST_CASE("Hanging muscle minimum time", "", MocoCasADiSolver) { const double error = trajSim.compareContinuousVariablesRMS( solutionTrajOpt, {{"states", {}}, {"controls", {}}}); - CHECK(error < 0.05); + CHECK(error < 0.01); if (!ignoreTendonCompliance && !isTendonDynamicsExplicit) { mutableDGFMuscle->set_tendon_compliance_dynamics_mode("implicit"); } @@ -1162,7 +1156,7 @@ TEMPLATE_TEST_CASE("Hanging muscle minimum time", "", MocoCasADiSolver) { // Track the kinematics from the trajectory optimization. // ------------------------------------------------------ // We will try to recover muscle activity. - if (!isTendonDynamicsExplicit) { + { std::cout << "Tracking the trajectory optimization coordinate solution." << std::endl; MocoStudy study; @@ -1174,30 +1168,13 @@ TEMPLATE_TEST_CASE("Hanging muscle minimum time", "", MocoCasADiSolver) { solutionTrajOpt.getTime()[solutionTrajOpt.getNumTimes() - 1]; problem.setTimeBounds(0, finalTime); problem.setStateInfo( - "/joint/height/value", {0.14, 0.16}, initHeight, finalHeight); - problem.setStateInfo("/joint/height/speed", {-1, 1}, 0, 0); - problem.setControlInfo("/forceset/actuator", {0.01, 1}); - - // Initial state constraints/costs. - if (!ignoreActivationDynamics) { - auto* initial_activation = - problem.addGoal(); - initial_activation->setName("initial_activation"); - } - if (!ignoreTendonCompliance) { - if (isTendonDynamicsExplicit) { - auto* initial_equilibrium = - problem.addGoal(); - initial_equilibrium->setName("initial_force_equilibrium"); - } else { - // The problem performs better when this is in cost mode. - auto* initial_equilibrium = problem.addGoal< - MocoInitialVelocityEquilibriumDGFGoal>(); - initial_equilibrium->setName("initial_velocity_equilibrium"); - initial_equilibrium->setMode("cost"); - initial_equilibrium->setWeight(0.001); - } - } + "/joint/height/value", heightBounds, initHeight, finalHeight); + problem.setStateInfo("/joint/height/speed", speedBounds, 0, 0); + problem.setControlInfo("/forceset/actuator", actuBounds); + + auto* initial_equilibrium = + problem.addGoal(); + initial_equilibrium->setName("initial_equilibrium"); auto* tracking = problem.addGoal(); tracking->setName("tracking"); @@ -1217,8 +1194,8 @@ TEMPLATE_TEST_CASE("Hanging muscle minimum time", "", MocoCasADiSolver) { tracking->setAllowUnusedReferences(true); auto& solver = study.initSolver(); - solver.set_num_mesh_intervals(25); - solver.set_multibody_dynamics_mode("implicit"); + solver.set_num_mesh_intervals(30); + solver.set_minimize_implicit_auxiliary_derivatives(true); MocoSolution solutionTrack = study.solve(); std::string solutionFilename = @@ -1230,7 +1207,7 @@ TEMPLATE_TEST_CASE("Hanging muscle minimum time", "", MocoCasADiSolver) { solutionTrack.write(solutionFilename); double error = solutionTrack.compareContinuousVariablesRMS(solutionTrajOpt); - CHECK(error < 0.015); + CHECK(error < 0.01); } } @@ -1275,31 +1252,30 @@ TEST_CASE("Fiber equilibrium") { auto& mutMuscle = model.updComponent("muscle"); mutMuscle.set_ignore_tendon_compliance(false); - const double pennOpt = 0.12; - double cosPenn = cos(pennOpt); - mutMuscle.set_pennation_angle_at_optimal(pennOpt); SimTK::State state = model.initSystem(); const double Vmax = muscle.get_optimal_fiber_length() * muscle.get_max_contraction_velocity(); const double muscleTendonVelocity = -0.21 * Vmax; coord.setSpeedValue(state, muscleTendonVelocity); - const double L = muscle.get_optimal_fiber_length() * cosPenn + - muscle.get_tendon_slack_length(); - - const auto activations = createVectorLinspace(4, 0.01, 1.0); - const auto lengths = createVectorLinspace(10, 1.1 * L, 0.6 * L); + const auto activations = createVectorLinspace(5, 0.01, 1.0); + const auto normFiberLengths = createVectorLinspace(20, 0.4, 1.2); // Around normalized length of 0.66857, the residual does not cross zero. + // - Doesn't seem activation dependent + // - Normalized fiber lengths at this normalized length is small (< 0.2) for (int ia = 0; ia < activations.size(); ++ia) { - for (int ilen = 0; ilen < lengths.size(); ++ilen) { + for (int ilen = 0; ilen < normFiberLengths.size(); ++ilen) { std::cout << "activation: " << activations[ia] - << "; normalized length: " << lengths[ilen] / L + << "; normalized fiber length: " << normFiberLengths[ilen] << std::endl; mutMuscle.set_tendon_compliance_dynamics_mode("explicit"); muscle.setActivation(state, activations[ia]); - coord.setValue(state, lengths[ilen]); + const double length = + muscle.get_optimal_fiber_length() * normFiberLengths[ilen] + + muscle.get_tendon_slack_length(); + coord.setValue(state, length); model.realizeDynamics(state); muscle.computeEquilibrium(state); @@ -1317,14 +1293,22 @@ TEST_CASE("Fiber equilibrium") { << "\n normalized active fiber force along tendon: " << muscle.getActiveFiberForceAlongTendon(state) / muscle.get_max_isometric_force() << "\n normalized passive fiber force along tendon: " - << muscle.getPassiveFiberForceAlongTendon(state) / muscle.get_max_isometric_force() - << std::endl; - CHECK(muscle.getNormalizedTendonForceDerivative(state) == - Approx(0.0).margin(1e-6)); + << muscle.getPassiveFiberForceAlongTendon(state) / muscle.get_max_isometric_force() + << "\n normalized tendon force derivative: " + << muscle.getNormalizedTendonForceDerivative(state); + + //CHECK(muscle.getNormalizedTendonForceDerivative(state) == + // Approx(0.0).margin(1e-6)); mutMuscle.set_tendon_compliance_dynamics_mode("implicit"); model.realizeDynamics(state); - CHECK(muscle.getEquilibriumResidual(state) == Approx(0.0)); + + std::cout << "\n equilibrium residual: " + << muscle.getEquilibriumResidual(state) + << std::endl; + + //CHECK(muscle.getEquilibriumResidual(state) == + // Approx(0.0).margin(1e-6)); } } From 79a8506218fde997130465eb640803ac17675b3d Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Mon, 15 Jun 2020 11:07:38 -0700 Subject: [PATCH 06/11] Fix muscle passive curve integral --- .../DeGrooteFregly2016MuscleDerivatives.m | 29 +++++++++++++------ 1 file changed, 20 insertions(+), 9 deletions(-) diff --git a/Moco/Sandbox/DeGrooteFregly2016MuscleDerivatives.m b/Moco/Sandbox/DeGrooteFregly2016MuscleDerivatives.m index 73675bc0a..8e9d43e89 100644 --- a/Moco/Sandbox/DeGrooteFregly2016MuscleDerivatives.m +++ b/Moco/Sandbox/DeGrooteFregly2016MuscleDerivatives.m @@ -6,6 +6,12 @@ f_t = c1*exp(kT*(lTtilde - c2)) - c3; simplify(diff(f_t, lTtilde)) +fprintf('Integral of tendon force length curve \n') +fprintf('===================================== \n') + +f_t = c1*exp(kT*(lTtilde - c2)) - c3; +simplify(int(f_t, lTtilde)) + fprintf('Derivative of active fiber force length curve \n') fprintf('============================================= \n') @@ -22,18 +28,23 @@ fprintf('Derivative of passive fiber force length curve \n') fprintf('============================================== \n') -f_pas = (exp(kPE * (lMtilde - 1.0) / e0) - exp(kPE * (min_lMtilde - 1) / e0)) / ... - (exp(kPE) - exp(kPE * (min_lMtilde - 1) / e0)); +offset = exp(kPE * (min_lMtilde - 1.0) / e0); +denom = exp(kPE) - offset; +f_pas = (exp(kPE * (lMtilde - 1.0) / e0) - offset) / denom; simplify(diff(f_pas, lMtilde)) -fprintf('Integral of tendon force length curve \n') -fprintf('===================================== \n') - -f_t = c1*exp(kT*(lTtilde - c2)) - c3; -simplify(int(f_t, lTtilde)) - fprintf('Integral of passive fiber force length curve \n') fprintf('============================================ \n') -f_pas_int = simplify(int(f_pas, lMtilde)) +f_pas_int = simplify(int(f_pas, lMtilde)); pretty(f_pas_int) + +temp1 = exp(kPE * lMtilde / e0); +temp2 = exp(kPE * min_lMtilde / e0); +temp3 = exp(kPE * (e0 + 1.0) / e0); +numer = - e0 * temp1 + kPE * lMtilde * temp2; +denom = kPE * (temp2 - temp3); +f_pas_int_decomp = numer / denom; + +fprintf('Expressions equal: ') +isequal(f_pas_int, f_pas_int_decomp) \ No newline at end of file From bfc9100ce511d18d3c6e3349e38d6c95e8bbd093 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Fri, 19 Jun 2020 10:22:18 -0700 Subject: [PATCH 07/11] Debugging initial conditions --- Moco/Tests/testMocoActuators.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/Moco/Tests/testMocoActuators.cpp b/Moco/Tests/testMocoActuators.cpp index 32fd0f832..77213ad25 100644 --- a/Moco/Tests/testMocoActuators.cpp +++ b/Moco/Tests/testMocoActuators.cpp @@ -1090,9 +1090,9 @@ TEMPLATE_TEST_CASE("Hanging muscle minimum time", "", MocoCasADiSolver) { "/joint/height/speed", speedBounds, 0, 0); problem.setControlInfo("/forceset/actuator", actuBounds); - auto* initial_equilibrium = - problem.addGoal(); - initial_equilibrium->setName("initial_equilibrium"); + //auto* initial_equilibrium = + // problem.addGoal(); + //initial_equilibrium->setName("initial_equilibrium"); problem.addGoal("final_time"); From 11987784fcc6bf529e0989fb94b8650763398f22 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Fri, 19 Jun 2020 17:17:58 -0700 Subject: [PATCH 08/11] Compute equilibrium residual to avoid caching issue // clean up test cases --- .../Components/DeGrooteFregly2016Muscle.cpp | 361 +++++++++--------- .../Components/DeGrooteFregly2016Muscle.h | 29 +- .../MocoInitialForceEquilibriumGoal.cpp | 48 +-- .../MocoInitialForceEquilibriumGoal.h | 3 +- Moco/Moco/MocoGoal/MocoOutputGoal.cpp | 1 + Moco/Tests/testMocoActuators.cpp | 104 +++-- 6 files changed, 271 insertions(+), 275 deletions(-) diff --git a/Moco/Moco/Components/DeGrooteFregly2016Muscle.cpp b/Moco/Moco/Components/DeGrooteFregly2016Muscle.cpp index 75883b4e5..29219569f 100644 --- a/Moco/Moco/Components/DeGrooteFregly2016Muscle.cpp +++ b/Moco/Moco/Components/DeGrooteFregly2016Muscle.cpp @@ -579,29 +579,18 @@ void DeGrooteFregly2016Muscle::computeInitialFiberEquilibrium( const auto& muscleTendonVelocity = getLengtheningSpeed(s); const auto& activation = getActivation(s); - // We have to use the implicit form of the model since the explicit form - // will produce a zero residual for any guess of normalized tendon force. // The implicit form requires a value for normalized tendon force // derivative, so we'll set it to zero for simplicity. const SimTK::Real normTendonForceDerivative = 0.0; - MuscleLengthInfo mli; - FiberVelocityInfo fvi; - MuscleDynamicsInfo mdi; - - auto calcResidual = [this, &muscleTendonLength, &muscleTendonVelocity, - &normTendonForceDerivative, &activation, &mli, - &fvi, - &mdi](const SimTK::Real& normTendonForce) { - calcMuscleLengthInfoHelper(muscleTendonLength, false, mli, - normTendonForce); - calcFiberVelocityInfoHelper(muscleTendonVelocity, activation, false, - false, mli, fvi, normTendonForce, normTendonForceDerivative); - calcMuscleDynamicsInfoHelper(activation, muscleTendonVelocity, false, - mli, fvi, mdi, normTendonForce); - - return calcEquilibriumResidual( - mdi.tendonForce, mdi.fiberForceAlongTendon); + // Wrap residual function so it is a function of normalized tendon force + // only. + const auto calcResidual = [this, &muscleTendonLength, &muscleTendonVelocity, + &normTendonForceDerivative, &activation] + (const SimTK::Real& normTendonForce) { + return calcEquilibriumResidual(muscleTendonLength, + muscleTendonVelocity, activation, normTendonForce, + normTendonForceDerivative); }; const auto equilNormTendonForce = solveBisection(calcResidual, @@ -664,173 +653,173 @@ void DeGrooteFregly2016Muscle::computeInitialFiberEquilibrium( } -std::pair -DeGrooteFregly2016Muscle::estimateMuscleFiberState(const double activation, - const double muscleTendonLength, const double muscleTendonVelocity, - const double normTendonForceDerivative, const double tolerance, - const int maxIterations) const { - - MuscleLengthInfo mli; - FiberVelocityInfo fvi; - MuscleDynamicsInfo mdi; - - double normTendonForce = get_default_normalized_tendon_force(); - calcMuscleLengthInfoHelper(muscleTendonLength, - get_ignore_tendon_compliance(), mli, normTendonForce); - - double fiberLength = mli.fiberLength; - double residual = SimTK::MostPositiveReal; - double partialFiberForceAlongTendonPartialFiberLength = 0.0; - double partialTendonForcePartialFiberLength = 0.0; - double partialResidualPartialFiberLength = 0.0; - double deltaFiberLength = 0.0; - - // Helper functions - // ---------------- - // Update position level quantities. - auto positionFunc = [&] { - const auto& fiberLengthAlongTendon = - sqrt(SimTK::square(fiberLength) - m_squareFiberWidth); - const auto& tendonLength = muscleTendonLength - fiberLengthAlongTendon; - const auto& normTendonLength = tendonLength / get_tendon_slack_length(); - normTendonForce = calcTendonForceMultiplier(normTendonLength); - calcMuscleLengthInfoHelper(muscleTendonLength, - get_ignore_tendon_compliance(), mli, normTendonForce); - }; - // Update velocity and dynamics level quantities and compute residual. - auto dynamicsFunc = [&] { - calcFiberVelocityInfoHelper(muscleTendonVelocity, activation, false, - false, mli, fvi, normTendonForce, normTendonForceDerivative); - calcMuscleDynamicsInfoHelper(activation, muscleTendonVelocity, false, - mli, fvi, mdi, normTendonForce); - - partialFiberForceAlongTendonPartialFiberLength = - mdi.userDefinedDynamicsExtras[ - m_mdi_partialFiberForceAlongTendonPartialFiberLength]; - partialTendonForcePartialFiberLength = mdi.userDefinedDynamicsExtras[ - m_mdi_partialTendonForcePartialFiberLength]; - - residual = calcEquilibriumResidual( - mdi.tendonForce, mdi.fiberForceAlongTendon); - }; - - // Initialize the loop. - int iter = 0; - positionFunc(); - dynamicsFunc(); - double residualPrev = residual; - double fiberLengthPrev = fiberLength; - double h = 1.0; - - while ((abs(residual) > tolerance) && (iter < maxIterations)) { - // Compute the search direction. - partialResidualPartialFiberLength = - partialFiberForceAlongTendonPartialFiberLength - - partialTendonForcePartialFiberLength; - h = 1.0; - - while (abs(residual) >= abs(residualPrev)) { - // Compute the Newton step. - deltaFiberLength = - -h * residualPrev / partialResidualPartialFiberLength; - - // Take a Newton step if the step is nonzero. - if (abs(deltaFiberLength) > SimTK::SignificantReal) - fiberLength = fiberLengthPrev + deltaFiberLength; - else { - // We've stagnated or hit a limit; assume we are hitting local - // minimum and attempt to approach from the other direction. - fiberLength = fiberLengthPrev - - SimTK::sign(deltaFiberLength) * SimTK::SqrtEps; - h = 0; - } - - if (fiberLength / get_optimal_fiber_length() < - m_minNormFiberLength) { - fiberLength = m_minNormFiberLength * get_optimal_fiber_length(); - } - if (fiberLength / get_optimal_fiber_length() > - m_maxNormFiberLength) { - fiberLength = m_maxNormFiberLength * get_optimal_fiber_length(); - } - - positionFunc(); - dynamicsFunc(); - - if (h <= SimTK::SqrtEps) { break; } - h = 0.5 * h; - } - - residualPrev = residual; - fiberLengthPrev = fiberLength; - - iter++; - } - - // Populate the result map. - ValuesFromEstimateMuscleFiberState resultValues; - - if (abs(residual) < tolerance) { // The solution converged. - - resultValues.iterations = iter; - resultValues.solution_error = residual; - resultValues.fiber_length = fiberLength; - resultValues.fiber_velocity = fvi.fiberVelocity; - resultValues.normalized_tendon_force = mdi.normTendonForce; - - return std::pair( - Success_Converged, resultValues); - } - - // Fiber length is at or exceeds its lower bound. - if (fiberLength / get_optimal_fiber_length() <= m_minNormFiberLength) { - - fiberLength = m_minNormFiberLength * get_optimal_fiber_length(); - positionFunc(); - normTendonForce = calcTendonForceMultiplier(mli.normTendonLength); - - resultValues.iterations = iter; - resultValues.solution_error = residual; - resultValues.fiber_length = fiberLength; - resultValues.fiber_velocity = 0; - resultValues.normalized_tendon_force = normTendonForce; - - return std::pair( - Warning_FiberAtLowerBound, resultValues); - } - - // Fiber length is at or exceeds its upper bound. - if (fiberLength / get_optimal_fiber_length() >= m_maxNormFiberLength) { - - fiberLength = m_maxNormFiberLength * get_optimal_fiber_length(); - positionFunc(); - normTendonForce = calcTendonForceMultiplier(mli.normTendonLength); - - resultValues.iterations = iter; - resultValues.solution_error = residual; - resultValues.fiber_length = fiberLength; - resultValues.fiber_velocity = 0; - resultValues.normalized_tendon_force = normTendonForce; - - return std::pair( - Warning_FiberAtUpperBound, resultValues); - } - - // Max iterations reached. - resultValues.iterations = iter; - resultValues.solution_error = residual; - resultValues.fiber_length = SimTK::NaN; - resultValues.fiber_velocity = SimTK::NaN; - resultValues.normalized_tendon_force = SimTK::NaN; - - return std::pair( - Failure_MaxIterationsReached, resultValues); -} +//std::pair +//DeGrooteFregly2016Muscle::estimateMuscleFiberState(const double activation, +// const double muscleTendonLength, const double muscleTendonVelocity, +// const double normTendonForceDerivative, const double tolerance, +// const int maxIterations) const { +// +// MuscleLengthInfo mli; +// FiberVelocityInfo fvi; +// MuscleDynamicsInfo mdi; +// +// double normTendonForce = get_default_normalized_tendon_force(); +// calcMuscleLengthInfoHelper(muscleTendonLength, +// get_ignore_tendon_compliance(), mli, normTendonForce); +// +// double fiberLength = mli.fiberLength; +// double residual = SimTK::MostPositiveReal; +// double partialFiberForceAlongTendonPartialFiberLength = 0.0; +// double partialTendonForcePartialFiberLength = 0.0; +// double partialResidualPartialFiberLength = 0.0; +// double deltaFiberLength = 0.0; +// +// // Helper functions +// // ---------------- +// // Update position level quantities. +// auto positionFunc = [&] { +// const auto& fiberLengthAlongTendon = +// sqrt(SimTK::square(fiberLength) - m_squareFiberWidth); +// const auto& tendonLength = muscleTendonLength - fiberLengthAlongTendon; +// const auto& normTendonLength = tendonLength / get_tendon_slack_length(); +// normTendonForce = calcTendonForceMultiplier(normTendonLength); +// calcMuscleLengthInfoHelper(muscleTendonLength, +// get_ignore_tendon_compliance(), mli, normTendonForce); +// }; +// // Update velocity and dynamics level quantities and compute residual. +// auto dynamicsFunc = [&] { +// calcFiberVelocityInfoHelper(muscleTendonVelocity, activation, false, +// false, mli, fvi, normTendonForce, normTendonForceDerivative); +// calcMuscleDynamicsInfoHelper(activation, muscleTendonVelocity, false, +// mli, fvi, mdi, normTendonForce); +// +// partialFiberForceAlongTendonPartialFiberLength = +// mdi.userDefinedDynamicsExtras[ +// m_mdi_partialFiberForceAlongTendonPartialFiberLength]; +// partialTendonForcePartialFiberLength = mdi.userDefinedDynamicsExtras[ +// m_mdi_partialTendonForcePartialFiberLength]; +// +// residual = calcEquilibriumResidual( +// mdi.tendonForce, mdi.fiberForceAlongTendon); +// }; +// +// // Initialize the loop. +// int iter = 0; +// positionFunc(); +// dynamicsFunc(); +// double residualPrev = residual; +// double fiberLengthPrev = fiberLength; +// double h = 1.0; +// +// while ((abs(residual) > tolerance) && (iter < maxIterations)) { +// // Compute the search direction. +// partialResidualPartialFiberLength = +// partialFiberForceAlongTendonPartialFiberLength - +// partialTendonForcePartialFiberLength; +// h = 1.0; +// +// while (abs(residual) >= abs(residualPrev)) { +// // Compute the Newton step. +// deltaFiberLength = +// -h * residualPrev / partialResidualPartialFiberLength; +// +// // Take a Newton step if the step is nonzero. +// if (abs(deltaFiberLength) > SimTK::SignificantReal) +// fiberLength = fiberLengthPrev + deltaFiberLength; +// else { +// // We've stagnated or hit a limit; assume we are hitting local +// // minimum and attempt to approach from the other direction. +// fiberLength = fiberLengthPrev - +// SimTK::sign(deltaFiberLength) * SimTK::SqrtEps; +// h = 0; +// } +// +// if (fiberLength / get_optimal_fiber_length() < +// m_minNormFiberLength) { +// fiberLength = m_minNormFiberLength * get_optimal_fiber_length(); +// } +// if (fiberLength / get_optimal_fiber_length() > +// m_maxNormFiberLength) { +// fiberLength = m_maxNormFiberLength * get_optimal_fiber_length(); +// } +// +// positionFunc(); +// dynamicsFunc(); +// +// if (h <= SimTK::SqrtEps) { break; } +// h = 0.5 * h; +// } +// +// residualPrev = residual; +// fiberLengthPrev = fiberLength; +// +// iter++; +// } +// +// // Populate the result map. +// ValuesFromEstimateMuscleFiberState resultValues; +// +// if (abs(residual) < tolerance) { // The solution converged. +// +// resultValues.iterations = iter; +// resultValues.solution_error = residual; +// resultValues.fiber_length = fiberLength; +// resultValues.fiber_velocity = fvi.fiberVelocity; +// resultValues.normalized_tendon_force = mdi.normTendonForce; +// +// return std::pair( +// Success_Converged, resultValues); +// } +// +// // Fiber length is at or exceeds its lower bound. +// if (fiberLength / get_optimal_fiber_length() <= m_minNormFiberLength) { +// +// fiberLength = m_minNormFiberLength * get_optimal_fiber_length(); +// positionFunc(); +// normTendonForce = calcTendonForceMultiplier(mli.normTendonLength); +// +// resultValues.iterations = iter; +// resultValues.solution_error = residual; +// resultValues.fiber_length = fiberLength; +// resultValues.fiber_velocity = 0; +// resultValues.normalized_tendon_force = normTendonForce; +// +// return std::pair( +// Warning_FiberAtLowerBound, resultValues); +// } +// +// // Fiber length is at or exceeds its upper bound. +// if (fiberLength / get_optimal_fiber_length() >= m_maxNormFiberLength) { +// +// fiberLength = m_maxNormFiberLength * get_optimal_fiber_length(); +// positionFunc(); +// normTendonForce = calcTendonForceMultiplier(mli.normTendonLength); +// +// resultValues.iterations = iter; +// resultValues.solution_error = residual; +// resultValues.fiber_length = fiberLength; +// resultValues.fiber_velocity = 0; +// resultValues.normalized_tendon_force = normTendonForce; +// +// return std::pair( +// Warning_FiberAtUpperBound, resultValues); +// } +// +// // Max iterations reached. +// resultValues.iterations = iter; +// resultValues.solution_error = residual; +// resultValues.fiber_length = SimTK::NaN; +// resultValues.fiber_velocity = SimTK::NaN; +// resultValues.normalized_tendon_force = SimTK::NaN; +// +// return std::pair( +// Failure_MaxIterationsReached, resultValues); +//} double DeGrooteFregly2016Muscle::getPassiveFiberElasticForce( const SimTK::State& s) const { diff --git a/Moco/Moco/Components/DeGrooteFregly2016Muscle.h b/Moco/Moco/Components/DeGrooteFregly2016Muscle.h index 7c3c6898c..a83800a1a 100644 --- a/Moco/Moco/Components/DeGrooteFregly2016Muscle.h +++ b/Moco/Moco/Components/DeGrooteFregly2016Muscle.h @@ -276,9 +276,9 @@ class OSIMMOCO_API DeGrooteFregly2016Muscle : public Muscle { /// The residual (i.e. error) in the muscle-tendon equilibrium equation: /// residual = tendonForce - fiberForce * cosPennationAngle double getEquilibriumResidual(const SimTK::State& s) const { - const auto& mdi = getMuscleDynamicsInfo(s); - return calcEquilibriumResidual( - mdi.tendonForce, mdi.fiberForceAlongTendon); + return calcEquilibriumResidual(getLength(s), getLengtheningSpeed(s), + getActivation(s), getNormalizedTendonForce(s), + getNormalizedTendonForceDerivative(s)); } /// The residual (i.e. error) in the time derivative of the linearized @@ -649,10 +649,25 @@ class OSIMMOCO_API DeGrooteFregly2016Muscle : public Muscle { } /// @copydoc getEquilibriumResidual() - SimTK::Real calcEquilibriumResidual(const SimTK::Real& tendonForce, - const SimTK::Real& fiberForceAlongTendon) const { - - return tendonForce - fiberForceAlongTendon; + SimTK::Real calcEquilibriumResidual(const SimTK::Real& muscleTendonLength, + const SimTK::Real& muscleTendonVelocity, + const SimTK::Real& activation, + const SimTK::Real& normTendonForce, + const SimTK::Real& normTendonForceDerivative) const { + + MuscleLengthInfo mli; + FiberVelocityInfo fvi; + MuscleDynamicsInfo mdi; + + calcMuscleLengthInfoHelper( + muscleTendonLength, false, mli, normTendonForce); + calcFiberVelocityInfoHelper(muscleTendonVelocity, activation, false, + m_isTendonDynamicsExplicit, mli, fvi, normTendonForce, + normTendonForceDerivative); + calcMuscleDynamicsInfoHelper(activation, muscleTendonVelocity, false, + mli, fvi, mdi, normTendonForce); + + return mdi.tendonForce - mdi.fiberForceAlongTendon; } /// @copydoc getLinearizedEquilibriumResidualDerivative() diff --git a/Moco/Moco/MocoGoal/MocoInitialForceEquilibriumGoal.cpp b/Moco/Moco/MocoGoal/MocoInitialForceEquilibriumGoal.cpp index ddff368a9..98af76bf4 100644 --- a/Moco/Moco/MocoGoal/MocoInitialForceEquilibriumGoal.cpp +++ b/Moco/Moco/MocoGoal/MocoInitialForceEquilibriumGoal.cpp @@ -17,45 +17,45 @@ * -------------------------------------------------------------------------- */ #include "MocoInitialForceEquilibriumGoal.h" -#include "../Components/DeGrooteFregly2016Muscle.h" using namespace OpenSim; void MocoInitialForceEquilibriumGoal::initializeOnModelImpl( const Model& model) const { - for (const auto& muscle : model.getComponentList()) { - if (!muscle.get_ignore_tendon_compliance()) { - if (const auto dgfmuscle = - dynamic_cast(&muscle)) { - if (dgfmuscle->get_tendon_compliance_dynamics_mode() - == "explicit") { - m_muscleRefs.emplace_back(&muscle); - } - } else { - m_muscleRefs.emplace_back(&muscle); + for (const auto& dgfmuscle : + model.getComponentList()) { + if (!dgfmuscle.get_ignore_tendon_compliance() && + dgfmuscle.get_tendon_compliance_dynamics_mode() == "explicit") { + m_muscleRefs.emplace_back(dgfmuscle); } } - } setRequirements(0, (int)m_muscleRefs.size()); } + + void MocoInitialForceEquilibriumGoal::calcGoalImpl( const GoalInput& input, SimTK::Vector& goal) const { const auto& s = input.initial_state; - getModel().realizeDynamics(s); - if (getModeIsCost()) { - for (int i = 0; i < (int)m_muscleRefs.size(); ++i) { - const auto residual = m_muscleRefs[i]->getTendonForce(s) - - m_muscleRefs[i]->getFiberForceAlongTendon(s); - goal[i] = residual * residual; - } - } else { - for (int i = 0; i < (int)m_muscleRefs.size(); ++i) { - const auto residual = m_muscleRefs[i]->getTendonForce(s) - - m_muscleRefs[i]->getFiberForceAlongTendon(s); - goal[i] = residual; + getModel().realizeVelocity(s); + + for (int i = 0; i < (int)m_muscleRefs.size(); ++i) { + const auto muscleTendonLength = m_muscleRefs[i]->getLength(s); + const auto muscleTendonVelocity = + m_muscleRefs[i]->getLengtheningSpeed(s); + const auto activation = m_muscleRefs[i]->getActivation(s); + const auto normTendonForce = + m_muscleRefs[i]->getNormalizedTendonForce(s); + // Assuming normalized tendon force derivative is zero. + const auto residual = m_muscleRefs[i]->calcEquilibriumResidual( + muscleTendonLength, muscleTendonVelocity, activation, + normTendonForce, 0); + goal[i] = residual; + if (getModeIsCost()) { + goal[i] *= goal[i]; } } + } diff --git a/Moco/Moco/MocoGoal/MocoInitialForceEquilibriumGoal.h b/Moco/Moco/MocoGoal/MocoInitialForceEquilibriumGoal.h index 0971b6f78..47364cd8a 100644 --- a/Moco/Moco/MocoGoal/MocoInitialForceEquilibriumGoal.h +++ b/Moco/Moco/MocoGoal/MocoInitialForceEquilibriumGoal.h @@ -19,6 +19,7 @@ * -------------------------------------------------------------------------- */ #include "MocoGoal.h" +#include "../Components/DeGrooteFregly2016Muscle.h" namespace OpenSim { @@ -51,7 +52,7 @@ class OSIMMOCO_API MocoInitialForceEquilibriumGoal : public MocoGoal { const GoalInput& input, SimTK::Vector& goal) const override; private: - mutable std::vector> + mutable std::vector> m_muscleRefs; }; diff --git a/Moco/Moco/MocoGoal/MocoOutputGoal.cpp b/Moco/Moco/MocoGoal/MocoOutputGoal.cpp index 5c8a8dc0b..f0ec120a9 100644 --- a/Moco/Moco/MocoGoal/MocoOutputGoal.cpp +++ b/Moco/Moco/MocoGoal/MocoOutputGoal.cpp @@ -45,6 +45,7 @@ void MocoOutputGoal::calcIntegrandImpl( const IntegrandInput& input, double& integrand) const { getModel().getSystem().realize(input.state, m_output->getDependsOnStage()); integrand = m_output->getValue(input.state); + integrand *= integrand; } void MocoOutputGoal::calcGoalImpl( diff --git a/Moco/Tests/testMocoActuators.cpp b/Moco/Tests/testMocoActuators.cpp index 77213ad25..182c5f151 100644 --- a/Moco/Tests/testMocoActuators.cpp +++ b/Moco/Tests/testMocoActuators.cpp @@ -31,6 +31,8 @@ using namespace OpenSim; +/* + // Function to compute fiber force (or fiber force along tendon) versus fiber // length (or fiber length along tendon). This checks fiber stiffness // calculations in DeGrooteFregly2016Muscle. @@ -1054,9 +1056,9 @@ Model createHangingMuscleModel(double optimalFiberLength, } TEMPLATE_TEST_CASE("Hanging muscle minimum time", "", MocoCasADiSolver) { - auto ignoreActivationDynamics = GENERATE(true, false); - auto ignoreTendonCompliance = GENERATE(true, false); - auto isTendonDynamicsExplicit = GENERATE(true, false); + auto ignoreActivationDynamics = GENERATE(false, true); + auto ignoreTendonCompliance = GENERATE(false, true); + auto isTendonDynamicsExplicit = GENERATE(false, true); CAPTURE(ignoreActivationDynamics); CAPTURE(ignoreTendonCompliance); @@ -1064,9 +1066,9 @@ TEMPLATE_TEST_CASE("Hanging muscle minimum time", "", MocoCasADiSolver) { const double optimalFiberLength = 0.1; const double tendonSlackLength = 0.05; - SimTK::Real initHeight = 0.151; - SimTK::Real finalHeight = 0.15; - MocoBounds heightBounds(0.145, 0.155); + SimTK::Real initHeight = 0.165; + SimTK::Real finalHeight = 0.155; + MocoBounds heightBounds(0.14, 0.17); MocoBounds speedBounds(-10, 10); MocoBounds actuBounds(0.01, 1); @@ -1083,16 +1085,16 @@ TEMPLATE_TEST_CASE("Hanging muscle minimum time", "", MocoCasADiSolver) { MocoStudy study; MocoProblem& problem = study.updProblem(); problem.setModelCopy(model); - problem.setTimeBounds(0, 0.1); + problem.setTimeBounds(0, {0.1, 1}); problem.setStateInfo( "/joint/height/value", heightBounds, initHeight, finalHeight); problem.setStateInfo( "/joint/height/speed", speedBounds, 0, 0); problem.setControlInfo("/forceset/actuator", actuBounds); - //auto* initial_equilibrium = - // problem.addGoal(); - //initial_equilibrium->setName("initial_equilibrium"); + auto* initial_equilibrium = + problem.addGoal(); + initial_equilibrium->setName("initial_equilibrium"); problem.addGoal("final_time"); @@ -1103,7 +1105,7 @@ TEMPLATE_TEST_CASE("Hanging muscle minimum time", "", MocoCasADiSolver) { solver.set_optim_constraint_tolerance(1e-4); solver.set_minimize_implicit_auxiliary_derivatives(true); - solutionTrajOpt = study.solve(); + solutionTrajOpt = study.solve().unseal(); std::string solutionFilename = "testDeGrooteFregly2016Muscle_solution"; if (!ignoreActivationDynamics) solutionFilename += "_actdyn"; if (ignoreTendonCompliance) solutionFilename += "_rigidtendon"; @@ -1168,9 +1170,9 @@ TEMPLATE_TEST_CASE("Hanging muscle minimum time", "", MocoCasADiSolver) { problem.setStateInfo("/joint/height/speed", speedBounds, 0, 0); problem.setControlInfo("/forceset/actuator", actuBounds); - auto* initial_equilibrium = - problem.addGoal(); - initial_equilibrium->setName("initial_equilibrium"); + //auto* initial_equilibrium = + // problem.addGoal(); + //initial_equilibrium->setName("initial_equilibrium"); auto* tracking = problem.addGoal(); tracking->setName("tracking"); @@ -1207,9 +1209,9 @@ TEMPLATE_TEST_CASE("Hanging muscle minimum time", "", MocoCasADiSolver) { } } +*/ + TEST_CASE("ActivationCoordinateActuator") { - // TODO create a problem with ACA and ensure the activation bounds are - // set as expected. auto model = ModelFactory::createSlidingPointMass(); auto* actu = new ActivationCoordinateActuator(); actu->setName("aca"); @@ -1227,7 +1229,9 @@ TEST_CASE("ActivationCoordinateActuator") { Approx(0.78)); } -TEST_CASE("Fiber equilibrium") { +TEST_CASE("DeGrooteFregly2016Muscle muscle-tendon equilibrium") { + + // Slider joint model with a single muscle. Model model; model.setName("muscle"); auto* body = new Body("body", 0.5, SimTK::Vec3(0), SimTK::Inertia(0)); @@ -1237,74 +1241,60 @@ TEST_CASE("Fiber equilibrium") { coord.setName("x"); model.addComponent(joint); auto* musclePtr = new DeGrooteFregly2016Muscle(); - musclePtr->set_ignore_tendon_compliance(true); musclePtr->set_fiber_damping(0); musclePtr->setName("muscle"); musclePtr->addNewPathPoint("origin", model.updGround(), SimTK::Vec3(0)); musclePtr->addNewPathPoint("insertion", *body, SimTK::Vec3(0)); model.addComponent(musclePtr); + // Get const and mutable muscle references. auto& muscle = model.getComponent("muscle"); auto& mutMuscle = model.updComponent("muscle"); mutMuscle.set_ignore_tendon_compliance(false); + // Set coordinate velocity so the muscle is shortening. SimTK::State state = model.initSystem(); const double Vmax = muscle.get_optimal_fiber_length() * muscle.get_max_contraction_velocity(); const double muscleTendonVelocity = -0.21 * Vmax; coord.setSpeedValue(state, muscleTendonVelocity); + // Test equilibrium while sweeping across activation and normalized + // fiber length values. const auto activations = createVectorLinspace(5, 0.01, 1.0); - const auto normFiberLengths = createVectorLinspace(20, 0.4, 1.2); - // Around normalized length of 0.66857, the residual does not cross zero. - // - Doesn't seem activation dependent - // - Normalized fiber lengths at this normalized length is small (< 0.2) + const auto normFiberLengths = createVectorLinspace(10, 0.4, 1.2); for (int ia = 0; ia < activations.size(); ++ia) { for (int ilen = 0; ilen < normFiberLengths.size(); ++ilen) { - std::cout << "activation: " << activations[ia] - << "; normalized fiber length: " << normFiberLengths[ilen] - << std::endl; - - mutMuscle.set_tendon_compliance_dynamics_mode("explicit"); + CAPTURE(activations[ia]); + CAPTURE(normFiberLengths[ilen]); + // Set activation. muscle.setActivation(state, activations[ia]); - const double length = - muscle.get_optimal_fiber_length() * normFiberLengths[ilen] + - muscle.get_tendon_slack_length(); + + // Set coordinate value based on current normalized fiber length + // and the tendon slack length. + const double length = + muscle.get_optimal_fiber_length() * normFiberLengths[ilen] + + muscle.get_tendon_slack_length(); coord.setValue(state, length); + // Explicit muscle-tendon dynamics. + mutMuscle.set_tendon_compliance_dynamics_mode("explicit"); model.realizeDynamics(state); muscle.computeEquilibrium(state); + CHECK(muscle.getEquilibriumResidual(state) == + Approx(0.0).margin(1e-4)); + CHECK(muscle.getNormalizedTendonForceDerivative(state) == + Approx(0.0).margin(1e-4)); - model.realizeDynamics(state); - std::cout << " fiber length: " << muscle.getFiberLength(state) - << "\n normalized fiber length: " - << muscle.getNormalizedFiberLength(state) - << "\n tendon length: " - << muscle.getTendonLength(state) - << "\n normalized tendon length: " - << muscle.getTendonLength(state) / muscle.get_tendon_slack_length() - << "\n normalized tendon force: " - << muscle.getNormalizedTendonForce(state) - << "\n normalized active fiber force along tendon: " - << muscle.getActiveFiberForceAlongTendon(state) / muscle.get_max_isometric_force() - << "\n normalized passive fiber force along tendon: " - << muscle.getPassiveFiberForceAlongTendon(state) / muscle.get_max_isometric_force() - << "\n normalized tendon force derivative: " - << muscle.getNormalizedTendonForceDerivative(state); - - //CHECK(muscle.getNormalizedTendonForceDerivative(state) == - // Approx(0.0).margin(1e-6)); - + // Implicit muscle-tendon dynamics. mutMuscle.set_tendon_compliance_dynamics_mode("implicit"); model.realizeDynamics(state); - - std::cout << "\n equilibrium residual: " - << muscle.getEquilibriumResidual(state) - << std::endl; - - //CHECK(muscle.getEquilibriumResidual(state) == - // Approx(0.0).margin(1e-6)); + muscle.computeEquilibrium(state); + CHECK(muscle.getEquilibriumResidual(state) == + Approx(0.0).margin(1e-4)); + CHECK(muscle.getNormalizedTendonForceDerivative(state) == + Approx(0.0).margin(1e-4)); } } From 212bae350593532a64bbfdb7ddca69f65e0c7910 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Fri, 19 Jun 2020 17:31:23 -0700 Subject: [PATCH 09/11] Correct linearized derivative calc function --- .../Components/DeGrooteFregly2016Muscle.cpp | 6 +-- .../Components/DeGrooteFregly2016Muscle.h | 42 ++++++++++++------- Moco/Moco/MocoGoal/MocoOutputGoal.cpp | 1 - Moco/Tests/testMocoActuators.cpp | 1 + 4 files changed, 30 insertions(+), 20 deletions(-) diff --git a/Moco/Moco/Components/DeGrooteFregly2016Muscle.cpp b/Moco/Moco/Components/DeGrooteFregly2016Muscle.cpp index 29219569f..999af838d 100644 --- a/Moco/Moco/Components/DeGrooteFregly2016Muscle.cpp +++ b/Moco/Moco/Components/DeGrooteFregly2016Muscle.cpp @@ -553,8 +553,7 @@ void DeGrooteFregly2016Muscle::calcMusclePotentialEnergyInfo( get_ignore_tendon_compliance(), mli, mpei); } -double -OpenSim::DeGrooteFregly2016Muscle::calcInextensibleTendonActiveFiberForce( +double DeGrooteFregly2016Muscle::calcInextensibleTendonActiveFiberForce( SimTK::State& s, double activation) const { MuscleLengthInfo mli; FiberVelocityInfo fvi; @@ -579,6 +578,8 @@ void DeGrooteFregly2016Muscle::computeInitialFiberEquilibrium( const auto& muscleTendonVelocity = getLengtheningSpeed(s); const auto& activation = getActivation(s); + // We have to use the implicit form of the model since the explicit form + // will produce a zero residual for any guess of normalized tendon force. // The implicit form requires a value for normalized tendon force // derivative, so we'll set it to zero for simplicity. const SimTK::Real normTendonForceDerivative = 0.0; @@ -652,7 +653,6 @@ void DeGrooteFregly2016Muscle::computeInitialFiberEquilibrium( } - //std::pair //DeGrooteFregly2016Muscle::estimateMuscleFiberState(const double activation, diff --git a/Moco/Moco/Components/DeGrooteFregly2016Muscle.h b/Moco/Moco/Components/DeGrooteFregly2016Muscle.h index a83800a1a..aa39c5f6c 100644 --- a/Moco/Moco/Components/DeGrooteFregly2016Muscle.h +++ b/Moco/Moco/Components/DeGrooteFregly2016Muscle.h @@ -275,6 +275,9 @@ class OSIMMOCO_API DeGrooteFregly2016Muscle : public Muscle { /// The residual (i.e. error) in the muscle-tendon equilibrium equation: /// residual = tendonForce - fiberForce * cosPennationAngle + /// This is always computed using implicit form of the model since the + /// explicit form will produce a zero residual for any guess of normalized + /// tendon force. double getEquilibriumResidual(const SimTK::State& s) const { return calcEquilibriumResidual(getLength(s), getLengtheningSpeed(s), getActivation(s), getNormalizedTendonForce(s), @@ -288,13 +291,10 @@ class OSIMMOCO_API DeGrooteFregly2016Muscle : public Muscle { /// (muscleTendonVelocity - fiberVelocityAlongTendon) double getLinearizedEquilibriumResidualDerivative( const SimTK::State& s) const { - const auto& muscleTendonVelocity = getLengtheningSpeed(s); - const FiberVelocityInfo& fvi = getFiberVelocityInfo(s); - const MuscleDynamicsInfo& mdi = getMuscleDynamicsInfo(s); - - return calcLinearizedEquilibriumResidualDerivative(muscleTendonVelocity, - fvi.fiberVelocityAlongTendon, mdi.tendonStiffness, - mdi.fiberStiffnessAlongTendon); + return calcLinearizedEquilibriumResidualDerivative(getLength(s), + getLengtheningSpeed(s), getActivation(s), + getNormalizedTendonForce(s), + getNormalizedTendonForceDerivative(s)); } static std::string getActivationStateName() { @@ -658,11 +658,10 @@ class OSIMMOCO_API DeGrooteFregly2016Muscle : public Muscle { MuscleLengthInfo mli; FiberVelocityInfo fvi; MuscleDynamicsInfo mdi; - calcMuscleLengthInfoHelper( muscleTendonLength, false, mli, normTendonForce); calcFiberVelocityInfoHelper(muscleTendonVelocity, activation, false, - m_isTendonDynamicsExplicit, mli, fvi, normTendonForce, + false, mli, fvi, normTendonForce, normTendonForceDerivative); calcMuscleDynamicsInfoHelper(activation, muscleTendonVelocity, false, mli, fvi, mdi, normTendonForce); @@ -672,14 +671,25 @@ class OSIMMOCO_API DeGrooteFregly2016Muscle : public Muscle { /// @copydoc getLinearizedEquilibriumResidualDerivative() SimTK::Real calcLinearizedEquilibriumResidualDerivative( - const SimTK::Real muscleTendonVelocity, - const SimTK::Real& fiberVelocityAlongTendon, - const SimTK::Real& tendonStiffness, - const SimTK::Real& fiberStiffnessAlongTendon) const { + const SimTK::Real& muscleTendonLength, + const SimTK::Real& muscleTendonVelocity, + const SimTK::Real& activation, const SimTK::Real& normTendonForce, + const SimTK::Real& normTendonForceDerivative) const { + + MuscleLengthInfo mli; + FiberVelocityInfo fvi; + MuscleDynamicsInfo mdi; + calcMuscleLengthInfoHelper( + muscleTendonLength, false, mli, normTendonForce); + calcFiberVelocityInfoHelper(muscleTendonVelocity, activation, false, + m_isTendonDynamicsExplicit, mli, fvi, normTendonForce, + normTendonForceDerivative); + calcMuscleDynamicsInfoHelper(activation, muscleTendonVelocity, false, + mli, fvi, mdi, normTendonForce); - return fiberStiffnessAlongTendon * fiberVelocityAlongTendon - - tendonStiffness * - (muscleTendonVelocity - fiberVelocityAlongTendon); + return mdi.fiberStiffnessAlongTendon * fvi.fiberVelocityAlongTendon - + mdi.tendonStiffness * + (muscleTendonVelocity - fvi.fiberVelocityAlongTendon); } /// @} diff --git a/Moco/Moco/MocoGoal/MocoOutputGoal.cpp b/Moco/Moco/MocoGoal/MocoOutputGoal.cpp index f0ec120a9..5c8a8dc0b 100644 --- a/Moco/Moco/MocoGoal/MocoOutputGoal.cpp +++ b/Moco/Moco/MocoGoal/MocoOutputGoal.cpp @@ -45,7 +45,6 @@ void MocoOutputGoal::calcIntegrandImpl( const IntegrandInput& input, double& integrand) const { getModel().getSystem().realize(input.state, m_output->getDependsOnStage()); integrand = m_output->getValue(input.state); - integrand *= integrand; } void MocoOutputGoal::calcGoalImpl( diff --git a/Moco/Tests/testMocoActuators.cpp b/Moco/Tests/testMocoActuators.cpp index 182c5f151..db73dbf86 100644 --- a/Moco/Tests/testMocoActuators.cpp +++ b/Moco/Tests/testMocoActuators.cpp @@ -1282,6 +1282,7 @@ TEST_CASE("DeGrooteFregly2016Muscle muscle-tendon equilibrium") { mutMuscle.set_tendon_compliance_dynamics_mode("explicit"); model.realizeDynamics(state); muscle.computeEquilibrium(state); + model.realizeDynamics(state); CHECK(muscle.getEquilibriumResidual(state) == Approx(0.0).margin(1e-4)); CHECK(muscle.getNormalizedTendonForceDerivative(state) == From 18dbdd7e2d0f92a537363113389269608b38a467 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Sat, 20 Jun 2020 14:35:25 -0700 Subject: [PATCH 10/11] Remove comments --- Moco/Tests/testMocoActuators.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/Moco/Tests/testMocoActuators.cpp b/Moco/Tests/testMocoActuators.cpp index db73dbf86..734af9fca 100644 --- a/Moco/Tests/testMocoActuators.cpp +++ b/Moco/Tests/testMocoActuators.cpp @@ -31,8 +31,6 @@ using namespace OpenSim; -/* - // Function to compute fiber force (or fiber force along tendon) versus fiber // length (or fiber length along tendon). This checks fiber stiffness // calculations in DeGrooteFregly2016Muscle. @@ -1209,7 +1207,6 @@ TEMPLATE_TEST_CASE("Hanging muscle minimum time", "", MocoCasADiSolver) { } } -*/ TEST_CASE("ActivationCoordinateActuator") { auto model = ModelFactory::createSlidingPointMass(); From 05aaba236c60bcc7fda360a5f14b84406a604a8c Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Sun, 30 Aug 2020 21:01:46 -0700 Subject: [PATCH 11/11] Debugging muscle tests --- Moco/Tests/testMocoActuators.cpp | 165 +++++++++++++++---------------- 1 file changed, 82 insertions(+), 83 deletions(-) diff --git a/Moco/Tests/testMocoActuators.cpp b/Moco/Tests/testMocoActuators.cpp index 734af9fca..e11124d8a 100644 --- a/Moco/Tests/testMocoActuators.cpp +++ b/Moco/Tests/testMocoActuators.cpp @@ -1017,7 +1017,7 @@ TEST_CASE("DeGrooteFregly2016Muscle basics") { } Model createHangingMuscleModel(double optimalFiberLength, - double tendonSlackLength,bool ignoreActivationDynamics, + double tendonSlackLength, bool ignoreActivationDynamics, bool ignoreTendonCompliance, bool isTendonDynamicsExplicit) { Model model; model.setName("isometric_muscle"); @@ -1054,9 +1054,9 @@ Model createHangingMuscleModel(double optimalFiberLength, } TEMPLATE_TEST_CASE("Hanging muscle minimum time", "", MocoCasADiSolver) { - auto ignoreActivationDynamics = GENERATE(false, true); - auto ignoreTendonCompliance = GENERATE(false, true); - auto isTendonDynamicsExplicit = GENERATE(false, true); + auto ignoreActivationDynamics = GENERATE(false); + auto ignoreTendonCompliance = GENERATE(false); + auto isTendonDynamicsExplicit = GENERATE(true); CAPTURE(ignoreActivationDynamics); CAPTURE(ignoreTendonCompliance); @@ -1129,9 +1129,9 @@ TEMPLATE_TEST_CASE("Hanging muscle minimum time", "", MocoCasADiSolver) { // tendon compliance dynamics mode to perform time stepping. auto* mutableDGFMuscle = dynamic_cast( &model.updComponent("forceset/actuator")); - if (!ignoreTendonCompliance && !isTendonDynamicsExplicit) { - mutableDGFMuscle->set_tendon_compliance_dynamics_mode("explicit"); - } + //if (!ignoreTendonCompliance && !isTendonDynamicsExplicit) { + // mutableDGFMuscle->set_tendon_compliance_dynamics_mode("explicit"); + //} const auto trajSim = simulateTrajectoryWithTimeStepping(solutionTrajOpt, model); std::string trajFilename = @@ -1144,9 +1144,9 @@ TEMPLATE_TEST_CASE("Hanging muscle minimum time", "", MocoCasADiSolver) { const double error = trajSim.compareContinuousVariablesRMS( solutionTrajOpt, {{"states", {}}, {"controls", {}}}); CHECK(error < 0.01); - if (!ignoreTendonCompliance && !isTendonDynamicsExplicit) { - mutableDGFMuscle->set_tendon_compliance_dynamics_mode("implicit"); - } + //if (!ignoreTendonCompliance && !isTendonDynamicsExplicit) { + // mutableDGFMuscle->set_tendon_compliance_dynamics_mode("implicit"); + //} } // Track the kinematics from the trajectory optimization. @@ -1172,8 +1172,7 @@ TEMPLATE_TEST_CASE("Hanging muscle minimum time", "", MocoCasADiSolver) { // problem.addGoal(); //initial_equilibrium->setName("initial_equilibrium"); - auto* tracking = problem.addGoal(); - tracking->setName("tracking"); + auto* tracking = problem.addGoal("tracking"); auto states = solutionTrajOpt.exportToStatesStorage().exportToTable(); TimeSeriesTable ref(states.getIndependentColumn()); @@ -1226,74 +1225,74 @@ TEST_CASE("ActivationCoordinateActuator") { Approx(0.78)); } -TEST_CASE("DeGrooteFregly2016Muscle muscle-tendon equilibrium") { - - // Slider joint model with a single muscle. - Model model; - model.setName("muscle"); - auto* body = new Body("body", 0.5, SimTK::Vec3(0), SimTK::Inertia(0)); - model.addComponent(body); - auto* joint = new SliderJoint("joint", model.getGround(), *body); - auto& coord = joint->updCoordinate(SliderJoint::Coord::TranslationX); - coord.setName("x"); - model.addComponent(joint); - auto* musclePtr = new DeGrooteFregly2016Muscle(); - musclePtr->set_fiber_damping(0); - musclePtr->setName("muscle"); - musclePtr->addNewPathPoint("origin", model.updGround(), SimTK::Vec3(0)); - musclePtr->addNewPathPoint("insertion", *body, SimTK::Vec3(0)); - model.addComponent(musclePtr); - - // Get const and mutable muscle references. - auto& muscle = model.getComponent("muscle"); - auto& mutMuscle = model.updComponent("muscle"); - mutMuscle.set_ignore_tendon_compliance(false); - - // Set coordinate velocity so the muscle is shortening. - SimTK::State state = model.initSystem(); - const double Vmax = muscle.get_optimal_fiber_length() * - muscle.get_max_contraction_velocity(); - const double muscleTendonVelocity = -0.21 * Vmax; - coord.setSpeedValue(state, muscleTendonVelocity); - - // Test equilibrium while sweeping across activation and normalized - // fiber length values. - const auto activations = createVectorLinspace(5, 0.01, 1.0); - const auto normFiberLengths = createVectorLinspace(10, 0.4, 1.2); - for (int ia = 0; ia < activations.size(); ++ia) { - for (int ilen = 0; ilen < normFiberLengths.size(); ++ilen) { - CAPTURE(activations[ia]); - CAPTURE(normFiberLengths[ilen]); - - // Set activation. - muscle.setActivation(state, activations[ia]); - - // Set coordinate value based on current normalized fiber length - // and the tendon slack length. - const double length = - muscle.get_optimal_fiber_length() * normFiberLengths[ilen] + - muscle.get_tendon_slack_length(); - coord.setValue(state, length); - - // Explicit muscle-tendon dynamics. - mutMuscle.set_tendon_compliance_dynamics_mode("explicit"); - model.realizeDynamics(state); - muscle.computeEquilibrium(state); - model.realizeDynamics(state); - CHECK(muscle.getEquilibriumResidual(state) == - Approx(0.0).margin(1e-4)); - CHECK(muscle.getNormalizedTendonForceDerivative(state) == - Approx(0.0).margin(1e-4)); - - // Implicit muscle-tendon dynamics. - mutMuscle.set_tendon_compliance_dynamics_mode("implicit"); - model.realizeDynamics(state); - muscle.computeEquilibrium(state); - CHECK(muscle.getEquilibriumResidual(state) == - Approx(0.0).margin(1e-4)); - CHECK(muscle.getNormalizedTendonForceDerivative(state) == - Approx(0.0).margin(1e-4)); - } - } - -} +//TEST_CASE("DeGrooteFregly2016Muscle muscle-tendon equilibrium") { +// +// // Slider joint model with a single muscle. +// Model model; +// model.setName("muscle"); +// auto* body = new Body("body", 0.5, SimTK::Vec3(0), SimTK::Inertia(0)); +// model.addComponent(body); +// auto* joint = new SliderJoint("joint", model.getGround(), *body); +// auto& coord = joint->updCoordinate(SliderJoint::Coord::TranslationX); +// coord.setName("x"); +// model.addComponent(joint); +// auto* musclePtr = new DeGrooteFregly2016Muscle(); +// musclePtr->set_fiber_damping(0); +// musclePtr->setName("muscle"); +// musclePtr->addNewPathPoint("origin", model.updGround(), SimTK::Vec3(0)); +// musclePtr->addNewPathPoint("insertion", *body, SimTK::Vec3(0)); +// model.addComponent(musclePtr); +// +// // Get const and mutable muscle references. +// auto& muscle = model.getComponent("muscle"); +// auto& mutMuscle = model.updComponent("muscle"); +// mutMuscle.set_ignore_tendon_compliance(false); +// +// // Set coordinate velocity so the muscle is shortening. +// SimTK::State state = model.initSystem(); +// const double Vmax = muscle.get_optimal_fiber_length() * +// muscle.get_max_contraction_velocity(); +// const double muscleTendonVelocity = -0.21 * Vmax; +// coord.setSpeedValue(state, muscleTendonVelocity); +// +// // Test equilibrium while sweeping across activation and normalized +// // fiber length values. +// const auto activations = createVectorLinspace(5, 0.01, 1.0); +// const auto normFiberLengths = createVectorLinspace(10, 0.4, 1.2); +// for (int ia = 0; ia < activations.size(); ++ia) { +// for (int ilen = 0; ilen < normFiberLengths.size(); ++ilen) { +// CAPTURE(activations[ia]); +// CAPTURE(normFiberLengths[ilen]); +// +// // Set activation. +// muscle.setActivation(state, activations[ia]); +// +// // Set coordinate value based on current normalized fiber length +// // and the tendon slack length. +// const double length = +// muscle.get_optimal_fiber_length() * normFiberLengths[ilen] + +// muscle.get_tendon_slack_length(); +// coord.setValue(state, length); +// +// // Explicit muscle-tendon dynamics. +// mutMuscle.set_tendon_compliance_dynamics_mode("explicit"); +// model.realizeDynamics(state); +// muscle.computeEquilibrium(state); +// model.realizeDynamics(state); +// CHECK(muscle.getEquilibriumResidual(state) == +// Approx(0.0).margin(1e-4)); +// CHECK(muscle.getNormalizedTendonForceDerivative(state) == +// Approx(0.0).margin(1e-4)); +// +// // Implicit muscle-tendon dynamics. +// mutMuscle.set_tendon_compliance_dynamics_mode("implicit"); +// model.realizeDynamics(state); +// muscle.computeEquilibrium(state); +// CHECK(muscle.getEquilibriumResidual(state) == +// Approx(0.0).margin(1e-4)); +// CHECK(muscle.getNormalizedTendonForceDerivative(state) == +// Approx(0.0).margin(1e-4)); +// } +// } +// +//}